Integration: Mechanics + Hydraulics + Navigation





5.00/5 (45 votes)
Sample of integration of branches of engineering.
- Download Mechanics Express project source code - 2.46 MB
- Download Mechanics + WPF 3D graphics project source code - 2.56 MB
- Download sample of Kalman filter of linear system - 3.01 KB
- Download sample of kinematics - 3.76 KB
- Download virtual reality sample - 424 KB
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:
- http://researchnews.osu.edu/archive/gps1.htm
- http://www.faqs.org/patents/app/20090069987
- http://www.freepatentsonline.com/EP1630636.pdf
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 xk- 1, xk are state vectors at k - 1 and kth steps, respectively. zk 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 xk- 1, xk correspond to k - 1 and kth steps. If we consider a GPS Bulldozer System, then the components of vectors xk- 1, xk 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 zk depends on the sensor which is used in the system. The zk vector contains the GPS measurements. The number of components of zk depends on the number of GPS antennas. The system can contain accelerometers and gyro. In this case, these measurements are also contained in zk. 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 xk and zk, 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 | x1 | Diff equations | x |
2 | x1 | Diff equations | y |
3 | x1 | Diff equations | z |
4 | a11 | Diff equations | a |
5 | a12 | Diff equations | b |
6 | a13 | Diff equations | c |
7 | a21 | Diff equations | d |
8 | a22 | Diff equations | f |
9 | a23 | Diff equations | g |
10 | a31 | Diff equations | h |
11 | a32 | Diff equations | i |
12 | a11 | Diff equations | a |
13 | z1 | Data | Formula_1 |
14 | z2 | Data | Formula_2 |
15 | b11 | Data | a |
16 | b12 | Data | b |
17 | b13 | Data | c |
18 | b21 | Data | d |
19 | b22 | Data | f |
20 | b23 | 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.
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:

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.
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.
- Download Mechanics + WPF 3D graphics project source code - 2.56 MB
- Download Virtual Reality sample - 424 KB
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 qi 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 x1, y1 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 q1, ..., qn are generalized coordinates of an unconstrained system. These coordinates depend on the generalized p1, ..., pm (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...