How to solve an elastic spring system with C#

How to write a simple FEA solver in C#. There is only one type of element, a linear spring. In the example below there are 5 nodes and 4 elements. They form a tree structure one branch connects nodes 1 through 4, and the second branch connects nodes 3 and 5. There is spring pre-loading between elements 2 and 3, and an external force applied on node 5. Also the stiffness varies between elements. Nodes 1 and 4 are immobile (fixed to the ground). The constraints and loadings have been highlighted in bold below.

The program result is:

NODE LIST
#         POS      DEFL     FORCE       FIX
1       0.000   0.00000     1.304     FIXED
2       1.000  -0.13043     0.000      FREE
3       2.000  -0.04348     0.000      FREE
4       3.000   0.00000     8.696     FIXED
5       3.000  -0.14348   -10.000      FREE

ELEMENT LIST
#   NODES STIFFNESS   PRELOAD     FORCE
1     1→2    10.000     0.000     1.304
2     2→3   100.000    10.000     1.304
3     3→4   200.000     0.000    -8.696
4     3→5   100.000     0.000    10.000

The full code listing is below:
/// <summary>
/// Nodes can either have prescribed force 
/// or displacement.
/// </summary>
public enum Apply
{
    Force,
    Displacement
}

public class Node
{
    /// <summary>
    /// Define position x, 
    /// deformation u, 
    /// and applied force W
    /// </summary>
    public double x, ux, W;
    /// <summary>
    /// Define if displacement or force 
    /// is applied to node
    /// </summary>
    public Apply fix;

    /// <summary>
    /// Defines a free node at a position.
    /// </summary>
    /// <param name="x"></param>
    public Node(double x) : this(x, Apply.Force, 0) { }
    /// <summary>
    /// Defines a node at a position, 
    /// with prescribed force or displacement.
    /// </summary>
    /// <param name="x">The node position</param>
    /// <param name="fix">constraint type</param>
    /// <param name="value">constraint value</param>
    public Node(double x, Apply fix, double value)
    {
        this.x=x;
        this.fix=fix;
        if(fix==Apply.Force)
        {
            this.W=value;
            this.ux=0;
        }
        else
        {
            this.ux=value;
            this.W=0;
        }
    }
}

public class Element
{
    /// <summary>
    /// Connects node i with node j
    /// </summary>
    public int i, j;
    /// <summary>
    /// Stiffness k, preload F0
    /// and element force f
    /// </summary>
    public double k, F0, f;

    /// <summary>
    /// Define new element between two nodes
    /// </summary>
    /// <param name="i">From node index</param>
    /// <param name="j">To node index</param>
    /// <param name="k">Element stiffness</param>
    public Element(int i, int j, double k)
        : this(i, j, k, 0) { }
    /// <summary>
    /// Define new element between two nodes
    /// </summary>
    /// <param name="i">From node index</param>
    /// <param name="j">To node index</param>
    /// <param name="k">Element stiffness</param>
    /// <param name="F0">Element pre-load</param>
    public Element(int i, int j, double k, double F0)
    {
        this.i=i;
        this.j=j;
        this.k=k;
        this.F0=F0;
        this.f=0;
    }
}

/// <summary>
/// Create a linear system of springs
/// </summary>
public class WorldSystem
{
    public List<Node> nodes;
    public List<Element> elements;
    Matrix P, A, R1, R2;
    SymmetricBandMatrix K;

    public WorldSystem()
    {
        this.nodes=new List<Node>();
        this.elements=new List<Element>();
    }

    #region Structure
    /// <summary>
    /// Add a free node
    /// </summary>
    /// <param name="x">The node position</param>
    public void AddFreeNode(double x)
    {
        nodes.Add(new Node(x));
    }
    /// <summary>
    /// Add a free node with force
    /// </summary>
    /// <param name="x">The node position</param>
    /// <param name="W">The applied force</param>
    public void AddFreeNode(double x, double W)
    {
        nodes.Add(new Node(x, Apply.Force, W));
    }
    /// <summary>
    /// Add a fixed node
    /// </summary>
    /// <param name="x">The node position</param>
    public void AddFixedNode(double x)
    {
        nodes.Add(new Node(x, Apply.Displacement, 0));
    }
    /// <summary>
    /// Add a fixed node with displacement
    /// </summary>
    /// <param name="x">The node position</param>
    /// <param name="ux">The applied displacement</param>
    public void AddFixedNode(double x, double ux)
    {
        nodes.Add(new Node(x, Apply.Displacement, ux));
    }
    /// <summary>
    /// Add a spring between two nodes
    /// </summary>
    /// <param name="i">From node index</param>
    /// <param name="j">To node index</param>
    /// <param name="k">Spring stiffness</param>
    /// <param name="F0">Spring preload</param>
    public void AddElement(int i, int j,
        double k, double F0)
    {
        elements.Add(new Element(i, j, k, F0));
    }
    /// <summary>
    /// Add a spring between two nodes
    /// </summary>
    /// <param name="i">From node index</param>
    /// <param name="j">To node index</param>
    /// <param name="k">Spring stiffness</param>
    public void AddElement(int i, int j, double k)
    {
        elements.Add(new Element(i, j, k));
    }
    /// <summary>
    /// Add a spring between two nodes
    /// </summary>
    /// <param name="A">From node</param>
    /// <param name="B">To node</param>
    /// <param name="k">Spring stiffness</param>
    public void AddElement(Node A, Node B, double k)
    {
        int i=nodes.IndexOf(A);
        int j=nodes.IndexOf(B);
        AddElement(i, j, k);
    }
    /// <summary>
    /// Add a spring between two nodes
    /// </summary>
    /// <param name="A">From node</param>
    /// <param name="B">To node</param>
    /// <param name="k">Spring stiffness</param>
    /// <param name="F0">Spring preload</param>
    public void AddElement(Node A, Node B,
        double k, double F0)
    {
        int i=nodes.IndexOf(A);
        int j=nodes.IndexOf(B);
        AddElement(i, j, k, F0);
    }
    #endregion

    /// <summary>
    /// Create an example system with 
    /// 5 nodes and 4 elements
    /// </summary>
    public static WorldSystem CreateTestCase()
    {
        //                  v==3==(D)
        // (A)==1==(B)==2==(C)
        //                  ^==4==(E)
        //
        // Fixed nodes (A), (D)
        // Force applied on node (E)

        WorldSystem sys=new WorldSystem();

        // Fixed Node (A)
        sys.AddFixedNode(0.0);
        // Free Node (B)
        sys.AddFreeNode(1.0);
        // Free Node (C)
        sys.AddFreeNode(2.0);
        // Fixed (D)
        sys.AddFixedNode(3.0);
        // Free Node (E) with force applied
        sys.AddFreeNode(3.0, -10.0);

        // Element (A)===(B)
        sys.AddElement(0, 1, 10);
        // Element (B)===(C)
        sys.AddElement(1, 2, 100, 10);
        // Element (C)===(D)
        sys.AddElement(2, 3, 200);
        // Element (C)===(E)
        sys.AddElement(2, 4, 100);

        return sys;
    }


    #region Solution
    public void SolveSystem(ref Vector b, ref Vector x)
    {
        // Split vectors x & b into two components
        //
        // x = R1*x1 + R2*x2
        // where x1=known(H), x2=unknown(N+M-H)
        //
        // b = R1*b1 + R2*b2
        // where b1=unknown(H), b2=known(N+M-H)

        // Filter known nodal displacements
        var x1=R1.Transpose()*x;
        // Filter for known element preloads
        // and nodal forces
        var b2=R2.Transpose()*b;

        var lse=new LinearEquations();

        // reduce the system by substituting for known
        // displacements for forces
        var A22=R2.Transpose()*A*R2;
        var b22=R2.Transpose()*(R2*b2-A*R1*x1);
        // solve for unknown element forces
        // and nodal displacements
        var x2=lse.Solve(A22, b22);
        x=(R1*x1+R2*x2).GetColumnVector(0);
        // back substitute for unknown nodal forces
        var b1=R1.Transpose()*A*x;
        b=(R1*b1+R2*b2).GetColumnVector(0);
    }

    public void ResetSystem()
    {
        Vector b, x;
        BuildSolution(out b, out x);
        // Filter out unknown quantities
        x=(R1*R1.Transpose()*x).GetColumnVector(0);
        b=(R2*R2.Transpose()*b).GetColumnVector(0);
        ReportSolution(b, x);
    }

    void BuildSystem()
    {
        int N=nodes.Count;
        int M=elements.Count;

        // Connection Matrix P:{#ELEM, #NODES}
        this.P=new Matrix(M, N);
        // Stiffness Matrix K:diag{#ELEM}
        this.K=new SymmetricBandMatrix(M, 0);

        this.A=new Matrix(M+N, M+N);
        for(int k=0; k<M; k++)
        {
            P[k, elements[k].i]=1;
            P[k, elements[k].j]=-1;
            K[k, k]=elements[k].k;
        }
        var PT=P.Transpose();
        var KP=K*P;
        // Element Forces:      f = F - K*P*u
        // Nodal Forces:        W - PT*f = 0

        // As a system of equations A*x=b
        //
        // | [1] -[K]*[P] | | [f] |   | [F] |
        // |              | |     | = |     |
        // | [P]T    0    | | [u] |   | [W] |
        //
        // At this stage some of [u] may be known 
        // and some of [W] may be unknown
        for(int k=0; k<M; k++)
        {
            A[k, k]=1;
        }
        int H=0; // Count fixed nodes
        for(int i=0; i<N; i++)
        {
            if(nodes[i].fix==Apply.Displacement) H++;
            for(int k=0; k<M; k++)
            {
                A[k, M+i]=-KP[k, i];
                A[M+i, k]=PT[i, k];
            }
        }
        // Build the known and unknown projection matrices

        // Example with 3 nodes & 2 elements. 
        // 2 nodes have prescribed displacements
        //
        // | f1 |   | 0 0 |          | 1 0 0 |        
        // | f2 |   | 0 0 | | u1 |   | 0 1 0 | | f1 | 
        // | u1 | = | 1 0 | |    | + | 0 0 0 | | f2 | 
        // | u2 |   | 0 0 | | u3 |   | 0 0 1 | | u2 | 
        // | u3 |   | 0 1 |          | 0 0 0 |        
        //
        //  x = R1*x1 + R2*x2

        this.R1=new Matrix(N+M, H);
        this.R2=new Matrix(N+M, N+M-H);
        for(int k=0; k<M; k++)
        {
            // all of internal forces are unknown
            R2[k, k]=1;
        }
        for(int i=0, j=0, l=0; i<N; i++)
        {
            // set projection matrix coefficients
            if(nodes[i].fix==Apply.Displacement)
            {
                R1[M+i, j++]=1;
            }
            else
            {
                R2[M+i, M+l++]=1;
            }
        }
    }
    void BuildSolution(out Vector b, out Vector x)
    {
        int N=nodes.Count;
        int M=elements.Count;

        b=new Vector(M+N);
        x=new Vector(M+N);
        for(int k=0; k<M; k++)
        {
            // get element forces into solution 
            // Vectors b, x
            b[k]=elements[k].F0;
            x[k]=elements[k].f;
        }
        for(int i=0; i<N; i++)
        {
            // get nodal forces and displacements
            // into Vectors b, x
            b[M+i]=nodes[i].W;
            x[M+i]=nodes[i].ux;
        }
    }
    void ReportSolution(Vector b, Vector x)
    {
        int N=nodes.Count;
        int M=elements.Count;

        for(int k=0; k<M; k++)
        {
            // set element forces from solution
            // Vectors b, x
            elements[k].f=x[k];
            elements[k].F0=b[k];
        }
        for(int i=0; i<N; i++)
        {
            // set nodal forces and displacements
            // from Vectors b, x
            nodes[i].ux=x[M+i];
            nodes[i].W=b[M+i];
        }
    }
    #endregion

    #region Report
    public override string ToString()
    {
        StringBuilder sb=new StringBuilder();
        sb.AppendLine("NODE LIST");
        sb.AppendFormat(
            "{0,-3} {1,9} {2,9} {3,9} {4,9}",
            "#",
            "POS",
            "DEFL",
            "FORCE",
            "FIX");
        sb.AppendLine();
        for(int i=0; i<nodes.Count; i++)
        {
            sb.AppendFormat(
                "{0,-3} {1,9:F3} {2,9:F5} {3,9:F3} {4,9}",
                i+1,
                nodes[i].x,
                nodes[i].ux,
                nodes[i].W,
                nodes[i].fix==Apply.Displacement?"FIXED":"FREE");
            sb.AppendLine();
        }

        sb.AppendLine("ELEMENT LIST");
        sb.AppendFormat(
            "{0,-3} {1,5} {2,9} {3,9} {4,9}",
            "#",
            "NODES",
            "STIFFNESS",
            "PRELOAD",
            "FORCE");
        sb.AppendLine();
        for(int k=0; k<elements.Count; k++)
        {
            var nodes=(elements[k].i+1).ToString()+"→"+(elements[k].j+1).ToString();
            sb.AppendFormat(
                "{0,-3} {1,5} {2,9:F3} {3,9:F3} {4,9:F3}",
                k+1,
                nodes,
                elements[k].k,
                elements[k].F0,
                elements[k].f);
            sb.AppendLine();

        }
        return sb.ToString();
    }

    #endregion

    static void Main(string[] args)
    {
        var lss=CreateTestCase();
        Vector b, x;
        // Build system strucutre
        lss.BuildSystem();
        // Move knowns from system to b, x
        lss.BuildSolution(out b, out x);
        // Solve for the unknowns
        lss.SolveSystem(ref b, ref x);
        // Move unknowns into system
        lss.ReportSolution(b, x);

        // Report results
        Console.WriteLine(lss.ToString());
        Console.WriteLine("Press ENTER to quit.");
        Console.ReadLine();
    }
}

Technorati Tags: Programming,FEM,C#,Engineering

Methods in Rigid Body Mechanics–Part 1

OVERVIEW

In this series of posts I will demonstrate different techniques for tackling problems with one or more rigid bodies in motion. Consider the inverted pendulum problem on a cart as described in this wikipedia page.

An approximation that can be done at this stage is that each rigid body acts like a point mass, and only the forces on each center of gravity are considered. Later we can see how to use all the degrees of freedom (including rotations) to see the effects of rotational inertias and what the reactions might be at the wheels above.

Method 1 - Cartesian Coordinates

KINEMATICS

In this example there are two bodies in motion (the cart and the ball) and most simplistic approach is to consider each rigid body moving separately. The center of gravity of the cart (Point A) has two degrees of freedom $x_A$ and $y_A$ when considered free of all contacts and constraints. Similarly the center of gravity of the ball (Point B) has also two degrees of freedom $x_B$ and $y_B$. In vector form the above coordinates are $$\vec{r}_A = \begin{pmatrix} x_A \\ y_A \\ 0 \end{pmatrix}$$ $$\vec{r}_B = \begin{pmatrix} x_B \\ y_B \\ 0 \end{pmatrix}$$

The kinematics (motion description) of such a system is trivial as the velocities and accelerations are just the derivatives of the above quantities. In vector form these are $$ \vec{v}_A = \begin{pmatrix} \dot{x}_A \\ \dot{y}_A \\ 0 \end{pmatrix}$$ $$ \vec{v}_B = \begin{pmatrix} \dot{x}_B \\ \dot{y}_B \\ 0 \end{pmatrix}$$ and $$ \vec{a}_A = \begin{pmatrix} \ddot{x}_A \\ \ddot{y}_A \\ 0 \end{pmatrix}$$ $$ \vec{a}_B = \begin{pmatrix} \ddot{x}_B \\ \ddot{y}_B \\ 0 \end{pmatrix}$$

There are a total of 4 unknown degrees of freedom in the system corresponding to the 4 acceleration components of $\vec{a}_A$ and $\vec{a}_B$.

In addition, there are two constraint equations applicable here, one keeping the cart on the ground and the other keeping the ball a distance $l$ from the cart. This makes the system with two effective degrees of freedom. To include the constraint equations in the system we are going to use what is called the Pfaffian form in terms of the velocity components.

Since the cart does not move in the vertical direction this can be written as $ \hat{j}\cdot\vec{v}_A = 0 $ or in expanded form $$ \begin{pmatrix}0\\1\\0\end{pmatrix}\cdot\begin{pmatrix} \dot{x}_A \\ \dot{y}_A \\ 0 \end{pmatrix} = \dot{y}_A = 0$$ Also the ball does not move towards the cart along the rod with direction vector $\hat{n}=\begin{pmatrix}\text{-}\sin\theta & \cos\theta & 0 \end{pmatrix}^\top$ so the constraint is $\hat{n}\cdot\vec{v}_B = 0$ or in expanded form $$\begin{pmatrix}\text{-}\sin\theta \\ \cos\theta \\ 0 \end{pmatrix} \cdot \begin{pmatrix} \dot{x}_B \\ \dot{y}_B \\ 0 \end{pmatrix} = \dot{y}_B \cos\theta – \dot{x}_B \sin\theta = 0 $$

This is kind of a tricky part, because $\theta$ depends on our degrees of freedom with $$\tan\theta = \frac{x_A-x_B}{y_B-y_A}$$ In addition, to solve for the motion we need to differentiate all these expressions in order to bring them in terms of accelerations.

To summarize, this technique quickly balloons into unmanageable complexity and thus it is not used at all in practice. The closest I have seen is the computer game particle systems because all interactions between bodies are modeled with compliant forces and not Pfaffian constraints.  Next time I will simplify this technique by only considering the reduced degrees of freedom $x$ and $\theta$ and derive everything else from those.