Integration: Mechanics + Hydraulics + Navigation

Petr Ivankov

5.00/5 (45 votes)

Jun 16, 2010

CPOL

21 min read

64944

6101

Sample of integration of branches of engineering.

1. Introduction

Every real science and engineering system is a result integration of ingredients which belong to different branches of science. For example, the GPS Bulldozer System requires integration of mechanics, hydraulics, and navigation. The above picture represents the following ingredients of the problem:

Bulldozer (main object)
Navigation satellite (right top object)
Inertial navigation system (left bottom object)
Kalman filter (transparent formula at center)

A lot of difficulties are caused by that different problems have different software. This article shows that a universal software makes the integration process easier. The main sample of this article is a GPS Bulldozer System.

2. Background

Here the relevant theoretical issues are considered.

2.1 Outlook

GPS Bulldozer Systems could have different configurations. The following references contain descriptions or some configurations:

Although these systems are different, they have very similar ingredients or building blocks:

GPS antennas
Gyro and accelerometers
Principles of math processing

It is clear that if we have a framework which can configure these blocks, then the design of such a system would become easier. The main problem is automatic control of the bulldozer blade. The classical automatic control scheme is represented in the following picture:

However, this problem requires a more complicated scheme which is presented below:

The interesting part of this scheme is the usage of indirect measurements (see also http://wiki.answers.com/Q/What_is_indirect_measuring). The control system uses GPS measurements of position and velocity of the bulldozer in an orthogonal reference frame. Also, measurements of Gyro and accelerometers are used. But these measurements do not provide direct information about the real and required positions of the bulldozer blade. It is a complicated dependence between system measurements and control parameters. So GPS and other measurements should be processed. The typical algorithm for this processing is the Kalman Filter. The following section is devoted to it.

2.2 Kalman filter

2.2.1 Theory

The Kalman filter is a mathematical method named after Rudolf E. Kalman. Its purpose is to use measurements that are observed over time that contain noise (random variations) and other inaccuracies, and produce values that tend to be closer to the true values of the measurements and their associated calculated values. Kalman filter theory does not depend on physical phenomenon. So Kalman filter can be used for the estimation of state of different types of objects (mechanical, electrical, pneumatic, etc). Moreover, the Kalman filter theory does not depend on the types of measurements. Any type of measurement is acceptable (inertial, electrical, temperature, optical, nuclear). So Kalman Filter is an engineering object which is invariant to the physical background. We have a universal engineering object and this idea is very close to the idea of a universal engineering software. How can the Kalman Filter theory exist without any physical background? It is based on abstract equations. Some of these equations are presented below:

where x_{k- 1}, x_k are state vectors at k - 1 and k^th steps, respectively. z_k is a vector of measurements. The vector functions f and h are respectively called transition function and function of measurements. All the above notions are abstract. These notions are not directly related to any special engineering problem. In a GPS Bulldozer System, these parameters have the following meaning. The control operations are being performed by a digital computer. So these operations are separated by steps. Vectors x_{k- 1}, x_k correspond to k - 1 and k^th steps. If we consider a GPS Bulldozer System, then the components of vectors x_{k- 1}, x_k include the following parameters of motion of the GPS Bulldozer System:

Three coordinates of the bulldozer;
The components of the bulldozer 3D velocity;
Quaternion of the bulldozer orientation;
Three components of the angular velocity of the bulldozer;
Position of the blade lifting cylinder;
Position of the blade tilting cylinder;
Speed of movement of lifting the hydraulic cylinder;
Speed of movement of tilting the hydraulic cylinder.

Detailed information about these parameters is contained here. Vector z_k depends on the sensor which is used in the system. The z_k vector contains the GPS measurements. The number of components of z_k depends on the number of GPS antennas. The system can contain accelerometers and gyro. In this case, these measurements are also contained in z_k. Let us explain the meaning of f and h. First of all, note that the behavior of the system is described by ordinary differential equations. These equations are known. The standard procedure of integration of ordinary differential equations provides the function f. Similarly, well known formulas of geometry and kinematics provide the expression of h.

2.2.2 Discussion

The previous section contains a lot of notions. The engineer should understand them. However, the engineer should not be overloaded by these notions in everyday work. A lot of operations related to the problems should be automated. For example, obtaining f from differential equations is a standard procedure and it should be automated. The derivation of equations also should be automated. In the best situation, the engineer sets the configuration of the bulldozer and (virtually) installs sensors on it. After this action, the math model should be automatically generated. Then the engineer attaches the Kalman Filter to the generated model.

2.2.3 Implementation

2.2.3.1 Ingredients

The Kalman Filter theory deals with abstract vector functions f and h. The IObjectTransformer interface is used for these function implementations:

///<summary> Transformer of objects</summary>
public interface IObjectTransformer 
{
     
    ///</summary> Input variables </summary>
    string[] Input { get; }
     
    /// <summary>Output variables </summary>
    string[] Output { get; } 
    
    /// <summary>Gets type of i th input variabe</summary>
    /// <param name="i" />Variable index </param>
    /// <returns>The type</returns> 
    object GetInputType(int i); 
    
    /// <summary>Gets type of i th output variable </summary>
    /// <param name="i" />Variable index </param>
    /// <returns>The type</returns> 
    object GetOutputType(int i); 
    
    /// <summary>Calculation </summary>
    /// <param name="input" /> Input </param>
    /// <param name="output" />Output </param>
    void Calculate(object[] input, object[] output); 
}

This interface transforms a set of inputs to a set of outputs. It is not used for Kalman Filter only and in general, a number of inputs can exceed 1 as well as outputs. However, this implementation of the Kalman Filter accepts a single input and a single output. An acceptable situation is presented in the following table:

N	Vector function	Number of inputs	Number of outputs	Input type	Output type
1	f	1	1	`double[n]`	`double[n]`
2	h	1	1	`double[n]`	`double[l]`

Where n and l are dimensions of x_k and z_k, respectively. The implementation of Kalman Filter is contained in the source code (class KalmanFilter) and readers with a strong interest in the problem could study it. Here is a brief explanation of a simple application of it. In 2.2.1, I noted that the f function could be obtained by the solution of ordinary differential equations. The following situation represents an implementation of this idea by the framework:

Here the Diff equations object represents the following ordinary differential equations system. It has the following properties:

It means that this object represents the following system of differential equations:

where x, y, z are variables and a, b, c, d, f, g, h, i, j are constants. The State object has the icon. This icon is a combination of the following icons: and . The first one is a collection icon, and the second one is a transformer icon. So the State object is a collection and transformer at the same time. There are two programming ways to be a collection and transformer at the same time. The first way is multiple inheritance which has been considered in my previous article. The second way is the usage of two objects, one of them a collection and the second one a transformer. Using this way, we can use several type of transformers for one collection. This fact could reduce the amount of code. The State object is an object of the DataPerformerCollectionStateTransformer type:

///<summary> Collection and transformer</summary>
public class DataPerformerCollectionStateTransformer : ObjectsCollection, 
             IChildrenObject, IPostSetArrow, IRemovableObject

This class is a subclass of ObjectsCollection. This fact means that DataPerformerCollectionStateTransformer is a collection of objects. Also, DataPerformerCollectionStateTransformer implements the IChildrenObject interface. One of the children is a transformer. The following code snippet shows this fact:

/// <summary>
/// Child transformer
/// </summary>
AbstractDoubleTransformer transformer;

/// <summary>
/// Sets child transformer
/// </summary>
void SetChildren()
{
    if (transformer != null)
    {
        if (transformer.GetType().Equals(transformerType))
        {
            return;
        }
        IDisposable d = transformer;
        d.Dispose();
    }
    if (transformerType.Equals(typeof(StateTransformer)))
    {
        transformer = new StateTransformer(this);
    }
    else
    {
        transformer = new StateVariableTransformer(this);
    }
    childen[0] = transformer;
}

#region IChildrenObject Members

IAssociatedObject[] IChildrenObject.Children
{
    get { return childen; }
}

#endregion

The type of child is AbstractDoubleTransformer. This type implements the IObjectTransformer interface. So it is a transformer. The class AbstractDoubleTransformer is abstract. One of its subtypes is really used. The usage of multiple inheritance does not provide this facility. There is a natural question: how can a child be found. The following snippet represents the function for this purpose:

/// <summary>
/// Gets object of predefined type
/// </summary>
/// <typeparam name="T">The type</typeparam>
/// <param name="obj">The prototype</param>
/// <returns>The object of predefined type</returns>
public static T GetObject<T>(IAssociatedObject obj) where T : class
{
    // If o is subtype of t
    if (obj is T)
    {
        // Returns o as T
        return obj as T;
    }
    // Search in childen
    // If o is IChildrenObject
    if (obj is IChildrenObject)
    {
        IChildrenObject co = obj as IChildrenObject;
        // Gets children
        IAssociatedObject[] ch = co.Children;
        if (ch != null)
        {
            // Recursive search among children
            foreach (IAssociatedObject ob in ch)
            {
                T a = GetObject<T>(ob);
                // If chid object is found
                if (a != null)
                {
                    // Returns the object
                    return a;
                }
            }
        }
    }
    return null;
}

The following code can be used for finding an object which implements IObjectTransformer:

// Gets transformer from "obj" object
IObjectTransformer transformer = this.GetObject<IObjectTransformer>(obj));

So the State object is a collection and transformer at the same time. As a collection, it could be the start of a "belongs arrow" . This arrow connects the collection with its elements. It means that Diff equations is a child of the State object. But State is also a transformer (it implements the IObjectTransformer interface). Any such object could be an end of arrow which connects the transformer and the consumer of the transformer. This situation is presented below:

The links State as IObjectTransformer with the Consumer object as IObjectTransformerConsumer. The properties of the State object are presented below:

Let us explain these properties. First of all, the bottom combo box shows the type of the transformer field type. Remember that the transformer is an object of the abstract class AbstractDoubleTransformer. The real transformer object could be an object of a subclass of AbstractDoubleTransformer. There are two subclasses of AbstractDoubleTransformer:

StateTransformer;
StateVariableTransformer..

The "State - State" text in the combo box means that the type of the transformer field is StateTransformer. An object of StateTransformer implements IObjectTransformer by the following way:

/// <summary>
/// Calculation
/// </summary>
/// <param name="input">Input</param>
/// <param name="output">Output</param>
public override void Calculate(object[] input, object[] output)
{
   ITimeMeasureProvider old = processor.TimeProvider;
    try
    {
        using (new TimeProviderBackup(collection, provider, 
                   DifferentialEquationProcessor.Processor))
        {
            using (new ComponentCollectionBackup(collection))
            {
                processor.TimeProvider = provider;

                // Input
                double[] inp = input[0] as double[];

                // Sets state vector
                collection.SetStateVector(inp);
                double start = provider.Time;
                double step = provider.Step;
                runtime.StartAll(start);
                runtime.UpdateAll();

                // Solution of differential equations
                processor.Step(start, start + step);
                provider.Time = start + step;
                collection.GetStateVector(outbuffer);

                // Sets final vector
                output[0] = outbuffer;
            }
        }
    }
    catch (Exception ex)
    {
        ex.Log();
    }
    processor.TimeProvider = old;
}

This code snippet has the following informal explanation. The object of the StateTransformer class collects all systems of ordinary differential equations of a collection of objects. In result, we have a common system of differential equations. The Calculate function performs integration of differential equations and transforms the initial state vector of the equations to the final one as described in 2.2.1. The final time is separated from the initial time by a Step. According to the above properties, the value of Step is equal to 0.1. The following situation presents a case where the type of transformer is StateVariableTransformer:

Here, the properties of the Diff equations object are the same as above. We also have the Data object with the following properties:

These properties have the following transparent sense. The output of the Data object is calculated by the following expressions:

Formula_1 = ax + by + cz;

Formula_2 = dx + fy + gz.

where x, y, z are state variables of the Diff equations object and a, b, c, d, f, g are constants. The Vector object has the following properties:

It means that the output of the Vector object is a 2D vector. The components of the vector are Formula_1 and Formula_2 of the Data object. The properties of the Mea object are presented below:

These properties have the following meaning. The "State - Output" text means that the type of transformer is StateVariableTransformer. The Measurements field of this form is "Vector". It means that the output of the Mea object is the output of the Vector. Roughly speaking, the Mea object is a transformation from the state vector of Diff equations to Vector.

2.2.3.2 Construction of Kalman Filter

Now we have the ingredients which enable us to construct the Kalman Filter. Let us construct it. First of all, let us provide the math formulation of the problem. Suppose that a dynamical system with measurements is described by the following equations:

x is the state vector of system, f is the right part function of the differential equation system, z is the measurements vector, and h is the function which links the measurements with the state vector. The system (1) is abstract since we do not know explicit expressions for f and h. Suppose that f and h are defined by the following expressions:

Then the following scenario contains the Kalman Filter for such a model:

The following are the objects which were already considered in 2.2.3.2:

Diff equations
Data
Vector
State
Mea

The following mapping links the variables of equations (3) and the objects of this picture:

N	Variable of equations (3)	Object	Variable of object
1	x₁	Diff equations	x
2	x₁	Diff equations	y
3	x₁	Diff equations	z
4	a₁₁	Diff equations	a
5	a₁₂	Diff equations	b
6	a₁₃	Diff equations	c
7	a₂₁	Diff equations	d
8	a₂₂	Diff equations	f
9	a₂₃	Diff equations	g
10	a₃₁	Diff equations	h
11	a₃₂	Diff equations	i
12	a₁₁	Diff equations	a
13	z₁	Data	Formula_1
14	z₂	Data	Formula_2
15	b₁₁	Data	a
16	b₁₂	Data	b
17	b₁₃	Data	c
18	b₂₁	Data	d
19	b₂₂	Data	f
20	b₂₃	Data	g

The objects Measurements, State Covariation, and Measurements Covariation are used for the simulation of measurements, covarition of the system noise, and covariation of the measurements noise. An explanation of these parameters can be found here. The Filter object represents the Kalman Filter and has the following properties:

The State transformation property corresponds to the vector function f in equation (1). The combo box of this property shows objects which implement the IObjectTransformer interface. The value of this property is State. So the State object as IObjectTransformer corresponds to f. Similarly, the Mea object as IObjectTransformer corresponds to Measurement transformation or h in (1). Equations (1) are presented below once again:

Other combo boxes contain the names of objects which implement the IMeasurements interface. Their meanings are explained above. Besides these parameters, the Kalman Filter uses matrixes of partial derivations:

There are two main paths of calculation of these matrixes. The first one based on math manipulations. The second one is the usage of finite difference. The first method is more accurate but requires a lot of mental work. The second one is very simple but is less accurate. However, the author of this article does not yet know a problem which strongly requires the first method. So this version of Kalman Filter uses finite differences and does not require special math expressions for these matrixes. It requires values of finite differences which are represented in the above properties as State difference and Measurement difference, respectively.

Download this sample - 3.01 KB

2.3 Kinematics

The above example is a new form of representation of math expressions only. So it is not useful without integration with other branches of science and engineering. Other branches of engineering also use math. But link to math could be implicit or declarative. Declarative programming has already proved its usefulness. Let us compare declarative and imperative approaches in kinematics. The following picture contains an image of a typical kinematics problem.

We have three reference frames. We know the law of absolute 6D motion of frame 1. Also, we know the law of relative 6D motion of frame 2 with respect to frame 1 and relative motion of frame 3 with respect to frame 2 as well. Absolute motion of frame 3 can be very complicated, even former 3 motion laws are simple. Suppose that we also have an additional Base frame and we would like to define the motion law of frame 3 with respect to Base. The reflection of this situation is presented below:

The objects 1, 2, and 3 represent moved frames. The L 1-2 arrow means that we consider relative motion of frame 2 with respect to 1. The L 3-2 arrow has a similar meaning. However, the 1 frame is not the beginning of any geometrical link . This fact means that an absolute motion of the frame is considered. Frame 1 has the following properties:

Here, X, Y, Z are orthogonal coordinates of the frame, and Q0, Q1, Q2, Q3 are components of quaternion which define the orientation of the frame. The above properties contain a mapping between the 6D motion parameters of the frame and the parameters of the Trans 1 object. Properties of the Trans 1 object are presented below:

The parameter t in the above formulas is time, other parameters are constants. The above formulae are not too complicated. The frames 2 and 3 are similarly linked to Trans 2 and Trans 3. However, coordinates and orientations of these frames are relative. Although motion law of frame 1 and relative motion laws of frames 2 and 3 are very simple, absolute motion of frame 3 is very complicated. However, the usage of the framework provides an easy definition of these law parameters. A combination of 6D motions can be obtained by a connection by the geometrical link only. Suppose that we would like to define parameters of motion of frame 3 with respect to frame Base. We have the following construction for this purpose:

The Relative object has the RelativeMeasurements type. This object is connected by M3 and MB arrows to frames 3 and Base, respectively. All these facts mean that the Relative object provides parameters of relative motion of frame 3 with respect to Base. The number of parameters depends on differentiability of the relative 6D motion parameters. If these parameters are not differentiable then the parameters include relative coordinates and orientation. If parameters are once differentiable by time, then the parameters also include relative linear and angular velocities. A sufficient condition of differentiability of frame 3 is differentiability of parameters of objects Trans 1, Trans 2, and Trans 3. The following picture presents some properties of Trans 1:

This picture means that output parameters of the Trans 1 object are once differentiable. In particular, the Diff object performs time differentiation of parameters of Trans 1 in the following way:

Since parameters of Trans 1, Trans 2, and Trans3 are more than once differentiable, the Relative object provides the following parameters:

If parameters of Trans 1 would not set at least once differentiable, then Relative would provide relative coordinates and relative distance only:

The following picture represents the indication of relative motion parameters.

So the kinematics branch of the framework makes easy determination of relative motion parameters in very complicated situations. Moreover, it automatically determines differentiability. This feature will be very essential for the discussion of mechanics issues. Some mechanical trajectories should be twice differentiable.

Download this sample - 3.76 KB

2.4 Usage of Kinematics for 3D Graphics

A lot of software products have been designed strictly for specific problems. These products could not be supported and/or extended. For example, a lot of CAD systems have been designed without perspective of integration with CAE (see http://www.nafems.org/downloads/AUTOSIM_Meetings/ Barcelona_Spain_January_2006/autosim_barcelona06_schweiger.pdf). The framework is a very farsighted project. It is clear that kinematics engineering software should be used for 3D animation. Otherwise, kinematics could exist without animation. So there are a lot of compatible versions of the framework. I would like show the low coupling of the framework. My articles are also devoted to the theme of foresighted projects. The animation was developed in 2002. OpenGL has been used for 3D Graphics (see UniversalEnggFrmwork7.aspx). However, I knew in 2002 that more advanced technologies of 3D graphics will appear. So 3D graphics has been developed with an abstract layer. And this layer is compatible with kinematics. This abstract layer is implemented as the following abstract class:

/// <summary>
/// Factory of 3D objects and cameras
/// </summary>
public abstract class PositionObjectFactory
{
    /// <summary>
    /// Global factory
    /// </summary>
    static private PositionObjectFactory factory;

    /// <summary>
    /// Global factory
    /// </summary>
    public static PositionObjectFactory Factory
    {
        get
        {
            return factory;
        }
        set
        {
            factory = value;
        }
    }

    /// <summary>
    /// Crreates  3D object
    /// </summary>
    /// <param name="type">Object's type</param>
    /// <returns>The object</returns>
    public abstract object CreateObject(string type);

    /// <summary>
    /// Creates new camera
    /// </summary>
    /// <returns></returns>
    public abstract Camera NewCamera();

    /// <summary>
    /// Creates editor of properties of camera
    /// </summary>
    /// <param name="camera">The camera</param>
    /// <returns>The property editor</returns>
    public abstract object CreateForm(Camera camera);

    /// <summary>
    /// Creates editor of properties of visible object
    /// </summary>
    /// <param name="position">Position of object</param>
    /// <param name="visible">Visible object</param>
    /// <returns>Editor of properties of visible object</returns>
    public abstract object CreateForm(IPosition position, IVisible visible);

    /// <summary>
    /// Type of camera
    /// </summary>
    public abstract Type CameraType
    {
        get;
    }

    /// <summary>
    /// Creates label on desktop
    /// </summary>
    /// <param name="obj">Object for label</param>
    /// <returns>The label</returns>
    public abstract IObjectLabel CreateLabel(object obj);

    /// <summary>
    /// Creates label of visible object
    /// </summary>
    /// <param name="position">Position of object</param>
    /// <param name="visible">Visible object</param>
    /// <returns>Editor of properties of visible object</returns>
    public abstract object CreateLabel(IPosition position, IVisible visible);
}

This factory creates 3D objects, cameras, and elements of the UI. The abstract factory doed not know either OpenGL or WPF. It does not know even System.Windows.Forms. So the object CreateForm method should not return System.Windows.Forms.Form only. It returns a Form in a wide sense (for example, System.Windows.Window). There are two subclasses of PositionObjectFactory:

OpenGLFactory;
WpfFactory.

Let us consider a sample of a 3D animation. In this sample, we have two airplanes. The following picture contains projections of the airplanes on two orthogonal planes:

Flight altitudes of the airplanes are different and so we do not have a collision. Since the kinematic module operates with relative positions, we can install a virtual camera on board the planes. The following picture represents the views of these cameras:

The view of the camera installed on the first plane contains the image of the second plane, and vice versa. The following scheme represents the objects and links of this sample:

Let us explain the meaning of this picture. Base 1 and Base 2 are base frames of motion of the first and second airplanes, respectively. The following picture reflects the orientations of the cameras:

This orientation reflects the fact that airplanes have almost opposite courses. The Parameters object contains parameters of linear uniform motion. This object has the following properties:

The Motion 1 object is moving reference frame. Its motion parameters with respect to Base 1 are represented below:

The essential part of this properties is Formula_3 of Parameters. This formula is a linear dependence of time. Formula_1 is equal to zero and Formula_2 is equal to unity. The Bomber object is a 3D model of the bomber. It is rigidly linked to the Motion 1 reference frame. So the motion of Bomber is the motion of the Motion 1 reference frame. The C 1 reference frame is rigidly linked to Motion 1 and Camera 1 is linked to C 1. So parameters of C 1 are parameters of the relative position and orientation of Camera 1 with respect to Motion 1 or Bomber. Otherwise, Camera 1 is linked to Transport by visibility link . So the viewport of Camera 1 contains the image of Transport. We have also Motion 2, C 2, and Camera 2. The purpose of these objects is similar to Motion 1, C 1, and Camera 1.

To use this sample, set the following animation parameters:

and press the green arrow button.

2.3 Lagrange Mechanics

2.3.1 Outlook

Bulldozer is an aggregate which is assembled from parts (see picture below):

The parts of the aggregate cannot be moved independently. The motion of parts are constrained. Lagrange mechanics provides a good model for constrained mechanical objects. The following sections are devoted to the theory and software implementation of Lagrange mechanics.

2.3.2 Theory

Lagrange mechanics is described by the Lagrange function which depends on generalized coordinates q_i and their time derivations. The Lagrange function is the difference between the kinetic energy and the potential one. Lagrange equations are presented below:

Let us consider an example of a 2D pendulum. This example which is presented below:

The parameters are generalized coordinates. Kinetic and potential energy (T and U) of the pendulum are presented below:

The pendulum is the building block of different objects of Lagrange mechanics. The complicated objects of Lagrange mechanics can be constructed from simple ones. Let us consider a constrained system which contains two pendulums.

This system is constrained. Both coordinates x₁, y₁ of the first pendulum are equal to zero. All constrains are presented by the following equations:

Generalized coordinates of two unconstrained pendulums are . Generalized coordinates of the constrained system of two pendulums are . Lagrangian of the system can be obtained by the following way. Langrangian as a function of pendulums are is a sum of Langranians of the pendulums. Lagrangian as a function of can be obtained by the substitution of by their dependences on . So Lagrangian of the aggregate can be obtained from the Lagrangians of the parts and constrains. Let us describe the common procedure of mechanical objects with constrains. Every Lagrangian can be represented the following way:

where A(q) is a matrix of quadratic form. This quadratic form is the kinetic energy. In the case of the pendulum, matrix A(q) is presented below:

What is constrains? The number of degrees of freedom of a constrained system is less than the number of degrees of freedom of an unconstrained one. Suppose q₁, ..., q_n are generalized coordinates of an unconstrained system. These coordinates depend on the generalized p₁, ..., p_m (m < n). This dependence is noted p(q). Lagrangian of the constrained system can be expressed the following way:

2.3.4 Implementation

We would like to develop software which enables us to obtain Lagrangials of a mechanical systems. We need interfaces for the Lagrange mechanical system and the constrains for this purpose. The following interface represents all objects of Lagrange mechanics:

/// <summary>
/// Object of Lagrange Mechanics
/// </summary>
public interface ILagrangeMechanicalObject
{
    /// <summary>
    /// Dimension
    /// </summary>
    int Dimension
    {
        get;
    }

    /// <summary>
    /// Generalized coordinates
    /// </summary>
    double[] Coordinates
    {
        get;
        set;
    }

    /// <summary>
    /// Vector of generalized velocity
    /// </summary>
    double[] VelocityVector
    {
        get;
        set;
    }

    /// <summary>
    /// Matrix of kinetic energy
    /// </summary>
    double[,] KineticEnergyMatrix
    {
        get;
    }

    /// <summary>
    /// Derivation of Lagrangian by generalized coordinates
    /// </summary>
    double[] LagrangianDerivaton
    {
        get;
    }

    /// <summary>
    /// Generalized force
    /// </summary>
    double[] Force
    {
        get;
    }

    /// <summary>
    /// Potential energy
    /// </summary>
    double PotentialEnergy
    {
        get;
    }

    /// <summary>
    /// Updates itself
    /// </summary>
    void Update();
    
}

The following table represents the meaning of members of this interface:

Number	Member	Comment (meaning)
1	`Dimension`	Number of generalized coordinates
2	`Coordinates`	Generalized coordinates
3	`VelocityVector`	Vector of generalized velocity
4	`KineticEnergyMatrix`	Matrix of kinetic energy
5	`LagrangianDerivaton`	Derivation of Lagrangian by generalized coordinates
6	`PotentialEnergy`	State of connection
7	`PotentialEnergy`	Potential energy
8	`Update`	Updates itself

The following snippet contains the implementation of this interface:

/// <summary>
/// Pendulum
/// </summary>
public class Pendulum : ILagrangeMechanicalObject
{

    #region Fields

    /// <summary>
    /// Mass of pendulum
    /// </summary>
    double mass;

    /// <summary>
    /// Distance to mass center
    /// </summary>
    double r;

    /// <summary>
    /// Momentum of intertia
    /// </summary>
    double J;

    /// <summary>
    /// Generalized coordinates
    /// </summary>
    double[] coordinates = new double[3];

    /// <summary>
    /// Vector of generalized velocity
    /// </summary>
    double[] velocity = new double[3];

    /// <summary>
    /// Matrix of kinetic energy
    /// </summary>
    double[,] kinteticEnergy = new double[,] {
        {0, 0, 0},
        {0, 0, 0},
        {0, 0, 0}
        };

    /// <summary>
    /// Force
    /// </summary>
    double[] force = new double[] { 0, 0, 0 };

    /// <summary>
    /// Partial derication of Lagrangian by generalized coordinates
    /// </summary>
    double[] derivation = new double[3];

    /// <summary>
    /// Earth's gravitational acceleration
    /// </summary>
    const double g = 9.780318;

    /// <summary>
    /// Potential energy
    /// </summary>
    double potentialEnergy;

    #endregion

    #region Ctor

    /// <summary>
    /// Constuctor
    /// </summary>
    /// <param name="mass">Mass</param>
    /// <param name="r">Distance to mass center</param>
    /// <param name="J"></param>
    public Pendulum(double mass, double r, double J)
    {
        this.mass = mass;
        this.r = r;
        this.J = J;
    }

    #endregion

    #region ILagrangeMechanicalObject Members

    int ILagrangeMechanicalObject.Dimension
    {
        get
        {
            return 3;
        }
    }

    double[] ILagrangeMechanicalObject.Coordinates
    {
        get
        {
            return coordinates;
        }
        set
        {
            Array.Copy(value, coordinates, coordinates.Length);
        }
    }

    double[] ILagrangeMechanicalObject.VelocityVector
    {
        get
        {
            return velocity;
        }
        set
        {
            Array.Copy(value, velocity, velocity.Length);
        }
    }
 
    double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
    {
        get { return kinteticEnergy; }
    }

    double[] ILagrangeMechanicalObject.LagrangianDerivaton
    {
        get { return derivation; }
    }

    double[] ILagrangeMechanicalObject.Force
    {
        get { return force; }
    }

    double ILagrangeMechanicalObject.PotentialEnergy
    {
        get { return potentialEnergy; }
    }

    void ILagrangeMechanicalObject.Update()
    {
        // Fill of kinetic energy matrix
        kinteticEnergy[0, 0] = mass / 2;
        kinteticEnergy[1, 1] = mass / 2;
        kinteticEnergy[2, 2] = J / 2;

        double s = mass * r * Math.Sin(coordinates[2]) / 2;
        double c = mass * r * Math.Cos(coordinates[2]) / 2;

        kinteticEnergy[0, 2] = c;
        kinteticEnergy[1, 2] = s;

        kinteticEnergy[2, 0] = c;
        kinteticEnergy[2, 1] = s;

        // Fill derivation of partial derivations
        derivation[1] = mass * g;
        derivation[2] = mass * g * r * Math.Cos(coordinates[2]);

        // Calculation of potential energy
        potentialEnergy = mass * g * (coordinates[1] - r * Math.Cos(coordinates[2]));
    }

    #endregion
}

This snippet represents the mechanical model of the pendulum. Let us consider the construction of a mechanical model of the constrained system of two pendulums. This problem is separated into two parts:

Construction of a model of an unconstrained system of pendulums (usage of AggregatedLagrangeMechanicalObject class);
Construction of a model of a constrained system from a model of unconstrained pendulums (usage of AggregatedLagrangeMechanicalObject class).

There are two classes (usage of ConstrainedLagrangeMechanicalObject class).

The following snippets represent the code of these classes:

/// <summary>
/// Aggregated Lagrange Mechanical object
/// </summary>
public class AggregatedLagrangeMechanicalObject : ILagrangeMechanicalObject
{
    #region Fields

    /// <summary>
    /// Update action
    /// </summary>
    event Action updateAll;

    /// <summary>
    /// Generalized force
    /// </summary>
    double[] force;


    /// <summary>
    /// Generalized coordinates
    /// </summary>
    double[] coordinates;

    /// <summary>
    /// Vector of generalized velocity
    /// </summary>
    double[] velocity;

    /// <summary>
    /// Auxiliatry coordinate variable
    /// </summary>
    double[][] coord;

    /// <summary>
    /// Auxiliatry velocity variable
    /// </summary>
    double[][] vel;


    /// <summary>
    /// Chidren 
    /// </summary>
    ILagrangeMechanicalObject[] children;

    /// <summary>
    /// Derivation of Lagrangian
    /// </summary>
    double[] lagrangianDerivaton;


    /// <summary>
    /// Dimension of constrained object
    /// </summary>
    int dimension = 0;

    /// <summary>
    /// Matrix of kinetic energy
    /// </summary>
    double[,] kineticEnergyMatrix;

    /// <summary>
    /// Porential energy
    /// </summary>
    double potentialEnergy;

    #endregion

    #region Ctor

    /// <summary>
    /// Constructor
    /// </summary>
    /// <param name="children">Children</param>
    /// <param name="addForce">Additioonal force</param>
    public AggregatedLagrangeMechanicalObject(
           ICollection<ILagrangeMechanicalObject> children, 
           Func<double[]> addForce)
    {

        ILagrangeMechanicalObject[] ch = 
          children.ToArray<ILagrangeMechanicalObject>();
        this.children = ch;
        coord = new double[ch.Length][];
        vel = new double[ch.Length][];
        for (int i = 0; i < ch.Length; i++)
        {
            ILagrangeMechanicalObject ob = ch[i];
            int dim = ob.Dimension;
            dimension += dim;
            coord[i] = new double[dim];
            vel[i] = new double[dim];
        }
        force = new double[dimension];
        coordinates = new double[dimension];
        velocity = new double[dimension];
        lagrangianDerivaton = new double[dimension];
        kineticEnergyMatrix = new double[dimension, dimension];
        for (int i = 0; i < dimension; i++)
        {
            force[i] = 0;
            coordinates[i] = 0;
            velocity[i] = 0;
            for (int j = 0; j < dimension; j++)
            {
                kineticEnergyMatrix[i, j] = 0;
            }
        }

        updateAll += CommonUpdate;

        // If system has additional forces then common
        // update operation contain adding forces
        if (addForce != null)
        {
            updateAll += () =>
                {
                    double[] af = addForce();
                    for (int i = 0; i < dimension; i++)
                    {
                        force[i] += af[i];
                    }
                };
        }
    }

    #endregion

    #region ILagrangeMechanicalObject Members

    int ILagrangeMechanicalObject.Dimension
    {
        get { return dimension; }
    }

    double[] ILagrangeMechanicalObject.Coordinates
    {
        get
        {
            return coordinates;
        }
        set
        {
            Array.Copy(value, coordinates, coordinates.Length);
        }
    }

    double[] ILagrangeMechanicalObject.VelocityVector
    {
        get
        {
            return velocity;
        }
        set
        {
            Array.Copy(value, velocity, velocity.Length);
        }
    }

    double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
    {
        get { return kineticEnergyMatrix; }
    }

    double[] ILagrangeMechanicalObject.LagrangianDerivaton
    {
        get { return lagrangianDerivaton; }
    }

    double[] ILagrangeMechanicalObject.Force
    {
        get { return force; }
    }

    double ILagrangeMechanicalObject.PotentialEnergy
    {
        get { return potentialEnergy; }
    }

    void ILagrangeMechanicalObject.Update()
    {
        updateAll();
    }

    #endregion

    #region Specific Members

    private void CommonUpdate()
    {
        int d = 0;
        potentialEnergy = 0;
        for (int i = 0; i < children.Length; i++)
        {
            ILagrangeMechanicalObject ob = children[i];
            int dim = ob.Dimension;
            
            double[] c = coord[i];
            Array.Copy(coordinates, d, coord, 0, dim);
            ob.Coordinates = c;         // Assignment of child coordinates

            double[] v = vel[i];
            Array.Copy(velocity, d, v, 0, dim);
            ob.VelocityVector = v;      // Assignment of child velocty

            ob.Update();

            potentialEnergy += ob.PotentialEnergy;  // Sum of potential energies

            double[,] km = ob.KineticEnergyMatrix;

            for (int k = 0; k < dim; k++)   // Block diagonal martix of potential energy
            {
                for (int l = 0; l < dim; l++)
                {
                    kineticEnergyMatrix[k + d, l + d] = km[k, l];
                }
            }

            // Assignment of force
            Array.Copy(ob.Force, d, force, 0, dim);
            // Assignment of derivation
            Array.Copy(ob.LagrangianDerivaton, d, lagrangianDerivaton, 0, dim);
           
            d += dim;
        }
    }
    #endregion
}

/// <summary>
/// Constrained Lagrange Mechanical object
/// </summary>
public class ConstrainedLagrangeMechanicalObject : ILagrangeMechanicalObject
{

    #region Fields

    /// <summary>
    /// Update action
    /// </summary>
    event Action updateAll;

     /// <summary>
    /// Generalized force
    /// </summary>
    double[] force;

    /// <summary>
    /// Generalized coordinates
    /// </summary>
    double[] coordinates;

    /// <summary>
    /// Vector of generalized velocity
    /// </summary>
    double[] velocity;

    /// <summary>
    /// Vector of generalized velocity
    /// </summary>
    double[] velocityBuffer;


    /// <summary>
    /// Unconstrained object
    /// </summary>
    ILagrangeMechanicalObject unconstrainedObject;

    /// <summary>
    /// Derivation of Lagrangian
    /// </summary>
    double[] lagrangianDerivaton;

    /// <summary>
    /// Dimension of constrained object
    /// </summary>
    int dimension;

    /// <summary>
    /// Matrix of kinetic energy
    /// </summary>
    double[,] kineticEnergyMatrix;

    #endregion

    #region Ctor

    /// <summary>
    /// Constructor
    /// </summary>
    /// <param name="unconstrainedObject">Unconstrained object</param>
    /// <param name="dimension">Dimension of constrsaided system</param>
    /// <param name="transformation">Transformation of constrains</param>
    /// <param name="partialDerivation">Partial derivation</param>
    /// <param name="addForce">Additioonal force</param>
    public ConstrainedLagrangeMechanicalObject(ILagrangeMechanicalObject unconstrainedObject, 
        int dimension,
        Func<double[], double[]> transformation, 
        Func<double[,]> partialDerivation,
        Func<double[]> addForce)
    {
        this.unconstrainedObject = unconstrainedObject;
        this.dimension = dimension;
        force = new double[dimension];
        coordinates = new double[dimension];
        velocity = new double[dimension];
        velocityBuffer = new double[unconstrainedObject.Dimension];
        lagrangianDerivaton = new double[dimension];
        kineticEnergyMatrix = new double[dimension, dimension];

        // Update operation
        updateAll += () =>
        {
            CommonUpdate(unconstrainedObject, transformation, partialDerivation);
        };

        // If system has additional forces then common
        // update operation contain adding forces
        if (addForce != null)
        {
            updateAll += () =>
                {
                    double[] af = addForce();
                    for (int i = 0; i < dimension; i++)
                    {
                        force[i] += af[i];
                    }
                };
        }
    }
    #endregion

    #region ILagrangeMechanicalObject Members

    int ILagrangeMechanicalObject.Dimension
    {
        get { return dimension; }
    }

    double[] ILagrangeMechanicalObject.Coordinates
    {
        get
        {
            return coordinates;
        }
        set
        {
            Array.Copy(value, coordinates, coordinates.Length);
        }
    }

    double[] ILagrangeMechanicalObject.VelocityVector
    {
        get
        {
            return velocity;
        }
        set
        {
            Array.Copy(value, velocity, velocity.Length);
        }
    }

    double[,] ILagrangeMechanicalObject.KineticEnergyMatrix
    {
        get { return kineticEnergyMatrix; }
    }

    double[] ILagrangeMechanicalObject.LagrangianDerivaton
    {
        get { return lagrangianDerivaton; }
    }

    double[] ILagrangeMechanicalObject.Force
    {
        get { return force; }
    }

    double ILagrangeMechanicalObject.PotentialEnergy
    {
        get { return unconstrainedObject.PotentialEnergy; }
    }

    void ILagrangeMechanicalObject.Update()
    {
        updateAll();
    }

    #endregion

    #region Members

    private void CommonUpdate(ILagrangeMechanicalObject unconstrainedObject, 
        Func<double[], double[]> transformation,
        Func<double[,]> partialDerivation)
    {
        // Coordinates of unconstrained object
        double[] coord = transformation(coordinates);
        unconstrainedObject.Coordinates = coord;

        double[,] pd = partialDerivation(); // Velocity of unconstrained object
        RealMatrix.Multiply(pd, velocity, velocityBuffer);
        unconstrainedObject.VelocityVector = velocityBuffer;

        unconstrainedObject.Update(); // Updates unconstrained object

        RealMatrix.HtAH(pd, unconstrainedObject.KineticEnergyMatrix, 
                        kineticEnergyMatrix); // Calculation of kinetic energy matrix
        RealMatrix.Multiply(unconstrainedObject.LagrangianDerivaton, 
                            pd, lagrangianDerivaton); // Calculation of lagrangian derivaton
        RealMatrix.Multiply(unconstrainedObject.Force, pd, force);
        // Calculation of generalized force
    }

    #endregion
}

The following code represents the construction of a constrained system of two pendulums:

/// <summary>
///   Constructs Lagrange system of two constrained pendulums
/// </summary>
/// <param name="mass1">Mass of first pendulum</param>
/// <param name="r1">Radius of mass center of first pendulum</param>
/// <param name="J1">Momentum of inertia of first pendulum</param>
/// <param name="mass2">Mass of second pendulum</param>
/// /// <param name="r2">Radius of second center of first pendulum</param>
/// <param name="J2">Momentum of inertia of second pendulum</param>
/// <param name="force">Additional force</param>
/// <returns>Mechanical model</returns>
static public ILagrangeMechanicalObject ConstructTwoConstrainedPendulums(
       double mass1, double r1, double J1,
       double mass2, double r2, double J2, double force)
{
    ILagrangeMechanicalObject[] childen = new ILagrangeMechanicalObject[] // Children
    {
        new Pendulum(mass1, r1, J1),
        new Pendulum(mass2, r2, J2)
    };

    AggregatedLagrangeMechanicalObject unconstrainedAggregate = 
        // Unconstrained aggregate
        new AggregatedLagrangeMechanicalObject(childen, null);

    double[] af = new double[] { 0, force };            

    Func<double[]> addForce = () => {return af;};
    // Additional generalized force

    double[] aggrCoord = new double[6];  // Auxiliary variables
    double[,]  pd = new double[,]
       {{0, 0} , {0, 0}, {0, 1}, {0, 0} , {0, 0}, {0, 1}};

   Func<double[,]> partialDerivation = () => { return pd;}; 

    Func<double[], double[]> transformation = (double[] coord) =>
    {
        aggrCoord[0] = 0;
        aggrCoord[1] = 0;
        aggrCoord[2] = coord[0];
        aggrCoord[3] = r1 * Math.Sin(coord[0]);
        aggrCoord[4] = r1 * (1 - Math.Cos(coord[0]));
        aggrCoord[5] = coord[1];
        // Calculation of partial dervations
        pd[3, 0] = - (r1 * Math.Sin(coord[0]));
        pd[4, 0] =  (r1 * Math.Cos(coord[0]));
       return aggrCoord;
    };

    return new ConstrainedLagrangeMechanicalObject(
        unconstrainedAggregate, // Constrained Lagrange object
        2, 
        transformation, 
        partialDerivation,
        addForce);
}

Points of Interest

This article will take a long time to finish and I have shared it for discussion during writing. Discussion ideas would be included in the article.

History

To be continued...