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 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:
public interface IObjectTransformer
{
string[] Input { get; }
string[] Output { get; }
object GetInputType(int i);
object GetOutputType(int i);
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:
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:
AbstractDoubleTransformer transformer;
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:
public static T GetObject<T>(IAssociatedObject obj) where T : class
{
if (obj is T)
{
return obj as T;
}
if (obj is IChildrenObject)
{
IChildrenObject co = obj as IChildrenObject;
IAssociatedObject[] ch = co.Children;
if (ch != null)
{
foreach (IAssociatedObject ob in ch)
{
T a = GetObject<T>(ob);
if (a != null)
{
return a;
}
}
}
}
return null;
}
The following code can be used for finding an object which implements IObjectTransformer:
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:
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;
double[] inp = input[0] as double[];
collection.SetStateVector(inp);
double start = provider.Time;
double step = provider.Step;
runtime.StartAll(start);
runtime.UpdateAll();
processor.Step(start, start + step);
provider.Time = start + step;
collection.GetStateVector(outbuffer);
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:
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.
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:
public abstract class PositionObjectFactory
{
static private PositionObjectFactory factory;
public static PositionObjectFactory Factory
{
get
{
return factory;
}
set
{
factory = value;
}
}
public abstract object CreateObject(string type);
public abstract Camera NewCamera();
public abstract object CreateForm(Camera camera);
public abstract object CreateForm(IPosition position, IVisible visible);
public abstract Type CameraType
{
get;
}
public abstract IObjectLabel CreateLabel(object obj);
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 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:
public interface ILagrangeMechanicalObject
{
int Dimension
{
get;
}
double[] Coordinates
{
get;
set;
}
double[] VelocityVector
{
get;
set;
}
double[,] KineticEnergyMatrix
{
get;
}
double[] LagrangianDerivaton
{
get;
}
double[] Force
{
get;
}
double PotentialEnergy
{
get;
}
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:
public class Pendulum : ILagrangeMechanicalObject
{
#region Fields
double mass;
double r;
double J;
double[] coordinates = new double[3];
double[] velocity = new double[3];
double[,] kinteticEnergy = new double[,] {
{0, 0, 0},
{0, 0, 0},
{0, 0, 0}
};
double[] force = new double[] { 0, 0, 0 };
double[] derivation = new double[3];
const double g = 9.780318;
double potentialEnergy;
#endregion
#region Ctor
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()
{
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;
derivation[1] = mass * g;
derivation[2] = mass * g * r * Math.Cos(coordinates[2]);
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:
public class AggregatedLagrangeMechanicalObject : ILagrangeMechanicalObject
{
#region Fields
event Action updateAll;
double[] force;
double[] coordinates;
double[] velocity;
double[][] coord;
double[][] vel;
ILagrangeMechanicalObject[] children;
double[] lagrangianDerivaton;
int dimension = 0;
double[,] kineticEnergyMatrix;
double potentialEnergy;
#endregion
#region Ctor
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 (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;
double[] v = vel[i];
Array.Copy(velocity, d, v, 0, dim);
ob.VelocityVector = v;
ob.Update();
potentialEnergy += ob.PotentialEnergy;
double[,] km = ob.KineticEnergyMatrix;
for (int k = 0; k < dim; k++)
{
for (int l = 0; l < dim; l++)
{
kineticEnergyMatrix[k + d, l + d] = km[k, l];
}
}
Array.Copy(ob.Force, d, force, 0, dim);
Array.Copy(ob.LagrangianDerivaton, d, lagrangianDerivaton, 0, dim);
d += dim;
}
}
#endregion
}
public class ConstrainedLagrangeMechanicalObject : ILagrangeMechanicalObject
{
#region Fields
event Action updateAll;
double[] force;
double[] coordinates;
double[] velocity;
double[] velocityBuffer;
ILagrangeMechanicalObject unconstrainedObject;
double[] lagrangianDerivaton;
int dimension;
double[,] kineticEnergyMatrix;
#endregion
#region Ctor
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];
updateAll += () =>
{
CommonUpdate(unconstrainedObject, transformation, partialDerivation);
};
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)
{
double[] coord = transformation(coordinates);
unconstrainedObject.Coordinates = coord;
double[,] pd = partialDerivation();
RealMatrix.Multiply(pd, velocity, velocityBuffer);
unconstrainedObject.VelocityVector = velocityBuffer;
unconstrainedObject.Update();
RealMatrix.HtAH(pd, unconstrainedObject.KineticEnergyMatrix,
kineticEnergyMatrix);
RealMatrix.Multiply(unconstrainedObject.LagrangianDerivaton,
pd, lagrangianDerivaton);
RealMatrix.Multiply(unconstrainedObject.Force, pd, force);
}
#endregion
}
The following code represents the construction of a constrained system of two pendulums:
static public ILagrangeMechanicalObject ConstructTwoConstrainedPendulums(
double mass1, double r1, double J1,
double mass2, double r2, double J2, double force)
{
ILagrangeMechanicalObject[] childen = new ILagrangeMechanicalObject[]
{
new Pendulum(mass1, r1, J1),
new Pendulum(mass2, r2, J2)
};
AggregatedLagrangeMechanicalObject unconstrainedAggregate =
new AggregatedLagrangeMechanicalObject(childen, null);
double[] af = new double[] { 0, force };
Func<double[]> addForce = () => {return af;};
double[] aggrCoord = new double[6];
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];
pd[3, 0] = - (r1 * Math.Sin(coord[0]));
pd[4, 0] = (r1 * Math.Cos(coord[0]));
return aggrCoord;
};
return new ConstrainedLagrangeMechanicalObject(
unconstrainedAggregate,
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...
