ABSTRACT This paper deals with teaching kinematic and dynamic
analysis of 3-D multibody systems in a context of courses with severe time constraints and the objective of attaining practical abilities. This high course efficiency is intended by the use of a simple theoretical approach (the natural or fully Cartesian coordinates) and a high level programming language (the function rich and easy to learn development tool Matlab1). The theoretical prerequisites for such a course can be kept to a minimum. This approach would allow the introduction of some lessons on multibody systems inside more general courses on machine dynamics. It can also be useful for short courses addressed to engineers in industry and for numerical analysis courses addressed to mechanical engineering students that are interested in practical applications of these numerical methods.
In this paper the achievable theory level will be presented in detail by means of a practical but non trivial example: a closed-chain 3-D robot.
Natural coordinates and Matlab are also a good starting point to present more advanced techniques such as numerical integration methods for ODEs and DAEs, or the not very well known automatic differentiation techniques. The latter is considered as a more advanced example in this paper.
1 INTRODUCTION The numerical methods for kinematic and dynamic
analysis of 3-D MBS can be classified as "global" or "topological". The former are simpler to implement because they treat all systems exactly in the same way, so they are also
1 Matlab® is a trademark of The Mathworks Inc. (http://www.mathworks.com).
more appropriate for teaching purposes when there are severe time limitations. Topological methods take advantage of the system characteristics and they are far more efficient, although more complex to describe and to implement. When applied to closed loop systems, topological methods need to use, at a lower problem size, the same or similar techniques that global methods use to deal with constraints. So, global methods are always a good starting point to learn all MBS techniques.
Global methods are normally based on some kind of Cartesian coordinates, chosen in such a way that they define directly the absolute position of each system body. Very often this is carried out by determining the position and orientation of a reference frame attached to every moving body. Nearly all formulations use three coordinates to determine the position of the frame origin or reference point, but they diverge in the method used to describe angular orientation.
There are two kinds of reference points that are more commonly used: the center of gravity and an input point (that is, a point related with the first joint of the body, when all the bodies are consecutively visited, starting from the fixed body). The center of gravity is a privileged point from the dynamic point of view, but it can have little or null interest from the geometric and kinematic points of view. It is desirable to let the user choose the point he or she prefers.
The angular orientation of a rigid body in the 3-D space is fully defined by the rotation or transformation matrix A. This is an orthogonal 3×3 matrix whose 9 elements must satisfy the 6 orthonormality conditions. Matrix A is very important because it transforms the body geometry, which is constant in its moving frame, to the global reference frame. However six constraints for each moving body are too many constraints and
most authors prefer to use a three or four parameter system to describe angular orientations.
The widest used three parameter systems are Euler angles (nutation θ , precession ψ and spin ϕ ) and Bryant angles (roll α , pitch β and yaw γ ). In the sequel, the expressions of the transformation matrix and of the angular velocity vector will be used as indicators of the formulation complexity. So, the transformation matrix in terms of Euler angles is given by:
c c s s c c s s c c s ss c c s c s s c c c c s
s s s c c
ψ ϕ ψ ϕ θ ψ ϕ ψ ϕ θ ψ θ
ψ ϕ ψ ϕ θ ψ ϕ ψ ϕ θ ψ θ
θ ϕ θ ϕ θ
⎡ ⎤− − −⎢ ⎥= + − + −⎢ ⎥⎢ ⎥⎣ ⎦
A (1)
The expression that relates the Euler angles rates and the angular velocity vector is:
cos 0 sin sinsin 0 sin cos
0 1 cos
x
y
z
ω ψ θ ψ θω ϕ θ ψ ψω θ ϕ
⎧ ⎫⎧ ⎫ ⎡ ⎤⎪ ⎪⎪ ⎪ ⎢ ⎥= −⎨ ⎬ ⎨ ⎬⎢ ⎥
⎪ ⎪ ⎪ ⎪⎢ ⎥⎩ ⎭ ⎣ ⎦ ⎩ ⎭
(2)
It can be seen that to compute matrix A from Euler angles needs six transcendental function evaluations. In addition to this, they have a singular position for 0θ = . Bryant angles present similar characteristics.
Because of this singularity, many authors prefer to use Euler parameters 0 1 2 3( , , , )e e e e , that don't have singularities. They are not independent, but related by the equation:
2 2 2 20 1 2 3 1e e e e+ + + = (3)
Using Euler parameters the transformation matrix A has a purely algebraic (quadratic) expression:
( ) ( )
( ) ( )( ) ( )
2 2 2 20 1 2 3 1 2 0 3 1 3 0 2
2 2 2 21 2 0 3 0 1 2 3 2 3 0 1
2 2 2 21 3 0 2 2 3 0 1 0 1 2 3
2 22 22 2
e e e e e e e e e e e ee e e e e e e e e e e ee e e e e e e e e e e e
⎡ ⎤+ − − − +⎢ ⎥= + − + − −⎢ ⎥⎢ ⎥− + − − +⎣ ⎦
A (4)
The relationship between their rates and angular velocity vector is given by:
There are many other systems of three or four parameters to represent angular orientation. References [1]-[3] give a good idea of this problem that is complex and difficult. To present this theory, even in a simple way, may require several lectures.
Another possibility is to use natural coordinates [4], which were introduced 25 years ago [5] as an alternative method to model general 3-D multibody systems. This method uses
Cartesian coordinates of points and Cartesian components of unit vectors to define both the bodies' position and orientation.
Consider a rigid body with two points ir and jr , and two
unit vectors u and v, non-coplanar with segment ( )j i−r r .
Then the following invertible 3×3 matrix can be defined:
j i⎡ ⎤≡ −⎣ ⎦X r r u v (6)
The transformation matrix A relates the current positions in X and the initial ones in 0X , allowing it to be written as a linear expression of natural coordinates:
10 0 −= ⇒ =X AX A XX (7)
To get the angular velocity vector ω is also very easy, using the related skew-symmetric matrix ω :
1 −= ⇒ =X ωX ω XX (8)
If a body only has two points and one non co-linear unit vector, it is always possible to generate a third vector in the initial and final position be using the cross product of vectors.
It can be seen that natural coordinates provide expressions to compute the transformation matrix and the angular velocity vector, that are simpler than the ones coming from the methods based on Euler angles or Euler parameters. This simplicity opens interesting possibilities in education.
2 A DRAFT PLAN FOR TEACHING MBS There are a relatively low number of universities that offer
courses on multibody dynamics. This situation and how it should be improved is considered in two recent papers from Cavacece, Pennestri and Sinatra [6] and Pennestri and Vita [7]. The authors of this paper agree with most of the remarks made in those papers, for instance on the proposal to introduce 45-hour long courses and on the necessity of doing practical work with computers. However, in this paper we are particularizing the methodology on one side, and enlarging the scope of possible learners, on the other.
The methodology we propose, based on natural coordinates and Matlab, can be applied to teaching multibody systems as a part of a more general course, for instance "Machine Theory", "Machine Dynamics", and so on. We think that 10 to 15 hours of lectures, plus 15 to 20 hours of personal work with Matlab, can be enough to get a limited but useful idea on the subject. The first author of this paper has good experience of introducing MBS lessons on a course on "Numerical Analysis" given to 4th year Mechanical Engineering students.
The fundamental ideas on which our proposal is based are the following ones: – Natural coordinates are appropriate because with them
many prerequisites are kept to a minimum. With them, 3-D
problems can be introduced directly, because they are not conceptually more complex than the 2-D ones.
– The introduction of several intermediate moving frames to define joints and forces is also avoided. Nearly everything is always considered in the global reference frame.
– The definition of joints can be carried out without constraints or with very simple ones. Rigid body constraints are always necessary, but they are very simple to formulate, also in the global reference frame.
– The need of pre-processing the data or post-processing the results is very low, so to get some graphic animations is quite easy.
– Natural coordinates allow the definition of the model and of the constraints of real world examples very quickly, just from the first lecture, if desired.
– For the dynamics, the virtual work (or virtual power) principle with constraints, allows in our opinion the easiest formulation of the motion differential equations.
– The complex problems that arise in MBS practice, such as the formulations leading to ODEs or DAEs of different index, can be quickly presented. The transformation matrix that relates dependent and independent velocities allows a simple and general implementation of the coordinate partitioning method. Many variants can be easily introduced based on this matrix. The need to stabilize the integration of index-1 DAEs and the relative efficiency of different approaches can be highlighted with no trouble.
– Matlab is a high level programming environment, very well suited to a direct implementation of the theoretical expressions and algorithms.
– The interactive debugger of Matlab with its very rich set of matrix functions, open the door to many pedagogical applications. For instance, the real number of degrees of freedom can be got from the rank of the Jacobian matrix. Over-determined systems can be analysed in the standard way, because the backslash operator (\) allows redundant but compatible equations.
– Singular positions can be detected easily, and their characteristics can be made evident.
– Matlab allows some verification code to be introduced with very few lines. This is important so as to be reasonably sure of the correctness of the program. A check of the constraints fulfilment in kinematics and an energy balance in direct dynamics are mandatory for the students to be sure about their results.
– Matlab is very compact. Many functions in MBS dynamics fit into a computer screen, so it is easy to explain how they work, even executing them step-by-step with the debugger and introducing checks and showing intermediate results.
– Matlab has a complete set of ODE integrators that can be used in many ways, allowing the user to see how the problem character determines its efficiency and behaviour.
– On the other side Matlab is not very efficient with small problems, but this is not very important for teaching.
The content of a short course on multibody dynamics can be constructed around some slides that concisely but with detail present the theory, and some practical examples that cover the more common real problems: the suspension or steering system of a car, a robot, a satellite, and so on. The selection of practical examples should include open-loop and closed-loop systems. It is very instructive to see how the integration time varies with the problem, with the formulation and with the numerical integrator.
In the rest of the paper the authors will try to prove the previous assertions using a simple but non trivial example.
Figure 1. An industrial closed-chain robot.
1u
X
Y
Z
2 u3u
3u 3u 4 u
3u5u
1r
2 r 3r
5r4 r
6 r
8r
1ψ
2ψ
3 ψ
5ψ
7r 4 ψ
6ψ
Figure 2. Kinematic scheme of the robot.
3 NATURAL COORDINATES THROUGH AN EXAMPLE
Natural coordinates allow a quick and simple definition of constraint equations. So they can find a very important field of application in teaching MBS, when there are important time limitations. Students only need a very elemental mathematical
and mechanical background, and the instructor can introduce a relatively complex model just in the first one-hour lecture.
Figure 1 shows an industrial closed chain robot, and Figure 2 shows its kinematic model including rigid bodies and joints (all revolute). Point 1r and unit vectors 1u and 2u are fixed. There are 8 rigid bodies (plus the fixed one) and 9 joints for 6 degrees of freedom. The angles 1ψ to 6ψ are depicted in Figure 2 as the degrees of freedom of the robot.
The dependent coordinates used to define the position of the robot in Figure 2 will be the grouped in the following 45×1 vector q:
TT T T T T T T T T T T T T ψ ψ⎡ ⎤= ⎣ ⎦q r r u u u r r r r r u r u (9)
Seven of the nine revolute joints have been introduced by the sharing of a point and a unit vector between two contiguous bodies. Observe that the unit vector 3u allows the axis definition in five joints. Consider also that two revolute joints, related with angles 4ψ and 6ψ have been defined by sharing two points.
3.1 Rigid body constraint equations The points and unit vectors that belong to a rigid body
must move according to the rigid body condition, which obliges distances and angles to be kept. Both types of conditions can be enforced with the dot product of vectors. A rigid body in the 3-D space has 6 degrees of freedom, so if it has, for instance, 15 natural coordinates they shall be related by 9 constraint equations. Next, the constraint equations for all the robot moving bodies will be explicitly given.
– Body 2 (points 1r , 2r and 3r ; unit vectors 1u and 3u )
( ) ( ) 22 1 122 1 0T L− − =−r r r r (10)
( ) ( ) 23 2 3 2 23 0T L− − − =r r r r (11)
( ) ( ) 21 3 1 3 13 0T L− − − =r r r r (12)
( )3 1 3 0T − =u r r (13)
( )3 2 3 0T − =u r r (14)
3 3 1 0T − =u u (15)
3 1 0T =u u (16)
( )2 1 1 12 cos 0T L α− − =r r u (17)
– Body 3 (points 2r and 4r ; unit vector 3u )
( ) ( ) 24 2 4 2 24 0T L− − − =r r r r (18)
( )3 4 2 0T − =u r r (19)
– Body 4 (points 3r and 5r ; unit vector 3u )
( ) ( ) 25 3 5 3 35 0T L− − − =r r r r (20)
( )3 5 3 0T − =u r r (21)
– Body 5 (points 4r , 5r and 6r ; unit vector 3u )
For each of the body constraints, care has been taken to not repeat constraints previously defined. For instance, vector 3u belongs to many elements, but its unit module condition is only introduced the first time it appears.
There are a total of 25 constraints equations, but only 24 are independent. In fact the four-bar part of the robot constitutes an over-constrained sub-system and the constraints for body 4 could be reduced to only the constant distance condition (20).
As a comparative remark, consider that eight movable bodies will represent 48 coordinates with Euler angles and 56 with Euler parameters. With natural coordinates there are 7 moving points, 3 moving unit vectors and 6 angles, for a total of 36 position variables. In addition to this, using natural coordinates, the constraint equations and their derivatives are by far easier to formulate and to program.
3,2 Joint constraint equations Some joints (such as the prismatic and cylindrical ones)
that don't share any point, can not be introduced by just sharing variables and need some specific constraint equations, that are also quadratic. As these types of joints don't appear in this example, they are not described here and the reader can find detailed information in García de Jalón and Bayo [5].
3.3 Constraints to define relative coordinates In many application fields and in Robotics in particular, it
is very useful to introduce relative coordinates in the joints that are driven by actuators. This is straightforward when natural coordinates are used. The angle ψ in a revolute joint can be introduced through a constraint equation that relates this angle with the Cartesian coordinates of points and vectors. This constraint equation can be based on the dot product of vectors and on the cross product of vectors. Both are necessary according to the angle value, but not at the same time.
Consider as an example angle 1ψ shown in Figure 2. This angle can be defined as the angle between vector 2u (fixed) and vector 3u (movable) measured on vector 1u (fixed). This definition is necessary in 3-D to set correctly the angle sign.
Angle 1ψ can be introduced as a new dependent coordinate through the dot product of vectors:
2 3 1cos 0T ψ− =u u (35)
but this equation fails to define the angle if its value is near ( 0,1, 2,...)n nπ± = rads. In this case it is necessary to use
one of the three equations coming from the cross product of vectors (the equation corresponding to the largest component of
1u ):
2 3 1 1sinψ× − =u u u 0 (36)
In any case, eq. (35) or one of eqs. (36) allows the definition of angle 1ψ as a new variable. The remaining robot angles are defined analogously as follows:
– 2ψ is the angle from ( )4 2−r r to 1u measured on 3u :
( )4 2 1 24 2sin 0T L ψ− − =r r u (37)
( )4 2 1 3 24 2sinL ψ− × − =r r u u 0 (38)
– 3ψ is the angle from ( )4 6−r r to ( )7 6−r r on 3u :
( ) ( )4 6 7 6 46 67 3cos 0T L L ψ− − − =r r r r (39)
( ) ( )4 6 7 6 3 46 67 3sin 0L L ψ− × − − =r r r r u (40)
– 4ψ is the angle from 3u to 4u on ( )7 6−r r :
3 4 4cos 0T ψ− =u u (41)
( )3 4 7 6 67 4sinL ψ× − − ⋅ =u u r r 0 (42)
– 5ψ is the angle from ( )6 7−r r to ( )8 7−r r on 4u :
( ) ( )6 7 8 7 67 78 5cos 0T L L ψ− − − =r r r r (43)
( ) ( )6 7 8 7 4 67 78 5sin 0L L ψ− × − − =r r r r u (44)
– 6ψ is the angle from 4u to 5u on ( )8 7 :−r r
4 5 6cos 0T ψ− =u u (45)
( )4 5 8 7 78 6sinL ψ× − − ⋅ =u u r r 0 (46)
Equations (35)-(46) allow the selection of the six equations that define the angles 1ψ to 6ψ as additional coordinates. Except for singular configurations of the robot, these angles can be chosen as its independent coordinates.
4 MBS DEFINITION USING MATLAB With natural coordinates, Matlab allows a simple definition
of multibody systems and also a simple implementation of the kinematic and dynamic analyses. In the sequel, both the geometry in the initial position and the constraint equations will be considered.
4.1 Geometry definition with natural coordinates It is a simple task to define the robot geometry at the initial
position. It is only necessary to define two matrices P and U
whose rows contain respectively the point coordinates and the unit vectors components, as it shown in Table 1.
Table 1. Points and vectors to define the MBS geometry. % Coordinates of points in the initial position % Element P(i,4) is the position of point i % coordinates in vector q P = [ 0 0 0 1 0 0.2088 0.6683 4 0 -0.1827 0.7257 16 0 0.2088 1.7647 19 0 -0.1827 1.8221 22 0 0.2871 2.0257 25 0 1.4130 1.3757 28 0 1.6427 1.3757 34]; % components of unit vectors % Element U(i,4) is the position of vector i % in vector q U=[ 0, 0, 1, 7 0, 1, 0, 10 1, 0, 0, 13 1, 0, 0, 31 0, 0, 1, 37];
These matrices P and U define points and vectors, but it is necessary also to define which points and vectors belong to each rigid body. This can be done by defining the constraint equations as in the next Section.
4.2 Constraint definition with natural coordinates There are two ways to get the definition of constraint
equations, the manual one and the automatic one. The latter one can be based on the joints and bodies definitions, because if the points and unit vectors that belong to a body are known, it is a relatively simple task to set the conditions that guarantee the rigid body motion. If there are joints that require the definition of constraints, this is also easy to introduce automatically.
For educational purposes, however, it is usually preferable to define constraints manually, so as to allow the instructor to explain in detail how they arise and to consider alternatives.
The constraint equations are manually defined through a matrix CONSTR whose rows represent each one of the different system constraints. The first element in each row is a code number that represents the constraint type. The most common constraints to set the rigid body condition are the following ones:
The equation type "1000" is a constant distance condition. To be defined, it needs the two point' numbers i and j, and the distance value Lij.
The equation type "1001" is a unit module condition that only needs the unit vector number.
The equation type "1002" is a constant angle condition between a unit vector and a segment determined by two points. The data to define is the number k of the unit vector, the numbers i and j of the points, the distance Lij and the cosine of the angle cfi that must be kept constant.
The equation type "1003" is a constant angle condition between two unit vectors i and j. It needs the unit vector numbers i and j and the cosine of the angle cfi between them.
The equations "1002" and "1003" can only be used for angles different from ( 0,1,2,...)n nπ± = .
For the rigid body condition of the bodies of the robot in Figure 2 no other kinds of equations are needed. There are a few more types of equations as simple as the previous ones, but for the sake of brevity will not be considered here. Table 3 shows how the user shall define the rigid body constraint equations for the robot in Figure 2 using a Matlab matrix.
Table 4. Constraints types to define angles in the joints. % 1010 (rj-ri)X(rl-rk)-um*Lij*Lkl*sin(psi) % [1011 i j k l m 0 0 0 Lij Lkl ipos] % 1011 (rj-ri)X(rl-rk)-(rm-rn)*Lij*Lkl*sin(psi)/Lmn % [1012 i j k l m n 0 Lij Lkl Lmn ipos] % 1012 uiXuj-uk*sin(psi) % [1014 i j k 0 0 0 0 0 0 0 ipos] % 1013 uiXuj-(rk-rl)*sin(psi)/Lkl % [1015 i j k l 0 0 0 0 0 Lkl ipos] % 1014 ukX(ri-rj)-ul*Lij*sin(psi) % [1017 k i j l 0 0 0 0 0 Lij ipos] % 1015 ukX(ri-rj)-(rl-rm)*Lij*sin(psi)/Llm % [1018 k i j l m 0 0 0 Lij Llm ipos]
Equation type "1010" defines the angle between segment i-j and segment k-l, measured on unit vector m. Both lengths Lij and Lkl are included, as well as the constant ipos that tells the position in vector q where the angle is stored, after the points and unit vectors.
Equation type "1011" is similar to the previous one, but in this case the angle is measured on the segment m-n, which means corresponding length Lmn must be given.
Equation type "1012" defines the angle between unit vector i to unit vector j measured on unit vector k. No lengths are necessary in this case.
Equation type "1013" defines the angle between two unit vectors measured on a segment k-l. Equations of types "1014" and "1015" measure angles from a unit vector k to a segment i-j measured respectively on a unit vector l or on a segment l-m.
If the dot product of vectors can be used, not all the information in these Tables is necessary.
For the robot being considered, the constraint equations (35)-(46) are defined by appending to the matrix CONSTR defined in Table 3 the rows added in Table 5.
Table 5. Angles definition for a robot. CONSTR = [ CONSTR; [... % psi1: angle from u3 to u2 on u1 . 1012, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 1 % psi2: angle from u1 to r4-r2 on u3 1014, 1, 4, 2, 3, 0, 0, 0, 0, 0, L24, 2 % psi3: angle from r4-r6 to r7-r6 on u3 1010, 6, 4, 6, 7, 3, 0, 0, 0, L46, L67, 3 % psi4: angle from u3 to u4 on r7-r6 1013, 3, 4, 7, 6, 0, 0, 0, 0, 0, L67, 4 % psi5: angle from r7-r7 to r8-r7 on u4 1010, 7, 6, 7, 8, 4, 0, 0, 0, L67, L78, 5 % psi6: angle from u5 to u4 on r8-r7 1013, 5, 4, 8, 7, 0, 0, 0, 0, 0, L78, 6 ] ];
4.3 Inertia and external forces definition In the previous Sections the reader can realize how easy it
is to define the kinematics of a relatively complex multibody system using the approach described in this paper. In this Section we will present how to define the inertia properties necessary to carry out dynamic analyses, both direct and inverse.
The inertia properties are defined by an array of struct variables whose fields are the mass 'm', the center of gravity position vector 'cog' with respect to the body moving frame,
and the inertia tensor 'IT' with respect to a reference frame located in the center of gravity and whose axes are parallel to the body moving frame. Natural coordinates also need to know on which points (at least two, given by the 'r' field) and on which unit vectors (at least one, given by the 'u' field) of the body, the computation of the inertia matrix will be based (see Chapter 4 in [4]). As an example we include the definition of inertia properties for the first two robot moving bodies:
The external forces always will be defined by a user function. This is a very general way of doing that.
The data that define a MBS is given by matrices P, U, CONSTR, the structure Inertia and the function that defines applied forces. The rest of the Matlab code is general purpose, the same for all applications.
4.4 Graphic representation of the MBS A graphic representation of the MBS, even a very simple
one, is always very important, but yet more important for education purposes. Natural coordinates provide simple graphic representations in a direct way, as it can be seen in Figure 3. The information needed to draw this picture is given by the matrices (in transposed form, for the sake of space) in Table 7.
Each row of matrix LINES represents a black line in the figure, joining points LINES(i,1) and LINES(i,2). Each row in matrix UVECT represents the unit vector UVECT(i,1) drawn in red from the point UVECT(i,2). A single unit vector can be drawn many times, with different origins. This is very useful to represent joints with parallel axes.
Figure 3. Direct representation of natural coordinates.
Figure 4. A more realistic robot representation.
If a more realistic drawing is wanted, for instance the one based in sets of polygons as shown in Figure 4, then a more conventional approach shall be applied, based on the transformation of the geometry defined in the moving frame to the global frame. This can be carried out using the position of a body point and the transformation matrix A given by eq. (7).
5 ROBOT KINEMATIC AND DYNAMIC SIMULATION The kinematic position analysis can be carried out from the
constraint equations (10)-(46), defined for Matlab in Table 3 and Table 5. These equations can be represented as:
( ) =Φ q 0 (47)
where q is the vector of coordinates and angles defined in (9). It is necessary to partition the elements into q in two
disjoint sets, called fixed and free. In this case, the fixed coordinates are those of point 1r and vectors 1u and 2u , so:
In a similar way, free coordinates can be partitioned into dependent qdep and independent qind coordinates. For the robot in Figure 1 the angles 1ψ to 6ψ can be taken as independent coordinates, so:
Non-linear eqs. (47) can be solved by the Newton-Raphson iterative method:
( ) ( )11i i i i
−+ = − qq q Φ q Φ q (48)
The velocity and acceleration constraint equations can be obtained by taking time derivatives of eq. (47). In this case it can be useful to distinguish the two types of free coordinates:
d
d ii
⎡ ⎤⎡ ⎤ =⎢ ⎥⎣ ⎦
⎣ ⎦q q
qΦ Φ 0
q (49)
d
d ii
⎡ ⎤⎡ ⎤ = −⎢ ⎥⎣ ⎦
⎣ ⎦q q q
qΦ Φ Φ q
q (50)
where matrix dqΦ shall be invertible, so as to allow the
dependent variables be expressed in terms of the independent ones. From the kinematics point of view it is also useful to compute a matrix R whose columns are a basis of the nullspace of qΦ :
Using matrix R the motion differential equations can be set as the following system of ODEs:
T i T T i= −R MRq R Q R MRq (54)
where Q is the vector of external forces and the product iRq can be computed from (50) and (53) as the vector of accelerations q computed with the true velocities and null independent accelerations.
Eqs. (47)-(54) can be implemented very easily with Matlab. The key point is the evaluation of the constraint
equations, its Jacobian and the r.h.s. of eq. (50), which can be done with calls to the following functions: Phi = formPhi(q,CONSTR,P,U); Phiq = formPhiq(q,CONSTR,P,U); c = formPhiqpqp(q,vel,CONSTR,P,U);
Matlab allows an easy implementation of several dynamic formalisms, based on dependent or independent coordinates, with Lagrange multipliers, penalty terms, or some stabilization techniques. It is instructive to see the Matlab code that computes the derivative of the state vector ,T iT iT⎡ ⎤= ⎣ ⎦y q q , because it includes the solution of the position problem with (48), the velocity calculation with (52), and the independent accelerations obtained from eq. (54).
Table 8. Computation of the state vector derivative function yp=derivR(t,y, ... CONSTR,P,U,Inertia,free,fixed,qini,qind,qdep) % The state vector is y=[vi,qi]; tol=1e-12; nitmax=40; persistent qprev if t==0, qprev=qini; end dof=length(qind); nf=length(free); n=length(qini); % Extract indep. pos. qi and vels. vi from y q = qprev; q(qind) = y(dof+1:dof*2); vi(1:dof,1) = y(1:dof); % solve the position problem Phi = formPhi(q,CONSTR,P,U); err = norm(Phi); nit=0; while err>tol && nit<nitmax Phiq = formPhiq(q,CONSTR,P,U); q(qdep) = q(qdep)-Phiq(:,qdep)\Phi; Phi = formPhi(q,CONSTR,P,U); err=norm(Phi); nit=nit+1; end qprev=q; % compute the updated Jacobian and matrix R Phiq = formPhiq(q, CONSTR, P, U); R = zeros(n,5); R(qdep,:) = -Phiq(:,qdep)\Phiq(:,qind); R(qind,:) = eye(dof); % compute the dependent velocities v and Rpqip v(free,1) = R(free,:)*vi; v(fixed) = 0; c = formPhiqdqd(q, v, CONSTR, P, U); Rpqip = zeros(length(q),1); Rpqip(qdep)=Phiq(:,qdep)\c; % dynamic terms of the motion differential equations [M,Qin] = mass(Inertia, q, v, P, U); Qext = forces(Inertia, q, v, P, U); Q=Qext+Qin; % independent accelerations ai = (R(free,:)'*M(free,free)*R(free,:))\... (R(free,:)'*(Q(free,:)-M(free,free)*Rpqip(free)); % state vector derivative yp = [ai; vi];
6 AN EXAMPLE OF A MORE ADVANCED TOPIC INTRODUCED THROUGH NATURAL COORDINATES
The simplicity of formulations based on natural coordinates also allows an easier introduction of more advanced topics on multibody system courses, such as numerical integrators, stabilization techniques for DAEs, sensibility analysis, and so on. As an example of these kinds of
applications, in this paper two examples of Automatic Differentiation (AD) applied to MBS will be presented (see references [8]-[9]).
AD [10]-[11] is a procedure to evaluate the derivatives of a function defined not by a mathematical expression but by a computer program. AD is nowadays a well established technique both in theory and practice, although many potential users are not yet familiar with it. It differs from numerical differentiation techniques because it is not an approximate technique but an exact one. The two main families of AD techniques are source transformation and operator overloading. The former is easier to understand to the neophyte and it has been the one used in this paper. In this case, the input is a computer function written in a language such as Fortran, C/C++ or Matlab. The user shall specify the set of variables with respect to the original function being differentiated. The output is a computer code that evaluates the derivative the user wants: the gradient, the Hessian, the Jacobian,... In this paper, the AD tool ADiMAT [12] has been used to generate Matlab code.
MBS formulations always need derivatives, at least to compute the Jacobian of the constraint equations (47) with respect to the dependent coordinates vector q. Equations (48)-(51) are based in the values of the Jacobian matrix. An efficient code to evaluate the Jacobian matrix is easy to obtain manually, particularly if natural coordinates are used. So it is a very difficult challenge for AD to become competitive in this case.
After installing ADiMAT, the Matlab code to compute the Jacobian is obtained by executing the function addiff in the Command Window with just two arguments, the handle of the Matlab function that evaluates the constraints (47) and the name of the variable with respect to which the differentiation is carried out: >> addiff(@formPhi,'q');
As a result, ADiMAT generates a Matlab function called g_formPhi. The user must include in his code an explicit call to this function, as for instance in the form: [g_q] = createFullGradients(q); [Phiq,Phi] = g_formPhi(g_q,q,CONSTR,P,U);
The results are quite satisfactory in terms of precision, but very disappointing from the point of view of efficiency, as can be seen in Table 9, which shows the results of a 0.5s long dynamic simulation of the robot in Figure 2, using
,T iT T⎡ ⎤= ⎣ ⎦y q q as state vector and a Pentium IV 3 Ghz PC.
Table 9. Manual and Automatic Differentiation in the computation of the Jacobian.
Matlab integrator CPU time for MD CPU time for AD ode113 1.843 73.469 ode45 4.125 198.625 ode15s 3.953 183.859
It shall be pointed out the ADiMAT generated code has not been designed for maximum numerical efficiency and it is not fully representative of AD capabilities. Using ADOLC [13], which is an operator overloading tool, we have obtained CPU times very similar to / sometimes faster than the ones obtained by manual code generation.
There are other more advantageous fields of application of AD to multibody systems, as for instance the computation of Jacobians for implicit numerical integrators. The dynamic equations (54) can be written in the state space form required by Matlab integrators in the form (see code in Table 8):
( ) ( ) ( )1
, ,i T T i
i
it
−⎡ ⎤⎡ ⎤ −⎢ ⎥= = =⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦
q R MR R Q MRqy f q qq Rq
(55)
The implicit integrators force the fulfilment of this equation in the new instant of time, where the state vector y is unknown. As a consequence a system of nonlinear equations shall be solved in each integration step. This task can be carried out faster if the integrator has the Jacobian of ecs. (55). The Matlab implicit integrators, such as the ode23t and ode15s functions allow the user to provide the handle of a function that computes this Jacobian. Otherwise, they approximate it numerically.
Table 10 shows the results obtained with two implicit integrators of Matlab, comparing numerical and automatic differentiation.
Table 10. Numerical and Automatic calculation of derivatives in Matlab implicit integrators.
Integrator Jac, evals. time with
numerical Jac. time with AD and ADiMAT
ode23t 1 7.531 9.266 ode15s 3 3.953 7.765
It seems that now the CPU times are more similar and it is possible that in other examples AD becomes more competitive. The role of natural coordinates and Matlab has been to allow an easier evaluation of the possibilities of AD in multibody systems to be carried out.
7 CONCLUSIVE REMARKS It is worthwhile that nearly all students of Mechanical
Engineering receive some formation and training in kinematic and dynamic simulation of multibody systems, considering the importance in many industrial sectors. However, until now only a relatively low number of universities have been able to offer full courses on this subject.
The methodology proposed in this paper, based on the use of natural coordinates and Matlab, allow the instructors to introduce the subject in a useful manner starting from very little time available, for instance inside more general courses as
Machine Dynamics or Numerical Methods for Mechanical Engineers. The students are able to produce, modify and use computer code to solve real life cases in a very simple and direct way. Of course, also full MBS dedicated courses and even distance courses offered through Internet can benefit from this approach.
8 REFERENCES [1]. Argyris, J., "An excursion into large rotations", Comput.
Methods Appl. Mech. Eng. 32, 85-155, (1982). [2]. Géradin, M. and Rixen, D., "Parametrization of finite
rotations in computational dynamics: a review", Revue Européenne des Éléments Finis, 4, 497-553, (1995).
[3]. Angeles, J., "Fundamentals of Robotic Mechanical Systems", 2nd ed., Springer-Verlag, New York, (2003).
[4]. García de Jalón, J. and Bayo, E., "Kinematic and Dynamic Simulation of Multi-Body Systems. the Real-Time Challenge", Springer-Verlag, New-York (1993). Available online from http://mat21.etsii.upm.es/mbs/bookPDFs/bookGjB.htm.
[5]. García de Jalón, J., Serna, M. A., Viadero, F. and Flaquer, J. "A Simple Numerical Method for the Kinematic Analysis of Spatial Mechanisms", ASME Journal on Mechanical Design, 104, 78-82, (1982).
[6]. Cavacece, M., Pennestri, E. and Sinatra, R. "Experiences in Teaching Multibody Dynamics", Multibody System Dynamics, 13, 363–369, (2005).
[7]. Pennestrì, E. and Vita, L. "Multibody dynamics in advanced education", Advances in Computational Multibody Systems, ed. by J.A.C. Ambrósio, Kluwer, pp. 345-370, (2006).
[8]. Griffith, T., Turner, J., Junkings, J.L., "Some Applications of Automatic Differentiation to Rigid, Flexible, and Constrained Multibody Dynamics", ASME Proceedings of IDECT/CIE, Paper No. DETC2005-85640, (2005).
[9]. Stadler, W. and Eberhard, P., "Jacobian Motion and its Derivatives", Mechatronics, 11, 563-593, (2001).
[10]. Rall, L. B., "Automatic Differentiation: Techniques and Applications", G. Goos and J. Hartamis editors, Lecture Notes in Computer Science, Springer-Verlag, New York, (1981).
[11]. Bücker, H. M., Corliss, G.. F., Hovland, P. D., Naumann, U. and Norris, B., "Automatic Differentiation: Applications, Theory, and Implementations", Springer-Verlag, (2005).
[12]. Bischof, C. H., Lang, B. and Vehreschild, A., "Automatic Differentiation for MATLAB Programs", Proceedings in Applied Mathematics and Mechanics, 2, 50-53, (2003).
[13]. Griewank, A., Juedes, D. and Utke, J., "ADOL-C, A Package for the Automatic Differentiation of Algorithms Written in C/C++", ACM Transactions on Mathematical Software, 22, 131-167, (1996).
