Differential-Geometric Modelling and Dynamic Simulation of Multibody Systems

Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 597

CODEN STJSAO ISSN 0562-1887 ZX470/1418 UDK 531.392:531.314:004.94:512.81

Andreas MÜLLER1) and Zdravko TERZE2) Original scientific paper A formulation for the kinematics of multibody systems is presented, 1) University Duisburg-Essen which uses Lie group concepts. With line coordinates the kinematics is Chair of Mechanics and Robotics parameterized in terms of the screw coordinates of the joints. Thereupon, the Lagrangian motion equations are derived, and explicit expressions are Lotharstrae 1, Germany given for the objects therein. It is shown how the kinematics and thus the 2) Fakultet strojarstva i brodogradnje Sveučilišta motion equations can be expressed without the introduction of body-fixed u Zagrebu (Faculty of Mechanical Eng. & reference frames. This admits the processing of CAD data, which refers to Naval Arch, University of Zagreb), a single (world) frame. For constrained multibody systems, the Lagrangian Ivana Lučića 5, HR-10000 Zagreb motion equations are projected to the constraint manifold, which yields the Republic of Croatia equations of Woronetz. The mathematical models for numerical integration routines of MBS are surveyed and constraint gradient projective method [email protected] for stabilization of constraint violation is presented.

Diferencijalno-geometrijsko modeliranje i dinamička Keywords simulacija diskretnih mehaničkih sustava s kinematičkim CAD vezama Constraint violation Lagrange equations Izvornoznanstveni članak Lie groups U radu je prikazano matematičko modeliranje kinematike diskretnih Multibody systems mehaničkih sustava s kinematičkim vezama pomoću Lievih grupa. Numerical integration Kinematika sustava parametarizirana je koristeći vijčane koordinate Woronetz equations zglobova kinematičkog lanca. Nastavljajući se na takav kinematički model, Lagrangeove dinamičke jednadžbe gibanja sustava izvedene su u nastavku Ključne riječi rada. Koristeći takav pristup, pokazano je kako se kinematički model, a CAD također i dinamičke jednadžbe gibanja mehaničkog sustava, mogu izvesti Diskretni mehanički sustavi s kinematičkim bez upotrebe lokalnih koordinatnih sustava vezanih za pojedina tijela vezama kinematičkog lanca. Takvo matematičko modeliranje omogućava izravnu Jednadžbe Woronetza upotrebu CAD podataka koji se, u pravilu, izražavaju u jedinstvenom Lagrangeove jednadžbe koordinantnom sustavu. U slučaju dodatnih kinematičkih ograničenja Lieve grupe narinutih na sustav, jednadžbe gibanja izvedene su projiciranjem Lagrangeovih jednadžbi na višestrukost ograničenja, čime se model Numerička integracija izražava u obliku jednadžbi Woronetza. U radu su, nadalje, prikazane Numeričke povrede kinematičkih ograničenja formulacije matematičkih modela koji se koriste kao podloga numeričkih algoritama za vremensko integriranje jednadžbi dinamike, a također je, Received (primljeno): 2009-04-30 uz izrađeni numerički primjer, opisana i metoda stabilizacije numeričkih Accepted (prihvaćeno): 2009-10-30 rješenja na višestrukosti ograničenja.

1. Introduction concepts, is the field of geometric integration [10]. The subject here is the integration of dynamic systems on Geometric methods in dynamics and control have manifolds, of which systems on Lie groups are special become widely established and valued approaches. The cases [12, 27]. actual meaning of ’geometric’ varies with the context In mechanism theory, one is concerned with the in which it is used. It generally refers to the differential geometry of finite motions, and on velocity level with geometry of the mathematical structures underlying their differential geometry. As it turns out in the case of a dynamic system. As an example, non-linear control rigid multibody body systems (MBSs), the differential- theory for continuous systems is solely built upon the geometric objects correspond to with the line geometry differential-geometry of the control system [5, 21-23]. In of the joints and bodies. The underlying reason is the other words, generally, one deals with control systems isomorphism of the algebra of screws and the algebra of the on manifolds. Another area that makes use of geometric Lie-group of rigid body motions [20, 34]. The latter is the 598 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009) group of isometric orientation preserving transformations MBS. It is assumed hereafter that Γ has one connected of the three-dimensional Euclidean space. In essence, the component. In this case Γ has γ = m − N fundamental parameterization of screws with line coordinates gives loops (FLs), where m:= |J| is the number of joints and N:= rise to geometric treatment of differential-geometric |B| − 1. Joints and bodies are respectively denoted by Jα, entities. Screw theoretic considerations also gave rise to α = 1, . . . ,m and Bk, k = 0, . . . ,N. Body B0 stands for the the development of advanced methods for the design of ground. An edge Jα∈Γ is an unordered pair Jα≡ (Bk, Bl) mechanisms [14, 25, 36]. Besides these geometric and (for brevity, we write α∈Γ or (k, l) ∈Γ if Jα ≡ (k, l)). kinematic applications, explicit use of the Lie group A spanning tree of Γ is denoted by G = (B, JG), and the structure admits to expressed motion equations of corresponding cotree by H (Γ,G) = (B, JH), or simply with mechanisms in a very compact and elegant way [4, 11, G H. Vertex B0 is chosen as the root of the tree. J is the set 17, 24, 26, 28] and also admits application of integration of tree-joints, and JH := J \ JG is the set of cut-joints. G schemes on Lie groups [25]. The difference of classical comprises N tree joints. The term cut-joint indicates that, matrix formulations and recursive formulations [1, if we ’cut’ these joints, we get a tree topology. 7-8, 32-33] is that these formulations are entirely formulated in terms of screw quantities, corresponding J to the mechanism’s joint geometry (by virtue of the 6 B parameterization with line coordinates). Furthermore, 5 J B 7 the coordinate invariance allows for a formulation 2 without body-fixed reference frames and hence for the B7 B6 J8 J9 incorporation of CAD data. This fact will be emphasized J2 throughout the paper. J5 The paper is organized as follows. Section 2 deals B with the geometry and summarizes the representation of 4 configurations of rigid MBSs. After recalling the graph J4 representation of the MBS topology, configurations B1 B are expressed as elements of the 3-dimensional special 3 Euclidean group. The configuration space is then J parameterized using canonical (axis/angle) and non- J 3 canonical parameters. A formulation is given that does not 1 need the definition of body-fixed frames. The kinematics is addressed in section 3. The closed form expression for rigid body velocities in terms of the instantaneous joint screws and the time derivatives of the parameters is given. This is a recursive relation that can also be expressed in matrix form. In section 4 the dynamics is addressed and the Lagrangian motion equations are given. Their differential-geometric meaning is briefly outlined, making use of the Riemannian metric defined by the generalized mass matrix. The motion equations are also given in matrix form. For completeness, the equations of Woronetz are given, i.e. a formulation of the motion equations of constrained MBSs, which does not involve Lagrange multipliers. In section 5 the mathematical models for numerical integration routines of MBS are surveyed. Furthermore, constraint gradient projective method for stabilization of constraint violation is addressed and a numerical example is presented. Figure 1. An example for the graph description of a MBS. Slika 1. Primjer graf-opisa diskretnog mehaničkog sustava s kinematičkim vezama 2. MBS geometry

d 2.1. The topological graph of an MBS Let G be the directed tree w.r.t. to the root B0, which is the directed subgraph of G such that to every vertex The MBS topology is represented as an oriented graph there is a path starting from the root of G (Figure 1). The Γ = (B, J), where the set of vertices B represents the directed tree induces the following ordering relation of bodies, and the set of edges J represents the joints of the joints and bodies: Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 599

• B is the direct predecessor of body B , denoted by B N l k l Cabs := {c ∈ SE (3) | Rα ∈ Gα}. (2) 1 d = Bk − 1, iff (l, k) ∈ G (for short l = k − 1) This is called the configuration space of a holonomic • Jβ is the direct predecessor of joint Jα, denoted by d d MBS in absolute representation. Jβ = Jα − 1, iff Jβ = (·, k) ∈ G Λ Jα= (k, *) ∈ G (for

short β = α − 1) Cabs is a rather abstract notion that covers MBSs with arbitrary topology. If we were to use this representation • Jβ is a predecessor of joint Jα, denoted by Jβ ≤ Jα, iff there is a finite m, such that β = α − 1 − 1 · · · − 1 (m for our actual computations, we would need to introduce times −1). constraints that restrict the relative motion of adjacent bodies to the motion spaces. In fact, this leads to absolute Note that possibly k − 1 = l − 1 for k ≠ l. E.g. in Figure coordinate formulations. What we will do instead is to 1., J = J − 1, J = J − 1, J = J − 1, so that J < J . For 1 2 2 6 6 7 1 7 eliminate the constraints by resolving for the relative each body B , there is a unique tree joint J connecting B k α k configuration of tree-joints, and introduce constraints to its predecessor, i.e. J = (·, k) ∈ Gd. This fact is denoted α only for the cut-joints. by α = J (k). In other words, body Bk is connected to Bk−1 via Jα. Finally, Jroot (k) denotes the smallest α ≤ J (k). In figure 1, J (6) = 7, J (5) = 6 and Jroot (k) = 1 for all k. 2.3. The geometry of an MBS with tree-topology An orientation of G w.r.t. Gd is an indicator function s 2.3.1. Relative configurations so that s(Jα) = 1 or s(Jα) = −1 if Jα ∈ G is respectively positively or negatively oriented w.r.t. Gd. Also s(α) is A joint Jα (l, k) ∈ Γ, connecting bodies Bl and used for short. Bk, is completely characterized by three consecutive Remark 1: Arithmetical operations on body or joint transformations (Figure 2.): indices, like l = k − 1 and β = α − 1, are to be understood • constant transformation S from the joint frame according to the above index arithmetics. In particular, k α, l (JFR) to the RFR on Bl, − 1 does not mean that we subtract 1 from the body index • constant transformation S from the JFR to the RFR α, k k, it rather refers to the body Bk − 1. on Bk,

• variable transformation Nα of these JFRs, according to the joint’s mobility, and the orientation of J ∈ Γ, 2.2. The configuration space of an MBS α that determines whether N transforms from the JFR α on B to that on B or vice versa. A body-fixed is attached to each body of the MBS k l reference frame (RFR).We assume the existence of an This information characterizes the joint kinematics inertial system, called the ground, which is referred to as without reference to a topological tree. By definition, Jα (l, k) ∈ Gd means that J connects B to its predecessor B B0. An inertial frame (IFR) is fixed at the ground, (Figure α k l = B − 1. For tree-joints, the above three transformations 2), and the configuration of body Bk is expressed as k C ∈ SE (3): must be combined with a transformation from Bk to k Bk−1 RFR. The function s(α) indicates whether Jα ∈ Γ d is positively oriented w.r.t. to G . Consequently, if Jα , with E ∈ SO (3), p ∈ 3, (1) is negatively oriented, the transformation N must be 0 1 α inverted. E.g., the meaning of a positive revolute joint E is the rotation matrix and p the position vector, angle is reversed if its orientation is opposite to the describing respectively the rotation and displacement of directed tree. Thus, the relative configuration from the the body-fixed RFR w.r.t. to the IFR. RFR on Bk to that on Bl is:

The configuration of the MBS is denoted by c ≡ (C, −1 s(α) −1 N 1 R = M Q, with M := S S , Q := S N S , (3) α . . . ,C) ∈ SE (3) . A subspace corresponds of relative α α α α, l α, k α α, k α α k N , transformations G ⊆ SE (3), to each joint called the α M transforms from JFR to BFR on B , and Q from RFR α l α motion space of Jα. This is not necessarily one of the SE ∆ (3)-subgroups [34]. E.g., for revolute joints G = SO (2) on Bk to JFR on Bl. Finally, the configuration of body α B is the group of rotations about a fixed axis. Denoted by k is given with that of its predecessor and the joint transformation R R the relative configuration of the two bodiesB and B is α α l k connected by Jα ≡ (l, k) ∈ Γ (Figure 2). A configuration C = C R, α = J (k). (4) k k-1 α of the MBS is admissible, only if Jα ∈ Gα. With there conditions, the set of admissible configurations is: Thus, the configuration of each individual body is recursively given in terms of the relative configuration of its predecessors. Therefore, we can express the MBS 1 d (k, l) ∈ G means that Bk is the source and Bl is the target of the edge. d d configuration byr ≡ (R, . . . ,R) ∈ Crel, where: Notice that (l, k) ∉ G , since G is directed. 1 N 600 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009)

S(α) α Crel := G1 × ... × GN, (5) 2) Using canonical coordinates of 1. kind, N (q ) ∧ ∧ Zqα1+...+ Zqαν is called the configuration space in relative representation. α αν = e 1 The degree of freedom (DOF) of the MBS is the dimension ∧ ∧ Xqα1+...+ Xqαν of the configuration spaceδ := dimC = Σ dimG . α α α α Q (q ) = e 1 αν , (8) Remark 2: The body-fixed RFRs are arbitrary. We α can choose other RFRs, and the configurations ¯r in with Xα i in (6), and Rα according to (3). This corresponds this representation are related to the original ones by i to the axis-and-angle parameterization of screw motions ¯r = grg−1, where g ∈ SE (3)N. These configuration [20, 34]. constitute another representation of the configuration 3) The parameterization (6) and (8) suffer from space, say C¯ . All such representations obtained by a rel singularities, when dealing with spacial rotations, change of body-fixed reference frames are equivalent. for which a global parameterization with canonical Moreover, the motion equations for the MBS are invariant coordinates does not exist, i.e. via the exponential map. w.r.t. such frame transformations. These singularities can be avoided using Euler-parameters

[20] to describe the rotation part of Nα . Another motivation 2.3.2. Parameterization of the configuration space for the use of non-canonical parameters, like Cayley- parameters, is to reduce the numerical complexity, in So far we have not used any kind of joint variables, applications where the full range of rotation is not used. which is necessary for the actual application. This requires at least a local parameterization of the motion spaces Gα. There are three options: canonical coordinates of 1. or 2. kind, and non-canonical parameterization. 1) The standard choice are canonical coordinates of 2. kind. That is, Q is expressed as series of successive α screw, i.e. 1-DOF, motions. E.g., a revolute joint allows for rotations about its axis of revolution, a prismatic for translations along its joint axis, a universal joint comprises successive rotations about two non-parallel Figure 2. Relative kinematics of two bodies B and B axes of revolution, while a spherical joint is considered l k connected by joint J as three consecutive rotations. If J has mobility2 α α Slika 2. Relativna kinematika dvaju tijela B i B povezana ν := dimG , the individual intermediate one-parametric l k α α kinematičkim zglobom J motions are given in terms of a set of (relative) generalized α

α1 αν coordinates for Jα, denoted by q , . . . ,q , and a set of screw coordinate vectors Z, . . . , Z (Figure 2), In order to keep the formulation as general as possible, α α ∧ ∧ 1 v α Zqα1 Zqαν we will write Q (q ), whenever possible, and only use the such that Ns(α) (qα) = eα1 ··· eαν. The screw coordinate α α specific form, such as (6), when necessary. α α α d vectors are expressed in the JFR either on Bl or Bk, The vector (q ) := (q 1, . . . ,q v, α ∈ G ) comprises depending on which one is the source of the oriented the generalized coordinates qa of the MBS with tree- d edge Jα ∈ G . With (3), topology. The parameter manifold of an MBS with nR ∧ α1 ∧ Xq Xqαν revolute and nP prismatic/screw-joints is: Q (qα) = eα1 ... eαν, where X := Ad-1 Z, i=1,...,v, (6) α S α α i α, k i (9) and the adjoined operator matrix [20], so that the relative transformation by the tree joint is: is also referred to as the configuration space of the a ∧ ∧ MBS, and q ≡ (q ) ∈ as its representing point. Xqα1 Xqαν α α α R (q ) = Me 1 ...e ν , (7) α α 2.3.3. Relative configurations without body-fixed joint Z are the coordinate vectors of the joint screws of J , frames αi α w.r.t. JFR on B , while X are the crew coordinate vectors k αi From a practitioner’s point of view, the definition of w.r.t. to the RFR on Bk. body-fixed joint frames is an irksome task additional to the design process. This task is dispensable, however, as the joint kinematics can be expressed without body 2 For compactness, we omit the joint index and write ν, if the joint is fixed joint frames. Such formulations are advantageous clear from the context. Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 601 when processing CAD data, which refer to a single world Since each FL comprises exactly one cut-joint, we frame (IFR). The CAD construction serves as reference denote with Λα the FL containing cut-joint Jα. Body Bk configuration to which we assignq = 0. is the root body of the FL iff Bk < Bl ∀l ∈ Λα. E.g. in Denote with m := C (0) the reference configuration Figure 1. the root body of Λ8 is B2 and that of Λ3 is B1. k k Denoted by Hd the directed cotree such that all J ∈ H are of Bk, then α positively oriented. Using bodyfixed reference frames, m = M ... M M, α = J (k), β = J (k) the geometric closure condition for Λ with cut-joint J k β α-1 α root α α ≡ (l, k) ∈ Hd is

αY = Adm X.α (10) i k i S-1 C-1 C S = N. (12) α,1 l k α,k α

The Y i are the screw coordinate vectors for joint Jα αi expressed in the IFR, in the reference configuration m. The corresponding closure constraints are the k independent components of (12), for which N is With this data, the body configurations are α identically zero whenever R ∈ Gα. These components are ∧ ∧ ∧ ∧ α Yq β1 Yq βν Yq α1 Yq αν listed in Table 1. for some technical joints. It is indicated C (q) = eβ1 ... eβv ... eα1 ... eαv m, k k (11) whether the motion space of the joint corresponds to a SE α = J (k), β = J (k). (3) -subgroup. A cut-joint with mobility να yields 6– να root (locally) independent constraint equations. Remark 3: The advantage of this formulation in Remark 4: The distinction between local and global conjunction with CAD-data shall be stressed. CAD- constraint maps should be stressed. The revolute joint is an systems use the IFR to express the geometry and the instructive example for which the geometric constraints inertia properties of the mechanism. Consequently, in the on the relative rotation are: E 3= E 3 = 0. These plus reference configuration the RFR of all bodies coincide 1 2 the three position constraints, only give in fact rise to a with the IFR, so that m = I . All that is needed are the k local constraint map. This is so because the conditions screw coordinate vectors Y w.r.t. the IFR, in the reference on E enforce the e -axes of the coupled JFRs to remain αi 3 configuration. The vector Y ≡ (ω, p × ω + hω) is merely parallel, but they may have opposite directions, for which α i the constraints are also fulfilled. given in terms of the direction vector ω along the joint axis, the position vector p of some point on the axis, and With parameterization of the tree-joint, the cut- the pitch h of the joint. Since the moments of inertia are joint constraints yield constraints on the generalized also expressed in this IFR the definition of body-fixed coordinates. The overall system of m := 6γ – ∑α∈Hνα RFRs is dispensable (section 4.). geometric constraints is summarized in: g (q) = 0. (13) 2.4. The geometry of MBS with kinematical loops The admissible parameter space, defined by (13) is An edge corresponds to any loop of the cotree H the configuration space:

(which is empty for treestructures). As we have seen, the configuration of an MBS can be represented in absolute V := {q ∈ | g (q) =0}, (14) formulation (2), listing the configurations of all individual of the MBS with kinematic loops. V is an analytic variety bodies plus the constraints due to the joint couplings. comprising smooth manifolds, separated by singular Alternatively, configurations can be represented in points [18-19, 30-31]. relative formulations ( 5), where for MBS with kinematic loops, the configuration must comply with loop closure constraints arising from cotree joints. 3. MBS Kinematics Hence, an MBS with kinematic loops can be modeled 3.1. The kinematics of an MBS with tree-topology as an MBS with tree topology subject to holonomic constraints. That means that the cut-joints J , α ∈ H are The velocity of body B in body representation is α ∧ k removed, and the resulting tree-system is subject to the -1 defined as V := C Ċ. V is the twist of the RFR on Bk corresponding closure conditions. k k k k w.r.t. to the IFR expressed in this RFR. It is verified with Definition 1: Denote with να = dim Gα the mobility (7), 6–ν of joint J . A smooth map g : SE (3) → α is called . α α V(q) = ∑ K (q) qa, (15) a local constraint map iff g (R) = 0 ↔ R ∈ U ∩ G , for k a α α α α k a≤n some U ⊂ SE (3) containing the identity. If this holds on with K := Ad -1 H , if m ≤ k, for α := J(m), (16) k αi C C αi all SE (3), g is called a global constraint map. k m α HQ=∂−1 α Q αi i . (17) αα 602 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009)

Table 1. Cut joint constraints for some technical joints. E is K is called the Kinematic Basic Functions (KBF) the rotation matrix and p is the position vector describing the k of relative configuration (1) of the two bodies connected by the Bk. From its definition (16) there follows immediately the cut joint. recursion Tablica 1. Kinematička ograničenja za odabrane kinematičke zglobove. Rotacijskom matricom E i vektorom položaja p (18) k definirana je relativna kinematika (1) dvaju tijela koja su Ad . α k-1 povezana određenim kinematičkim ograničenjem.

DOF Motion (Joint type) Constraints / The expression for Hα depends on the choice of local Subgroup / i / / Relativno gibanje Kinematička coordinates on G (2.3.2.). Subgrupa α SSG (vrsta zgloba) ograničenja 1.) For canonical coordinates of 2 kind (6): Translation along ∧ α ∧ Xq 1 Xqαν fixed axis (prismatic 2 3 3 E1 = E2 = E1 = 0 (19) α joint) / Translacija 1 2 × i duz nepomične osi p = p = 0 1 (translacijski zglob) 2.) For canonical coordinates of 1 kind (8), using the Rotation about left and right dexp-map on se (3), fixed axis (revolute E 2 = E 3 = 0 joint) / Rotacija 1 2 × p1 = p2 = p3 = 0 oko nepomične osi α α α i (20) (rotacijski zglob) α, k

2 3 3 α α αi Planar translation / E1 = E2 = E1 = 0 × α α Planarna translacija p3 = 0 with Y = Xq 1+ ... + Xq v. α1 αv Rotation + 3.) Using non-canonical coordinates, H is of a αi translation specific form. e.g., when parameterizing the rotation along same axis (cylindrical joint) / E 3 = E 3 = 0 matrix E in (7) with Cayley- or Euler-parameters. 1 2 × Rotacija i translacija p1 = p2 = 0 For the standard choice of canonical coordinates of -1 oko iste nepomične 2. kind, using A in (19), K = Ad -1 Ad X, are the k αi C C A α osi (cilindričan k m i 2 zglob) instantaneous joint screws of Jα expressed in the RFR of Two successive B . This is why K is occasionally called the geometric k k rotations about Jacobian. orthogonal axes 2 With the MBS configurations c ∈ Cabs, we define the (universal joint) / E1 = 0 ∧ ∧ ∧ - -1 . Dvije slijedne p1 = p2 = p3 = 0 velocity of an MBS as V := c c ≡ (V, ..., V) ∈ TICabs. rotacije oko What is more important for the following is the state . ortogonalnih osi space T , and the generalized velocities q ∈ T , that (univerzalni zglob) q are related to V via (15). Spatial translation / E 2 = E 3 = E 3 = 0 × Prostorna translacija 1 2 1 Planar motion 3.2. Kinematics without body-fixed joint frames (planar joint) / E 3 = E 3 = 0 3 1 2 × Planarno gibanje 3 The KBF can also be expressed in terms of the Y in p = 0 αi (planarni zglob) (10) as: Spatial rotation (spherical joint) / α , (21) p1 = p2 = p3 = 0 × k k i Prostorna rotacija (sferni zglob) ∧ ∧ ∧ ∧ β1 β Yq α1 Yq αν Yq Yq ν α α β β Planar motion + with A = e 1 ... e v ... e 1 ... e v, β = J (k). spatial translation (joint macro) / This is again an expression, without reference to 4 E 3 = E 3 = 0 × Planarno gibanje + 1 2 body-fixed JFRs. prostorna translacija (složeni zglob) 3.3. Matrix formulation for tree-topology MBS Spatial motion 6 - × Prostorno gibanje The recursion (18), allows for a matrix expression of V in terms of the generalized velocities. Consider a

terminal body Bk of the topological tree, and the (unique) Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 603

path from Bk to B0. Let us, for the moment, assume that the For our standard choice of canonical coordinates of bodies and joints of this path are numbered increasingly 2nd kind, (26) simplifies to with 1, 2, . . . , k. Combining the corresponding KBFs to ∂ βjK = [K K ], α ≤ β ≤ J(k), i

Computationally, (27) is very easy to evaluate,∧ ∧ since , (22) in vector representation the Lie bracket of two X1, X2 ∈ se (3) is [X1, X2] = (ω1 × ω2, ω1 × v2 + v1 × ω2). We thus arrive at a very compact expression for the body acceleration of

k Bk · ··b ·b ·c k then Kk = AkHk, with Hk:= diag (H1 ,..., Hk ) and V =K q + [K , K ]q q, b ≤ c ≤ α , α = J (k). (28) i i k b k b k c ν

. . We use the summation convention over repeated b b 2 1 indices: Aabx ≡ Σb Aabx . As the individual KBFs can . . . . . (23) be combined to K, so can their derivatives, to give the a differential dK(q, x) := ∂ K(q)x . Using [K K ] = Ad –1 a αi, βj C C k–1 1 k k k m [K ,H ] = −Ad ad K , β = J(m), and the matrix αi βj C–1C H αi . m βi m k 1 k 2 k k–1 k m

∧ The H ≡ (H , ··· ,H ) are 6× v matrices according α α1 αv α ∧ ∧ (29) to the mobility of Jα , and I and 0 is respectively the 6×6 , identity and zero matrix. With our primary choice of H such that ad Y := [X, X], then ( 26) yields coordinates, the α1 ≡ Xα1 are constant. X The items of A and H are obvious, in terms of k k dK (q) (x) = A (q) [dH (q, x) − a (q, x)K (q)] (30) arbitrary body indices. In summary, combining in K the k k k k k

Kk matrices of all terminal bodies, the MBS velocity is:

V = Kq· (24) . . . The structure of K reflects the tree-topology of the .. (31) MBS. It is interesting to notice that the lower block . −1 . triangular matrix can be expressed as Ak = (I − Dk) =

I + Dk + + . . . + , with help of the nil-potent matrix ,

. 1 2 1 . i . . .. (25) . . . . . d .. (32) . . , k k–1 k that only comprises the relative configurationsC −1C. This i i i–1 . is a matrix version of the recursion (18). With canonical coordinates 2. kind, H = diag (X, . . ., k 1 X) is constant, and dK (q) (x) = −A (q) a (q, x)K (q). If k k k k k 3.4. Partial derivatives of the KBFs x is a sufficiently small perturbation,

Due to the recursive formulation in terms of relative K (q + x) = K (q) + dK (q, x) = (I − A a )K (q). (33) configurations, it holds: k k k k k k

∂qβjKα = AdC–1C∂qβjHα , if α=β k i k m i (26) + [K K ], if α<β, for β ≤ J(k), α = J(m). k αi, k βj 604 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009)

3.5. Kinematics of an MBS with kinematical loops Table 2. Selector Pα for the constrained velocity components

of a cut joint Jα of which the motion space corresponds to a The kinematic constraints, i.e. the closure conditions subgroup of SE(3) on velocity level, are formulated under the condition that Tablica 2. Selektor Pα za komponentu kinematički ograničene g (q) = 0. Consider the relative velocity of Bk and Bl, brzine zgloba Jα čiji prostor gibanja odgovara subgrupi SE(3) connected by cut-joint Jα≡(l, k), Motion (Joint type) / α · DOF / Selector P / Relativno gibanje α α, k k l α, k k l SSG Selektor P k l k l (34) (vrsta zgloba) · · · ∧ ∧ ∧ . α αv–1 α 1 00000  eα2 ... eαv 1 eαv v Translation along fixed axis 0 1 0000  The last ν terms are the twist vectors of the successive (prismatic joint) / Translacija   00 1 000  screw motions that describe the relative joint motion, duž nepomične osi expressed in the JFR on B . If the motion space of J is 000 1 00  k α (translacijski zglob)   a SE (3)-subgroup these twists belong to the respective 0000 1 0 subalgebra. 1 The kinematic closure condition is the disappearance 1 00000  of the components of the relative velocity, not in this Rotation about fixed axis 0 1 0000  subalgebra. That is, we need to eliminate the twist (revolute joint) / Rotacija   000 1 00  components in (34) that correspond to the joint motion, oko nepomične osi (rotacijski 0000 1 0 which is unconstrained. To formalize this ellimination, zglob)   introduce a selector Pα that restricts the complementary3 00000 1 se (3)-subspace of the να -dimensional space of cut-joint twists, expressed in the JFR on Bk. Application to (34) 1 00000  yields a sufficient set of kinematic constraints for the cut-   Planar translation / Planarna 0 1 0000 joint Jα   translacija 00 1 000  Pα Lα q=· 0, (35) 0000 01  where Lα ≡ ( ) is the 6 × N matrix with columns 2

:= (36) Rotation + translation along 1 00000  k l . α, k k l same axis (cylindrical joint) / 0 1 0000  Rotacija i translacija oko iste   This is the standard formulation if the cut-joint’s nepomične osi (cilindričan 000 1 00  motion space is a subgroup. In this case, the selector also zglob) 0000 10  determines the relevant components of the geometric constraints (12). The definition of the joint axes 1 00000  corresponds to the se (3) standard basis, e.g., the e -axis 3 Spatial translation / Prostorna 0 1 0000  of the JFR is used as joint axis of pris-matic/revolute/ translacija   00 1 000  cylindrical joints and for a planar joint the e1, e2 -axes are used as principle axes of translations and e3 as axis of revolution. Table 2. lists the according selectors for all 3 00 1 000 Planar motion (planar joint)   subgroups. / Planarno gibanje (planarni 000 10 0   Combining the constraints (34) for all γ loops yields zglob) 0000 10  the system of m kinematic constraints · hq= 0. (37) 000 1 00 Spatial rotation (spherical   The time derivative of (35) yields the dynamic joint) / Prostorna rotacija 0000 1 0 constraints for Λ , i.e. the constraints on acceleration   α (sferni zglob) level 00000 1 ·· · (38) hq+ hq = 0. Planar motion + spatial The recursions (27), (26) provide a closed form for translation (joint macro) / 000 10 0 · ·a 4 Planarno gibanje + prostorna h = ∂ ahq .   q translacija 0000 10  (složeni zglob) 3 Notice that this has nothing to do with an orthogonal complement since this would require a metric on SE (3). Furthermore, the particular complement space depends on the specific JFR used to express joint twists. Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 605

Remark 5: A problem often encountered, is the · · · dependence of loop constraints, i.e. h permanently a rank form g , with the symmetric less then m. The reason is that for a certain cut-joint a certain number of constraints are formulated, regardless g := (43) of whether or not the relative motion (to be constrained k k k by the closure condition) can actually take place if the cut-joint is removed. The obvious problem for the Besides singularities of our parameterization of the formulation of loop constraints, (in particular when configuration space, gab is positive definite. Hence gab automatically generating constraints) is the detection/ defines a metric on the tangent spaceT q in every point removal of redundant constraints. Fortunately, one can q, which makes a Riemannian space. cope with quite a large class of problems, by noting the In the Lagrangian framework, the dynamics of a geometric background [16]. holonomic MBS are governed by the Lagrangian motion equations (LME) of second kind 4. MBS Dynamics (44) · , 4.1. The Lagrangian motion equations of a MBS with tree structure where the Lagrangian Λ(q, q· ) := T* (q, q· ) − U (q) is Define a frame with origin at the center of mass given in terms of the kinetic coenergy and the potential — (COM) of B . Let M∈SE (3) be the transformation from energy U. are generalized forces not covered by the k k Lagrangian, such as dissipation or control forces. COM-frame of Bk to its RFR. The velocity of the COM- frame is The LME for a skleronomic holonomic MBS with — tree topology are [13]: − . (39) k k k ··b · b ·c (45) ggab (q) q + Γabc (q) q q = Qa , With the mass m and the inertia tensor Θ = (Θ ) of B ij k with := and the Christoffel symbols of the k k w.r.t. the COM-frame, define the inertia matrix ofB k first kind:

k (40) g g g . (46) k . In view of (27) they can be expressed as: The kinetic coenergy of Bk is then — — — * — — — k k k k k , (41) k k k k k k k k k k k k k (47) a< b ≤ c or b ≤ a < c, r = max (a,b), s = min (a,b). where: Relation (47) can be simplified using Binet’s inertia — tensor: := − − (42) k k , k k ~ — (48) is the inertia matrix J of B w.r.t. its RFR. k k k k k k ~ Remark 6: We shall comment on the use of CAD- where := − − and Ω(X) = ω gives data here. In section 2.3.3 we abandoned the use of body- the angular part of the twist X = (ω, ν). fixed RFRs and introduced the formulation (11). We In matrix notation, the LME are expressed as: have further seen that the reference configurations are m k G(q) q··+ C (q, q· ) q· = Q, (49) = I since CAD-systems relate to all quantities to a world frame, which in the reference frame coincides with the where RFR. An analogous argument holds for the inertia data, · · c — G(q) := ( g ) , C (q, q) := (Γ q ) , Q := (Q ), (50) so that CAD-system already determine J . In summary, ab abc a k local RFRs are completely dispensable. G is the generalized mass matrix, Cq· represents * * The total kinetic coenergy of the MBS is T := ΣkTk generalize Coriolis and centrifugal forces, and Q covers In terms of generalized velocities, this is the quadratic all potential forces and remaining forces not represented by the Lagrangian Λ With (24), and (30) follows: 606 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009)

~c ~ a T n,n 1 1 −1 where (f a1) := (f c1) , and the generalized forces Q G(q) = K JK ∈ (51) a′2:= a a′2 . The equations (55) are the equations of Woronetz. F Qa C (q, q·) = KT (bT J − JAa)K, (52) Assuming regularity of Fa, (56) defines a metric on V. − − where J:= diag(J−, . . . , J), and b only depending on the The Woronetz equations can be written in matrix 1 k form: body twists V = K q·: k k – – – – 6N,6N G(q) q2 + C (q, q) q2 + N (q) = Q, (58) b (V):= diag(adV, . . . , adV) ∈ (53) 1 N The first term in the brackets of (52) represents where gyroscopic forces, due to the fact that q· TKT bT JKq· = VT – – · – – G:=FTGF, C:=FT (CF + GF ), N:=FTN, Q:=FTQ. (59) bT JV ≡ 0, since b (V ) V ≡ 0. Hence, one can separate a gyroscopic potential from the Lagrangian Λ, which The system (55) together with the kinematic a a contributes to gyroscopic forces. constraints f 1 a (q)· q = 0 yield n differential equations in q ∈ , which completely describe the MBS dynamics. The constraints are holonomic if and only if the 4.2. The Lagrangian motion equations of a constrained b integrability conditions A 1 ≡ 0 are satisfied, where: MBS

b1 b c c c A := b' c' 1 1 (60) b' c' F 2 F 2 (∂ b f − ∂ c f ) Using relative coordinates, an MBS with kinematic 2 2 q q loops is a tree-topology system subject to a system of is called the object of non-holonomy. r holonomic constraints (13). More generally, an MBS is subject to further geometric and/or non-holonomic velocity constraints. Denote the overall system of m k a1 · a geometric constraints by f (q) = 0, and with f a (q) q 5. Numerical integration of MBS Dynamics

= 0 the overall system of r = |{a1}| ≥ m (linear) velocity constraints. The geometric constraints define the 5.1. ODE integration configuration space of the constrained MBSV := {q ∈ k k As explained, the LME (45) yield an ODE system |f (q) = 0}. For simplicity we presume rank (∂qa f ) = m a1 and rank (f a ) = r on all V . Then V is a n − m-dimensional on the state space T , where n ODE is assumed. smooth manifold. T is a tangential bundle that comprises configuration

b1 b1 b1 a a a There exists a partitioning (f ) = f 1| f 2) with rank manifold and union of all tangential spaces Tq . It b a a 1 a 1 (f 1 ) = r, and thus an orthogonal complement Fa2 of f a is a 2n-dimensional manifold covered by the coordinates a1 a a a a · · · with rank (F a2)= n − r, i.e. f F 2= 0. Hence, the system q, q: T = {(q, q) : q ∈ , q ∈ Tq }. By adopting the of kinematic constraints has a solution: generalized mass matrix G(q) as a Riemannian metric a on the configuration manifold, the tangential space T · a a' · a′2 q q = F 2 (q) q , q ∈ V. (54) (“the fiber of the tangential bundle at point q”) becomes (54) is a parametrization of the n−r-dimensional a locally Euclidean vector space spanned by a covariant vector space defined by the velocity constraints -a basis ĝ . The LME can be writen in the shorter matrix qi subspace of the n−m-dimensional tangential space TqV. form as: I.e. the vector space of admissible velocities may have G(q)q·· = Q*(q, q,· t), (61) a lower dimension than the base manifold V. We use and following the standard procedure, it can be turned primed indices to indicate reference to this subspace. The into the 2n ODE form: solution (54) is not unique and a different choice of the partitioning yields a different chart. · –1 · – q v = G (q) Q* (q, v, t), q = v, q =   . v The motion equations for the constrained MBS in · – – v  ·a′ · a terms of q 2 follow by substitution of q , using (54), xf==()xf,( x)  . GQ−1 * [13]:   (62) The solution of (62) is the integral curve of the vector b′ b′ · c′ g ··q 2 q· 2q 2 = Q (55) – a'2b'2 + Λa’2b’2c’2 a’2, field f ( x ) on tangential bundle (“velocity phase space”) for a set of Cauchy data (t , –x ). a b 0 0 with the metric g := g F a' 2 F a' , (56) a’2b’2 ab 2 the generalized Christoffel symbols of first kind: 5.2. DAE integration

~c a a b c 1 1 a' c' Λ := (Γ − g f a1∂ c f b)F 2 F b'2 F 2, (57) a’2b’2c’2 abc ac1 q If additional holonomic/nonholonomic constraints are imposed on the system, instead of reducing the system to the constrained LME form as explained in chapter 4.2, Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 607 one can model the system using a mathematical model 5.3. Stabilization of constraint violation in descriptor form. This is straightforward modelling procedure that can be easilly applied for a wide class If the governing equations of the system are based of MBS (thus being a basis for many general MBD on a mathematical model in descriptor form, leading to packages), but it leads to DAE integration (numerical time integration of redundant coordinates, a constraint integration of system of differential-algebraic equations) violation stabilization method has to be applied during that is generally more formidable differential-algebraic the integration procedure. Alternatively, a well- than standard ODE integration routine. known algorithm described in [39] can be used. It is based on a coordinate partitioning procedure prior to To avoid high-index DAE formulations, the integration, followed by the time integration of the mathematical model for dynamic simulation of multibody selected independent coordinates only. Another pojective systems with imposed holonomic constraints Φ (q, t) = 0 stabilization procedure is described in [3]. * · k and Φq (q, t) x = 0, where Φ(q, t) := (f (q, t)) and * k The stabilization algorithm proposed in [38] is based Φq := (∂qa f ), is often shaped as a differential-algebraic system (DAE) of index 1 (redundant coordinates on projection of the steppost-integration results (system formulation, ideal constraints’ reaction forces are current state point) to the constraint manifold in the assumed) course of simulation. An advantage of the procedure is straightforward integration of dynamic equations, T  G Φ*      while stabilization algorithm is to be applied only after q (63)   *       intolerable constraint violation is detected. The procedure Φ 0    q        , can be briefly described as follows. where the Lagrangian equations of the first type: If constraint violation (specified by given numerical ·· T tolerances) occurs after step-integration phase, the G (q) q + Φ* (q, t) λ = Q* (q, q, t) (64) “position” stabilization step is to be performed by T n * m×n * dep q ∈ , Φq ∈ , rank [Φq ]= m, correcting dependent coordinates sub-vector q via and the constraint equations at the acceleration level solving system constraint equation, providing thus shifting of the system configuration-point q back to Φ * (q, t) q·· = ξ, (65) q constraint manifold n−m. The procedure is then repeated are put together. System (63) is uniquely solvable for the . . at the velocity level by correcting qdep to bring q in set of consistent initial values. It can be integrated in time accordance with (66). This means that, as stabilization to obtain the motion of the system as well as the constraint . step final result, time-integration valuesq, q are projected reactions. Although constraints at the acceleration level will be immanently satisfied, since (65) is included in to the constraint manifold tangential bundle T , thus mathematical model (63) and will be explicitly solved completely satisfying constraints of the system. As will be during integration, the numerical non-stability of (65) seen later, a crucial point of the algorithm is appropriate dep . dep can induce constraints violation at both the position and selection of sub-vectors q and q to provide the velocity level. optimal stabilization effect. As explained, holonomic constraints restrict system Criteria for coordinates selection can be expressed configuration space and impose constraints at velocity geometrically: basically, every selection that returns sub- level: a trajectory T:qi = qi (t) is “forced to move” on the vector of dependent coordinates qdep whose basis vectors n − m dimensional constraint manifold n−m(t) = {q ∈ n−m, have non-zero projections on the constraint subspace n−m (corresponding m×m sub-matrix of constraint matrix Φ(q, t) = 0}, t ≥ 0, q(t ) ∈ (t ) and linear constraint 0 0 Φ * is non-singular) is a correct one and can be used equation (66) is imposed at system velocities: q for stabilization procedure. Consequently, basis vectors . * ind Φq (q, t) x = – Φt. (66) of independent coordinates q must have projections Although analytical solution curves for consistent on tangential space of constraint manifold T n−m that n−m q initial conditions will move on a tangential bundle T complement . If the extracted sub-vectors do not n−m (having satisfied all constraints imposed on the satisfy specified conditions, selection is not a valid one system), a numerical solution will tend to drift away from and calculation will fail. constraint manifold. If, beside holonomic constrains, additional non- 5.3.1. Numerical errors along configuration manifold holonomic constraints given in linear (Pfaffian) form are imposed on the system (further reducing dimensionality The main problem that may occur during stabilization of admissible velocities by forming subspace at Tq ), a procedure is an inadequate coordinate selection that may mathematical model similar to (63) can be introduced have a negative effect on integration accuracy along the [38], allowing the same integration procedure as for the constraint manifold. Although, as was explained, every holonomic systems. 608 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009) partitioning that returns acceptable sub-vectors can be coordinate curves” to the constraint manifold. This can used for stabilization, a non-optimal choice of coordinate be accomplished by utilizing criterion for optimized sub-vectors may cause an increase of numerical errors partitioning that allows for determination of those along the manifold during stabilization part of the coordinates whose direction vectors ĝ are directed most qi integration procedure. If this happens, a correction of orthogonally toward constraint manifold. By selecting constraint violation will be accomplished at the expense these coordinates as dependent ones during projection of “kinetic motion” accuracy that comes as a result of procedure, the constraint equation would be solved along · ODE integrators of system variables q, q. the selected “orthogonally directed” curves , providing The “mechanism” of emerging of numerical errors thus optimized projection and “controlled” numerical along configuration manifold, because of an inadequate error along the manifold. The basic idea of the criterion partitioning during the stabilization procedure is outlined is to determine those m coordinates whose direction in Figure 3. For illustrative simplicity, holonomic (as vectors ĝq deliver the biggest relative projections to the well as scleronomic) example q = x ∈ ≡ 2, rank i (i.e. “small” value of αp, Figure 3) and select them * n−m [Φx ] = 1, = is discussed. Fundamentally, nothing as dependent variables which will be adjusted during the is changed if more general case is analysed. stabilization procedure. By correcting coordinates whose Assuming that, starting from , an integration of ODE direction vectors align well with the constraint gradients, gives result instead of exact position , a projection it is ensured that correction procedure will shift a state- on the constraint manifold by adjusting coordinate x1 point of the system “as direct as possible using coordinate (solving constraint equation along x1 curve) yields result curves” to constraint “hyper-surfaces”, minimizing thus which is consistent to the constraint. If instead of x1 , an error along constraint manifold. Alternatively, the the variable x2 was chosen to be a dependent coordinate, coordinates that have “good” projections on the local 2 n−m an adjustment of the integration result along x curve tangential space Tq can be extracted and treated as would yield solution , which is also consistent which independent coordinates [37]. the constraint but contains considerable error along the The main “measure” for expressing the criterion manifold . A remedy for the problem of an inadequate is relative length of the projection of the coordinates selection of dependent coordinates has been described direction vectors ĝq to the constraint subspace i n−m in [2], where a projective criterion to the coordinate (or tangent subspace Tq ). The relative lengths of partitioning method is discussed. constraint subspace projection and tangent subspace projection are given by (67) and (68): , (67)

, (68)

where the squared length of the projections and (length of the projections of ĝ on the subspace and qi n−m subspace Tq respectively) and is the squared length of ĝ in the natural basis G. Those coordinates qi which have the biggest ΔC should be chosen as dependent

coordinates and those with the biggest ΔT are pertinent for independent variables. Of course, squared length indicated in denominator of the equations (67) and (68) Figure 3. Numerical correction of the constraint violation at should be calculated with respect to the system manifold the configuration (“generalized position”) level Riemannian metric G, while expressions in nominator Slika 3. Numerička korekcija povreda kinematičkih must be calculated via sub-metrices associated with the ograničenja na nivou poopćenih položaja constraint subspace and its orthogonal counterpart, as explained in [2, 39]). 5.3.2. Criterion for optimized coordinates partitioning

In the framework of constraint violation stabilization 5.4. Numerical example algorithm proposed in [38], the main idea of “controlling” 5.4.1. Mechanical system with non-holonomic constraint the numerical error that is eventually introduced during stabilization projection step is to “minimize” In the framework of the first numerical example that the artificial numerical shift along the manifold by presents numerical forward dynamics of the satellite projecting integration point “as direct as possible using motion while its panel system develops, the effect of Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 609 the described constraint stabilization procedure will be illustrated without details on mathematical modeling of the satellite dynamics (for more details, refer to [38]).

Figure 5. Total angular momentum error versus time Slika 5. Numerička greška kinetičkog momenta oko središta masa sustava

Figure 4. Satellite multibody model 5.4.2. Mechanical system with mixed holonomic and non- Slika 4. Mehanički model satelita holonomic constraints The second numerical example focuses on the The considered satellite is based on the INTELSAT V numerical forward dynamics of a snakeboard. The satellite [35]; it consists of a main body and two panels mathematical model of the snakeboard is synthesized of four parts (Figure 4). A planar model is used, and no using descriptor formulation, resulting in a mix of gravitational force is incorporated in the simulation. holonomic and nonholonomic constraints. To investigate Since angular momentum of the system is conserved, the characteristics of optimized generalized coordinates additional constraint equation that express conservation partitioning procedure when holonomic and non- of the angular momentum is imposed on the system holonomic constraints are present in the system, the during part of the analysis (it can be treated as non- separate partitioning at position level and velocity level holonomic constraint as it involves kinematic constraint is performed and analyzed. It is shown that optimal at the velocity level which cannot be directly transposed selection of independent coordinates is not necessarily to the generalized positions level). the same as the best set of independent velocities. The equations of motion of the satellite were obtained The snakeboard is illustrated in Figure 6. It is modelled symbolically starting from (45) using Mathematica as a planar multibody system with 4 bodies connected to programming package. Forward dynamics of the satellite each other by means of pin-joints. There is one coupler was performed for a duration time of 10 s with a variable (B), two small boards with wheels (A and C) and one step Runge-Kutta method of mixed order (4th and 5th) rotor (D) on the coupler to model dynamical excitation using an absolute tolerance of 1e-18. The purpose of this of human body torso motion. The two pairs of wheels simulation is to obtain a reference trajectory which can cannot slide and therefore impose two non-holonomic be treated as the “exact” solution. The maximum error constraints on the system. At the configuration level, the on the total energy, which should be constant, is 1,1e-9 J snakeboard has 6 DOF. and the maximum error on the total angular momentum 2 The snakeboard is mathematically modelled in is 1,6e − 9 kg·m /s. Note that no constraint stabilization descriptor form, i.e. at the configuration level each was performed in the process of obtaining reference component has 3 degrees of freedom and holonomic trajectory. After obtaining reference solution, dynamic constraints are added to model the joints. Therefore, the simulation was performed using a Euler first order mathematical model is synthesised using 12 coordinates numerical integration scheme and a fixed timestep of 0,01 and 6 holonomic constraints, supplemented by additional s. As Figure 5 shows, the total angular momentum of the 2 non-holonomic constraints. satellite (about the center of mass) is not constant, and the The chosen coordinates are (x , y , θ ) representing the error grows steadily. These errors were eliminated during i i i subsequent stabilized integration that utilized described position of the center of mass and the global orientation projection algorithm by considering system constraint of body i = A, B, C or D. manifold defined via additional nonholonomic constraint During performing forward dynamic analysis, the equation of angular momentum conservation (Figure 5). following scenario was considered. The snakeboard is actuated by a force with constant amplitude acting on 610 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009) the system centre of mass and with varying orientations parallel to the coupler, towards the front wheels. The wheels are forced into a sinusoidal motion of 1 Hz and amplitude of 0,5 rad by means of PD-controllers (P = 10 N/m, D = 0,1 N·s/m). The forward dynamics was first performed using Runge-Kutta method of mixed order (4th and 5th) using an absolute tolerance of 1e-18 to obtain a reference simulation presented in Figure 7.

Figure 7. System reference generalized coordinates Slika 7. Referentne poopćene koordinate položaja sustava

For the analyzed motion, the relative projections of the configuration coordinates and velocity coordinates on the respective constraint manifolds were calculated Figure 6. Snakeboard multibody model and presented in Figure 8. (configuration coordinates Slika 6. Mehanički model ‘snakeboard-a’ projections are presented left and velocities are right).

tend . .

t=0

Figure 8. Relative length of projection for configuration coordinates and velocity coordinates Slika 8. Relativna duljina projekcija baznih vektora na višestrukost ograničenja Strojarstvo 51 (6) 597-612 (2009) A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... 611

As one can see, there are clear differences between the References lengths of relative projections at both levels. Since the coordinate which has smaller projection at the respective [1] Anderson, K.: An Order n Formulation for the Motion constraint manifold would be a more appropriate choice Simulation of General Multi-Rigid-Body Constrained for dependent integration parameter than one with Systems, Computers & Structures, 43, (1992) 3, 565-579. longer projection, it is clear that optimal choices for [2] Blajer, W.; Schiehlen, W. and Schirm, W.: dependent position coordinates would differ from those A Projective Criterion to the Coordinate Partitioning for velocities. Method for Multibody Dynamics, Archive of Applied Mechanics, 64, (1994), 86-98. In the sequel, the performance of constraint violation stabilization procedure based on optimal coordinates [3] Blajer, W.: Elimination of Constraint Violation and Accuracy Aspects in Numerical Simulation of Multibody partitioning, is discussed in the framework of the Systems, Multibody System Dynamics, 7, (2002), 265- presented example. For this purpose, only constraint 284. stabilization at velocity level is considered. In order to [4] Brockett, R. W.; Stokes, A.; Park, F. C.: determine and compare local integration/stabilization Geometrical Formulation of the Dynamical Equations velocity errors, the integration procedure was performed Describing Kinematic Chains, IEEE Int. Conf. on as follows. The state vector of the reference trajectory Robotics and Automation, (1993), 637-642. has been integrated using fixed step Euler numerical [5] Bullo, F.; Lewis, A. D.: Geometric Control of integrator at any given time for one single step (0.01 s) Mechanical Systems, Springer, 2005. with and without projective stabilization step and the [6] Dai, J. S.: An historical review of the theoretical difference between the stabilized and unstable solution development of rigid body displacements from Rodrigues has been calculated. This procedure was followed twice, parameters to the finite twist, Mech. Mach, Theory, 41, with a different choice of dependent coordinates in the (2006), 41-52. case of stabilized solution: once with optimal stabilization [7] Featherstone, R.: The Calculation of Robot (as shown in Figure 8) and once with a suboptimal set Dynamics Using Articulates-Body Inertias, Int. J. Robotics of dependent coordinates (the coordinate with smallest Research, 2, (1983) 1, 13 - 30. projection is replaced with the 7th smallest one). [8] Fijany, A.; Sharf, I.; D’Eleuterio, G.: Parallel The errors ε were calculated by integrating the O(log N) Algorithms for Computation of Manipulator weighted squared difference (using system metric M) in Forward Dynamics, IEEE Trans. Rob. and Automation, velocity between both sets of trajectories: 11, (1995) 3, 389-399. tend [9] Funda, J.; Taylor, R. H.; Paul, R. P.: On homogenous . ε = ∫ (ΔqTMΔq. )dt. transformations, Quaternions, and Computational t=0 efficiency, IEEE Trans. on Robotics and Automation, 6, The obtained result shows that error between the June (1990) 3, 382–388. unstabilized fixed step solution and the optimal solution [10] Hairer, E.; Lubich, C.; Wanner G.: Geometric is 0,00003e-5, while the difference between unstabilized Numerical Integration, Springer, 2002. and the suboptimal solution is significantly larger and [11] Herve, J. M.: Intrinsic formulation of problems of yields 0,47836e-5. The comparison between the errors geometry and kinematics of mechanisms. Mech. Mach. shows that during integration step, the distance from Theory, 17, (1982) 3, 179-184. solution before (i.e. after unstable ODE step that violates [12] Lewis, D.; Simo, J. C.: Conserving Algortihms for constraints) and after constraint stabilization is smaller the Dynamics of Hamiltonian Systems on Lie Groups, J. when dependent coordinate is chosen according to Nonlinear Sci., 4, (2004), 253-299. projective criterion. As one could have expected, this [13] Maisser, P.: A Differential Geometric Approach to the means that the additional stabilization step, based on Multibody System Dynamics, ZAMM, Z. angew. Math. optimized partitioning, shifts unstable ODE numerical Mech., 71, (1991) 4, 116–119. solution back to the constraint manifold with controlled [14] McCarthy, J. M.: Geometric Design of Linkages, error along the manifold (see [38] for details). However, Springer–Verlag, New York, 2000. as has been shown previously, for non-holonomic systems [15] Morawiec, A.: Orientations and Rotations, Springer, the optimized coordinates partitioning should be done 2004. separately at position and velocity level. [16] MÜller, A.: A Conservative Elimination Procedure for Redundant Closure Constraints in MBS Models, Multibody Systems Dynamics, Springer, 16, (2006) 4, Acknowledgments 309-330. The authors acknowledge dr. Joris Naudet (University [17] MÜller, A.; Maisser, P.: Lie group formulation of of Brussels, VUB) for performing computations in the kinematics and dynamics of constrained MBS and its framework of presented numerical example. application to analytical mechanics, Multibody System Dynamics, 9, (2003), 311-352. 612 A. MÜLLER et. al., Differential-Geometric Modeling and Dynamic... Strojarstvo 51 (6) 597-612 (2009)

[18] MÜller, A.: Geometric characterization of the [31] Rico, J. M.; Ravani, B.: On mobility analysis of linkages configuration space of rigid body mechanisms in using group theory, ASME Journal of Mechanical Design, regular and singular points, 29th Mechanisms & 125, March (2003), 70-80. Robotics Conference, ASME 2005 International Design [32] Rodriguez, G.; Kreutz-Delgado, K.: Spatial Engineering Technical Conferences, September 22-28 , operator factorization and inversion of the manipulator 2005, Long Beach, California, USA. mass matrix, IEEE Trans. Robotics and Automation, 8, [19] MÜller, A.: On Non-Smooth Phenomena in the (1992) 1, 65 - 76. Kinematics of Rigid Body Mechanisms, Multibody [33] Saha, S. K.; Schiehlen, W. O.: Recursive Kinematics Dynamics 2007, ECCOMAS Thematic Conference, 25- and Dynamics for Parallel Structured Closed-Loop 28 June 2007, Milano, Italy. Multibody Systems, Mech. Struct. & Mach., 29, (2001) 2, [20] Murray, R. M.; Li, Z.; Sastry, S. S.: A mathematical 143-175. Introduction to robotic Manipulation, CRC Press, 1993. [34] Selig, J. M.: Geometrical methods in robotics, Springer, [21] Murray, R. M.; Sastry, S. S.: Nonholonomic motion 1996. planing: steering using sinusoids, IEEE Trans. Autom. [35] Wie, B.: Space Vehicle Dynamics and Control, AIAA, Control, 38, (1993) 5, 700–716. 1998. [22] Ortega, R. et al: Passivity-based Control of Euler- [36] Wolf, A.; Shoham, M.: Screw theory tools for the Lagrange-Systems, Springer, 1998. synthesis of the geometry of a parallel robot for a given [23] Ostrowski, J.: Geometric Perspectives on the instantaneous task, Mech. Mach. Theory, 41, (2006), Mechanics and Control of Robotic Locomotion, 7th Int. 656-670. Symp. Robotics Research, Munich, Germany, Oct. 1995; [37] Terze, Z.; Lefeber, D.: Dynamic Simulation of in G.Giralt, G. Hitzinger (Eds), Robotics Research, Multibody Systems With no Constraints Violation, Springer–Verlag, 1996. Transactions of FAMENA, 24, (2000), 1-9. [24] Park, F. C.; Bobrow, J. E.; Ploen, S. R.: A Lie [38] TERZE, Z.; NAUDET, J.: Geometric Properties of Group Formulation of Robot Dynamics, Int. J. Robotics Projective Constraint Violation Stabilization Method for Reserch, 14, (1995) 6, 609–618. Generally Constrained Multibody Systems on Manifolds, [25] Park, F. C.: Distance metrics on the rigid-body motions Multibody System Dynamics, 20, (2008), 85-106. with applications to mechanism design, ASME Journal of [39] Terze Z. et al: Null Space Integration Method for Mechanical Design, 117, (1995) 1, 48-54. Constrained Multibody System Simulation with no [26] Park, F. C.; Choi, J.; Ploen, S. R.: Symbolic Constraint Violation, Multibody System Dynamics, 6, formulation of closed chain dynamics in independent (2001), 229-243. coordinates, Mech. Mach. Theory, 34, (1999), 731-751. [40] Wehage, R. A. and Haug, E. J.: Generalized [27] Park, J.; Chung, W.-K.: Geometric Integration Coordinate Partitioning for Dimension Reduction in on Euclidean Group With Application to Articulated Analysis of Constrained Dynamic Systems, Journal of Multibody Systems, IEEE Trans. Robotics, Vol. 21, No. 5, Mechanical Design, 104, (1982), 247-255. 2005, pp. 850-863. [41] Zefran, M.; Kumar, V.; Croke, C.: Choice of [28] Ploen, S. R.; Park, F. C.: Coordinate-Invariant Riemannian Metric for Rigid Body Kinematics, Proc. ASME Algorithms for Robot Dynamics, IEEE Trans. Robotics 1996 Inte. Design Engineering Technical Conferences, and Automation, 15, (1999) 6, 1130-1135. August 19-22, 1996, Irvine, California, USA 29. [29] Papastavridis, J. G.: Analytical Mechanics, Oxford University Press, 2002. [30] Rico, J. M.; Gallargo, J.; Ravani, B.: Lie Algebra and the Mobility of Kinematic Chains, Journal of Robotics System, 20, (2003) 8, 477-499.