Differential-Geometric Modelling and Dynamic Simulation of ... · Kinematika sustava...

Strojarstvo 51 (6) 597-612 (2009) A MUumlLLER et al Differential-Geometric Modeling and Dynamic 597Differential-Geometric Modeling and Dynamic 597 597

CODEN STJSAO ISSN 0562-1887 ZX4701418 UDK 5313925313140049451281

Original scientific paperA formulation for the kinematics of multibody systems is presented which uses Lie group concepts With line coordinates the kinematics is parameterized in terms of the screw coordinates of the joints Thereupon the Lagrangian motion equations are derived and explicit expressions are given for the objects therein It is shown how the kinematics and thus the motion equations can be expressed without the introduction of body-fixed reference frames This admits the processing of CAD data which refers to a single (world) frame For constrained multibody systems the Lagrangian motion equations are projected to the constraint manifold which yields the equations of Woronetz The mathematical models for numerical integration routines of MBS are surveyed and constraint gradient projective method for stabilization of constraint violation is presented

Diferencijalno-geometrijsko modeliranje i dinamička simulacija diskretnih mehaničkih sustava s kinematičkim vezama

Izvornoznanstveni članakU radu je prikazano matematičko modeliranje kinematike diskretnih mehaničkih sustava s kinematičkim vezama pomoću Lievih grupa Kinematika sustava parametarizirana je koristeći vijčane koordinate zglobova kinematičkog lanca Nastavljajući se na takav kinematički model Lagrangeove dinamičke jednadžbe gibanja sustava izvedene su u nastavku rada Koristeći takav pristup pokazano je kako se kinematički model a također i dinamičke jednadžbe gibanja mehaničkog sustava mogu izvesti bez upotrebe lokalnih koordinatnih sustava vezanih za pojedina tijela kinematičkog lanca Takvo matematičko modeliranje omogućava izravnu upotrebu CAD podataka koji se u pravilu izražavaju u jedinstvenom koordinantnom sustavu U slučaju dodatnih kinematičkih ograničenja narinutih na sustav jednadžbe gibanja izvedene su projiciranjem Lagrangeovih jednadžbi na višestrukost ograničenja čime se model izražava u obliku jednadžbi Woronetza U radu su nadalje prikazane formulacije matematičkih modela koji se koriste kao podloga numeričkih algoritama za vremensko integriranje jednadžbi dinamike a također je uz izrađeni numerički primjer opisana i metoda stabilizacije numeričkih rješenja na višestrukosti ograničenja

Andreas MUumlLLEr1) and Zdravko TErZE2)

1) University Duisburg-Essen Chair of Mechanics and Robotics Lotharstrae 1 Germany 2) Fakultet strojarstva i brodogradnje Sveučilišta u Zagrebu (Faculty of Mechanical Eng amp Naval Arch University of Zagreb) Ivana Lučića 5 HR-10000 Zagreb Republic of Croatia

KeywordsCAD Constraint violation Lagrange equations Lie groups Multibody systems Numerical integration Woronetz equations

Ključne riječiCAD Diskretni mehanički sustavi s kinematičkim vezama Jednadžbe Woronetza Lagrangeove jednadžbe Lieve grupe Numerička integracija Numeričke povrede kinematičkih ograničenja

Received (primljeno) 2009-04-30 Accepted (prihvaćeno) 2009-10-30

Differential-Geometric Modelling and Dynamic Simulation of Multibody Systems

zdravkoterzefsbhr

1 Introduction

Geometric methods in dynamics and control have become widely established and valued approaches The actual meaning of rsquogeometricrsquo varies with the context in which it is used It generally refers to the differential geometry of the mathematical structures underlying a dynamic system As an example non-linear control theory for continuous systems is solely built upon the differential-geometry of the control system [5 21-23] In other words generally one deals with control systems on manifolds Another area that makes use of geometric

concepts is the field of geometric integration [10] The subject here is the integration of dynamic systems on manifolds of which systems on Lie groups are special cases [12 27]

In mechanism theory one is concerned with the geometry of finite motions and on velocity level with their differential geometry As it turns out in the case of rigid multibody body systems (MBSs) the differential-geometric objects correspond to with the line geometry of the joints and bodies The underlying reason is the isomorphism of the algebra of screws and the algebra of the Lie-group of rigid body motions [20 34] The latter is the

598 A MUumlLLER et al Differential-Geometric Modeling and Dynamic Strojarstvo 51 (6) 597-612 (2009)

group of isometric orientation preserving transformations of the three-dimensional Euclidean space In essence the parameterization of screws with line coordinates gives rise to geometric treatment of differential-geometric entities Screw theoretic considerations also gave rise to the development of advanced methods for the design of mechanisms [14 25 36] Besides these geometric and kinematic applications explicit use of the Lie group structure admits to expressed motion equations of mechanisms in a very compact and elegant way [4 11 17 24 26 28] and also admits application of integration schemes on Lie groups [25] The difference of classical matrix formulations and recursive formulations [1 7-8 32-33] is that these formulations are entirely formulated in terms of screw quantities corresponding to the mechanismrsquos joint geometry (by virtue of the parameterization with line coordinates) Furthermore the coordinate invariance allows for a formulation without body-fixed reference frames and hence for the incorporation of CAD data This fact will be emphasized throughout the paper

The paper is organized as follows Section 2 deals with the geometry and summarizes the representation of configurations of rigid MBSs After recalling the graph representation of the MBS topology configurations are expressed as elements of the 3-dimensional special Euclidean group The configuration space is then parameterized using canonical (axisangle) and non-canonical parameters A formulation is given that does not 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 ie 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

2 MBS geometry

21 The topological graph of an MBS

The MBS topology is represented as an oriented graph Γ = (B J) where the set of vertices B represents the bodies and the set of edges J represents the joints of the

MBS It is assumed hereafter that Γ has one connected component In this case Γ has γ = m minus N fundamental loops (FLs) where m= |J| is the number of joints and N= |B| minus 1 Joints and bodies are respectively denoted by Jα α = 1 m and Bk k = 0 N Body B0 stands for the ground An edge JαisinΓ is an unordered pair Jαequiv (Bk Bl) (for brevity we write αisinΓ or (k l) isinΓ if Jα equiv (k l))

A spanning tree of Γ is denoted by G = (B JG) and the corresponding cotree by H (ΓG) = (B JH) or simply with H Vertex B0 is chosen as the root of the tree JG is the set of tree-joints and JH = J JG is the set of cut-joints G comprises N tree joints The term cut-joint indicates that if we rsquocutrsquo these joints we get a tree topology

Figure 1 An example for the graph description of a MBSSlika 1 Primjer graf-opisa diskretnog mehaničkog sustava s kinematičkim vezama

Let Gd be the directed tree wrt to the root B0 which is the directed subgraph of G such that to every vertex there is a path starting from the root of G (Figure 1) The directed tree induces the following ordering relation of joints and bodies

J1

J2

J3

J4

J5

J6

J7

J8

B1

B2

B3

B4

B5

B6B7J9


Bbull l is the direct predecessor of body Bk denoted by Bl = Bk minus 1 iff1 (l k) isin Gd (for short l = k minus 1)Jbull β is the direct predecessor of joint Jα denoted by Jβ = Jα minus 1 iff Jβ = (middot k) isin Gd Λ Jα= (k ) isin Gd (for short β = α minus 1)Jbull β is a predecessor of joint Jα denoted by Jβ le Jα iff there is a finite m such that β = α minus 1 minus 1 middot middot middot minus 1 (m times minus1)

Note that possibly k minus 1 = l minus 1 for k ne l Eg in Figure 1 J1 = J2 minus 1 J2 = J6 minus 1 J6 = J7 minus 1 so that J1 lt J7 For each body Bk there is a unique tree joint Jα connecting Bk to its predecessor ie Jα = (middot k) isin Gd This fact is denoted by α = J (k) In other words body Bk is connected to Bkminus1 via Jα Finally Jroot (k) denotes the smallest α le J (k) In figure 1 J (6) = 7 J (5) = 6 and Jroot (k) = 1 for all k An orientation of G wrt Gd is an indicator function s so that s(Jα) = 1 or s(Jα) = minus1 if Jα isin G is respectively positively or negatively oriented wrt Gd Also s(α) is used for short

Remark 1 Arithmetical operations on body or joint indices like l = k minus 1 and β = α minus 1 are to be understood according to the above index arithmetics In particular k minus 1 does not mean that we subtract 1 from the body index k it rather refers to the body Bk minus 1

22 The configuration space of an MBS

A body-fixed is attached to each body of the MBS reference frame (RFR)We assume the existence of an inertial system called the ground which is referred to as B0 An inertial frame (IFR) is fixed at the ground (Figure 2) and the configuration of body Bk is expressed as C isin Se (3)

with e isin So (3) p isin 3 (1)

e is the rotation matrix and p the position vector describing respectively the rotation and displacement of the body-fixed RFR wrt to the IFR

The configuration of the MBS is denoted by c equiv (C C) isin Se (3)N A subspace corresponds of relative transformations Gα sube Se (3) to each joint called the motion space of Jα This is not necessarily one of the Se (3)-subgroups [34] Eg for revolute joints Gα = So (2) is the group of rotations about a fixed axis Denoted by r the relative configuration of the two bodies Bl and Bk is connected by Jα equiv (l k) isin Γ (Figure 2) A configuration of the MBS is admissible only if Jα isin Gα With there conditions the set of admissible configurations is

1 (k l) isin Gd means that Bk is the source and Bl is the target of the edge Notice that (l k) notin Gd since Gd is directed

k

1

N

∆

α

Cabs = c isin Se (3)N | r isin Gα (2)

This is called the configuration space of a holonomic MBS in absolute representation

Cabs is a rather abstract notion that covers MBSs with arbitrary topology If we were to use this representation for our actual computations we would need to introduce constraints that restrict the relative motion of adjacent bodies to the motion spaces In fact this leads to absolute coordinate formulations What we will do instead is to eliminate the constraints by resolving for the relative configuration of tree-joints and introduce constraints only for the cut-joints

23 The geometry of an MBS with tree-topology

231 Relative configurationsA joint Jα (l k) isin Γ connecting bodies Bl and

Bk is completely characterized by three consecutive transformations (Figure 2)

constant transformation bull S from the joint frame (JFR) to the RFR on Blconstant transformation bull S from the JFR to the RFR on Bkvariable transformation bull N of these JFRs according to the jointrsquos mobility and the orientation of Jα isin Γ that determines whether N transforms from the JFR on Bk to that on Bl or vice versa

This information characterizes the joint kinematics without reference to a topological tree By definition Jα (l k) isin Gd means that Jα connects Bk to its predecessor Bl = Bk minus 1 For tree-joints the above three transformations must be combined with a transformation from Bk to Bkminus1 RFR The function s(α) indicates whether Jα isin Γ is positively oriented wrt to Gd Consequently if Jα is negatively oriented the transformation N must be inverted Eg the meaning of a positive revolute joint angle is reversed if its orientation is opposite to the directed tree Thus the relative configuration from the RFR on Bk to that on Bl is

R = M Q with M = S Sminus1 Q = S N s(α) Sminus1 (3)

M transforms from JFR to BFR on Bl and Q from RFR on Bk to JFR on Bl Finally the configuration of body Bk is given with that of its predecessor and the joint transformation r

C = C R α = J (k) (4)

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 configuration by r equiv (r r) isin Crel where

α

α l

α k

α

α

α

α α α α α kα l α α k α α k

α α

α

k k-1 α

1 N

0 1


Crel = G1 times times GN (5)

is called the configuration space in relative representation The degree of freedom (DOF) of the MBS is the dimension of the configuration space δ = dimC = Σα dimGα

Remark 2 The body-fixed RFRs are arbitrary We can choose other RFRs and the configurations r in this representation are related to the original ones by r = grgminus1 where g isin Se (3)N These configuration constitute another representation of the configuration space say Crel All such representations obtained by a change of body-fixed reference frames are equivalent Moreover the motion equations for the MBS are invariant wrt such frame transformations

232 Parameterization of the configuration space

So far we have not used any kind of joint variables 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 ie 1-DOF motions Eg 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 axes of revolution while a spherical joint is considered as three consecutive rotations If Jα has mobility2

να = dimGα the individual intermediate one-parametric motions are given in terms of a set of (relative) generalized coordinates for Jα denoted by qα1 qανand a set of screw coordinate vectors Z Z (Figure 2)

such that Ns(α) (qα) = eα1 middotmiddotmiddot eαν The screw coordinate vectors are expressed in the JFR either on Bl or Bk depending on which one is the source of the oriented edge Jα isin Gd With (3)

Q (qα) = eα1 eαν where X = Ad-1 Z i=1v (6)

and the adjoined operator matrix [20] so that the relative transformation by the tree joint is

r (qα) = Meα1 eαν (7)

Z are the coordinate vectors of the joint screws of Jα wrt JFR on Bk while X are the crew coordinate vectors wrt to the RFR on Bk

2 For compactness we omit the joint index and write ν if the joint is clear from the context

macr

macr

macr

α

α1 αv

α

Zqα1and

Zqανand

α

Xqα1and

Xqανand

αiS

α k αi

α α

Xqα1and

Xqανand

αi

αi

2) Using canonical coordinates of 1 kind NS(α) (qα)

= eα1 αν

Q (qα) = eα1 αν (8)

with X i in (6) and r according to (3) This corresponds to the axis-and-angle parameterization of screw motions [20 34]

3) The parameterization (6) and (8) suffer from singularities when dealing with spacial rotations for which a global parameterization with canonical coordinates does not exist ie via the exponential map These singularities can be avoided using Euler-parameters [20] to describe the rotation part of N Another motivation for the use of non-canonical parameters like Cayley-parameters is to reduce the numerical complexity in applications where the full range of rotation is not used

Figure 2 Relative kinematics of two bodies Bl and Bk connected by joint Jα

Slika 2 Relativna kinematika dvaju tijela Bl i Bk povezana kinematičkim zglobom Jα

In order to keep the formulation as general as possible we will write Q (qα) whenever possible and only use the specific form such as (6) when necessary

The vector (qα) = (qα1 qαv α isin Gd) comprises the generalized coordinates qa of the MBS with tree-topology The parameter manifold of an MBS with nR revolute and nP prismaticscrew-joints is

(9)

is also referred to as the configuration space of the MBS and q equiv (qa) isin as its representing point

233 Relative configurations without body-fixed joint frames

From a practitionerrsquos point of view the definition of 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 fixed joint frames Such formulations are advantageous

Zqα1++and

Zqανand

Xqα1++and

Xqανand

α

αi α

α

α


when processing CAD data which refer to a single world frame (IFR) The CAD construction serves as reference configuration to which we assign q = 0

Denote with m = C (0) the reference configuration of Bk then

m = M M M α = J (k) β = Jroot (k)

Y = Adm X (10)

The Y i are the screw coordinate vectors for joint Jα expressed in the IFR in the reference configuration m With this data the body configurations are

C (q) = eβ1 eβv eα1 eαv m

α = J (k) β = Jroot (k) (11)

Remark 3 The advantage of this formulation in conjunction with CAD-data shall be stressed CAD-systems use the IFR to express the geometry and the inertia properties of the mechanism Consequently in the reference configuration the RFR of all bodies coincide with the IFR so that m = I All that is needed are the screw coordinate vectors Y wrt the IFR in the reference configuration The vector Y equiv (ω p times ω + hω) is merely

given in terms of the direction vector ω along the joint axis the position vector p of some point on the axis and the pitch h of the joint Since the moments of inertia are also expressed in this IFR the definition of body-fixed RFRs is dispensable (section 4)

24 The geometry of MBS with kinematical loops

An edge corresponds to any loop of the cotree H (which is empty for treestructures) As we have seen the configuration of an MBS can be represented in absolute formulation (2) listing the configurations of all individual bodies plus the constraints due to the joint couplings Alternatively configurations can be represented in relative formulations ( 5) where for MBS with kinematic loops the configuration must comply with loop closure constraints arising from cotree joints

Hence an MBS with kinematic loops can be modeled as an MBS with tree topology subject to holonomic constraints That means that the cut-joints Jα α isin H are removed and the resulting tree-system is subject to the corresponding closure conditions

Definition 1 Denote with να = dim Gα the mobility of joint Jα A smooth map gα Se (3) rarr 6ndashνα is called a local constraint map iff gα(r) = 0 harr r isin U cap Gα for some U sub Se (3) containing the identity If this holds on all Se (3) gα is called a global constraint map

k k

k β α-1 α

αi kαi

αi

k

kk

Yqβ1and

Yqβνand

Yqα1and

Yqανand

k

αi

αi

α α

Since each FL comprises exactly one cut-joint we denote with Λα the FL containing cut-joint Jα Body Bk is the root body of the FL iff Bk lt Bl foralll isin Λα Eg in Figure 1 the root body of Λ8 is B2 and that of Λ3 is B1 Denoted by Hd the directed cotree such that all Jα isin H are positively oriented Using bodyfixed reference frames the geometric closure condition for Λα with cut-joint Jα equiv (l k) isin Hd is

S-1 C-1 C S = N (12)

The corresponding closure constraints are the independent components of (12) for which N is identically zero whenever r isin Gα These components are listed in Table 1 for some technical joints It is indicated whether the motion space of the joint corresponds to a Se (3) -subgroup A cut-joint with mobility να yields 6ndash να (locally) independent constraint equations

Remark 4 The distinction between local and global constraint maps should be stressed The revolute joint is an instructive example for which the geometric constraints on the relative rotation are e1

3= e23 = 0 These plus

the three position constraints only give in fact rise to a local constraint map This is so because the conditions on e enforce the e3-axes of the coupled JFRs to remain parallel but they may have opposite directions for which the constraints are also fulfilled

With parameterization of the tree-joint the cut-joint constraints yield constraints on the generalized coordinates The overall system of m = 6γ ndash sumαisinHνα geometric constraints is summarized in

g (q) = 0 (13)

The admissible parameter space defined by (13) is the configuration space

V = q isin | g (q) =0 (14)

of the MBS with kinematic loops V is an analytic variety comprising smooth manifolds separated by singular points [18-19 30-31]

3 MBS Kinematics

31 The kinematics of an MBS with tree-topology

The velocity of body Bk in body representation is defined as V = C-1 Ċ V is the twist of the RFR on Bk

wrt to the IFR expressed in this RFR It is verified with (7)V(q) = sum Ka (q) qa (15)

with Kαi = AdC -1C Hαi

if m le k for α = J(m) (16)

H Q Qi iα

α αα= partminus1

(17)

kα1 l αk α

α

α

k

and

k k k

k

alen k

k

k m


Table 1 Cut joint constraints for some technical joints e is the rotation matrix and p is the position vector describing the relative configuration (1) of the two bodies connected by the cut jointTablica 1 Kinematička ograničenja za odabrane kinematičke zglobove Rotacijskom matricom e i vektorom položaja p definirana je relativna kinematika (1) dvaju tijela koja su povezana određenim kinematičkim ograničenjem

DOF

SSG

Motion (Joint type) Relativno gibanje

(vrsta zgloba)

Constraints Kinematička ograničenja

Subgroup Subgrupa

1

Translation along fixed axis (prismatic joint) Translacija duz nepomične osi (translacijski zglob)

e12 = e2

3 = e13 = 0

p1 = p2 = 0times

Rotation about fixed axis (revolute joint) Rotacija oko nepomične osi (rotacijski zglob)

e12 = e2

3 = 0p1 = p2 = p3 = 0

times

Planar translation Planarna translacija

e12 = e2

3 = e13 = 0

p3 = 0times

2

Rotation + translation along same axis (cylindrical joint) Rotacija i translacija oko iste nepomične osi (cilindričan zglob)

e13 = e2

3 = 0p1 = p2 = 0

times

Two successive rotations about orthogonal axes (universal joint) Dvije slijedne rotacije oko ortogonalnih osi (univerzalni zglob)

e12 = 0

p1 = p2 = p3 = 0-

Spatial translation Prostorna translacija e1

2 = e23 = e1

3 = 0 times

3

Planar motion (planar joint) Planarno gibanje (planarni zglob)

e13 = e2

3 = 0p3 = 0

times

Spatial rotation (spherical joint) Prostorna rotacija (sferni zglob)

p1 = p2 = p3 = 0 times

4

Planar motion + spatial translation (joint macro) Planarno gibanje + prostorna translacija (složeni zglob)

e13 = e2

3 = 0 times

6 Spatial motionProstorno gibanje - times

K is called the Kinematic Basic Functions (KBF) of Bk From its definition (16) there follows immediately the recursion

(18)

The expression for Hαi depends on the choice of local

coordinates on Gα (232)1) For canonical coordinates of 2 kind (6)

(19)

2) For canonical coordinates of 1 kind (8) using the left and right dexp-map on se (3)

(20)

with Y = Xqα1+ + Xqαv

3) Using non-canonical coordinates Hαi is of a

specific form eg when parameterizing the rotation matrix e in (7) with Cayley- or Euler-parameters

For the standard choice of canonical coordinates of 2 kind using A in (19) Kαi = AdC-1 C AdA

-1 X are the

instantaneous joint screws of Jα expressed in the RFR of Bk This is why K is occasionally called the geometric Jacobian

With the MBS configurations c isin Cabs we define the velocity of an MBS as V = c-1 c equiv (V V) isin TICabs What is more important for the following is the state space T and the generalized velocities q isin Tq that are related to V via (15)

32 Kinematics without body-fixed joint frames

The KBF can also be expressed in terms of the Y in (10) as

(21)

with A = eα1 eαv eβ1 eβv β = J (k)

This is again an expression without reference to body-fixed JFRs

33 Matrix formulation for tree-topology MBS

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)

k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv


path from Bk to B0 Let us for the moment assume that the bodies and joints of this path are numbered increasingly with 1 2 k Combining the corresponding KBFs to

(22)

then Kk = AkHk with Hk= diag (H1i Hki

) and

(23)

The Hα equiv (Hα1 middotmiddotmiddot Hαv

) are 6times vα matrices according to the mobility of Jα and I and 0 is respectively the 6times6 identity and zero matrix With our primary choice of coordinates the Hα1 equiv Xα1 are constant

The items of Ak and Hk are obvious in terms of arbitrary body indices In summary combining in K the Kk matrices of all terminal bodies the MBS velocity is

V = Kq (24)

The structure of K reflects the tree-topology of the MBS It is interesting to notice that the lower block triangular matrix can be expressed as Ak = (I minus Dk)

minus1 = I + Dk + + + with help of the nil-potent matrix

(25)

that only comprises the relative configurations Cminus1C This is a matrix version of the recursion (18)

34 Partial derivatives of the KBFs

Due to the recursive formulation in terms of relative configurations it holds

partqβjKαi = AdCndash1CpartqβjHαi

if α=β

+ [KαiKβj] if αltβ for β le J(k) α = J(m)

(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1

For our standard choice of canonical coordinates of 2nd kind (26) simplifies to

partqβjKαi = [KαiKβj

] α le β le J(k) iltj (27)

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

V =Kbq + [Kb Kc]q q b le c le αν α = J (k) (28)

We use the summation convention over repeated indices Aabx

b equiv Σb Aabxb As the individual KBFs can

be combined to K so can their derivatives to give the differential dK(q x) = partaK(q)xa Using [KαiKβj

] = AdCndash1C

[KαiHβj

] = minusAdCndash1CadHβi Kαi

β = J(m) and the matrix

(29)

such that adXY = [X X] then ( 26) yields

dKk (q) (x) = Ak (q) [dHk (q x) minus ak (q x)Kk (q)] (30)

(31)

(32)

With canonical coordinates 2 kind Hk = diag (X X) is constant and dKk (q) (x) = minusAk (q) ak (q x)Kk (q) If x is a sufficiently small perturbation

Kk (q + x) = Kk (q) + dKk (q x) = (I minus Akak)Kk (q) (33)

k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d


35 Kinematics of an MBS with kinematical loops

The kinematic constraints ie the closure conditions on velocity level are formulated under the condition that g (q) = 0 Consider the relative velocity of Bk and Bl connected by cut-joint Jαequiv(l k)

(34)

The last ν terms are the twist vectors of the successive screw motions that describe the relative joint motion expressed in the JFR on Bk If the motion space of Jα is a SE (3)-subgroup these twists belong to the respective subalgebra

The kinematic closure condition is the disappearance of the components of the relative velocity not in this subalgebra That is we need to eliminate the twist components in (34) that correspond to the joint motion which is unconstrained To formalize this ellimination introduce a selector pα that restricts the complementary3 se (3)-subspace of the να -dimensional space of cut-joint twists expressed in the JFR on Bk Application to (34) yields a sufficient set of kinematic constraints for the cut-joint Jα

pα Lα q= 0 (35)where Lα equiv ( ) is the 6 times N matrix with columns

(36)

This is the standard formulation if the cut-jointrsquos motion space is a subgroup In this case the selector also determines the relevant components of the geometric constraints (12) The definition of the joint axes corresponds to the se (3) standard basis eg the e3 -axis of the JFR is used as joint axis of pris-maticrevolutecylindrical 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 subgroups

Combining the constraints (34) for all γ loops yields the system of m kinematic constraintshq= 0 (37)

The time derivative of (35) yields the dynamic constraints for Λα ie the constraints on acceleration levelhq+ hq = 0 (38)

The recursions (27) (26) provide a closed form for h = partqahqa

3 Notice that this has nothing to do with an orthogonal complement since this would require a metric on Se (3) Furthermore the particu-lar complement space depends on the specific JFR used to express joint twists

kα k k ll α k k k l l

middot

and and and

eα2 eαv α1

middot middot middotαv

αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot

Table 2 Selector pα for the constrained velocity components of a cut joint Jα of which the motion space corresponds to a subgroup of Se(3)

Tablica 2 Selektor pα za komponentu kinematički ograničene brzine zgloba Jα čiji prostor gibanja odgovara subgrupi Se(3)

DOF SSG


(vrsta zgloba)

Selector Pα Selektor Pα

1

Translation along fixed axis (prismatic joint) Translacija duž nepomične osi(translacijski zglob)

1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0

Spatial translation Prostorna translacija

1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1

3Planar motion (planar joint) Planarno gibanje (planarni zglob)

0 0 1 0 0 00 0 0 1 0 00 0 0 0 10

Spatial rotation (spherical joint) Prostorna rotacija(sferni zglob)

0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4

Planar motion + spatialtranslation (joint macro) Planarno gibanje + prostorna translacija(složeni zglob)

0 0 0 1 0 00 0 0 0 10

middot middot

=


Remark 5 A problem often encountered is the dependence of loop constraints ie h permanently a rank less then m The reason is that for a certain cut-joint a certain number of constraints are formulated regardless of whether or not the relative motion (to be constrained by the closure condition) can actually take place if the cut-joint is removed The obvious problem for the formulation of loop constraints (in particular when automatically generating constraints) is the detectionremoval of redundant constraints Fortunately one can cope with quite a large class of problems by noting the geometric background [16]

4 MBS Dynamics

41 The Lagrangian motion equations of a MBS with tree structure

Define a frame with origin at the center of mass (COM) of Bk Let MisinSe (3) be the transformation from

COM-frame of Bk to its RFR The velocity of the COM-frame is

(39)

With the mass m and the inertia tensor Θ = (Θij) of Bk

wrt the COM-frame define the inertia matrix of Bk

(40)

The kinetic coenergy of Bk is then

(41)

where

(42)

is the inertia matrix J of Bk wrt its RFR

Remark 6 We shall comment on the use of CAD-data here In section 233 we abandoned the use of body-fixed RFRs and introduced the formulation (11) We have further seen that the reference configurations are m = I since CAD-systems relate to all quantities to a world frame which in the reference frame coincides with the RFR An analogous argument holds for the inertia data so that CAD-system already determine J In summary local RFRs are completely dispensable

The total kinetic coenergy of the MBS is T = ΣkTk

In terms of generalized velocities this is the quadratic

mdashk

mdashminusk k

k k

kk

mdash mdash mdashk k k k k k

mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)

Besides singularities of our parameterization of the configuration space gab is positive definite Hence gab

defines a metric on the tangent space Tq in every point q which makes a Riemannian space

In the Lagrangian framework the dynamics of a holonomic MBS are governed by the Lagrangian motion equations (LME) of second kind

(44)

where the Lagrangian Λ(q q) = T (q q) minus U (q) is given in terms of the kinetic coenergy and the potential energy U are generalized forces not covered by the Lagrangian such as dissipation or control forces

The LME for a skleronomic holonomic MBS with tree topology are [13]

gab (q) qb + Γabc (q) qb qc = Qa (45)

with and the Christoffel symbols of the

first kind

(46)

In view of (27) they can be expressed as

alt b le c or b le a lt c r = max (ab) s = min (ab) (47)

Relation (47) can be simplified using Binetrsquos inertia tensor

(48)

where and Ω(X) = ω gives the angular part of the twist X = (ω ν)

In matrix notation the LME are expressed as

G(q) q+ C (q q) q = Q (49)

where

G(q) = ( ab) C (q q) = (Γabcqc) Q = (Qa) (50)

G is the generalized mass matrix Cq represents generalize Coriolis and centrifugal forces and Q covers all potential forces and remaining forces not represented by the Lagrangian Λ With (24) and (30) follows

middot middot middot

k k k

middot

middot middot

middotmiddot middot middot

mdash mdash mdashk k k k k k k k k k k k

k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=


G(q) = KT JK isin nn (51)

C (q q) = KT (bT J minus JAa)K (52)

where J= diag(J J) and b only depending on the body twists V = Kkq

b (V)= diag(adV adV) isin 6N6N (53)

The first term in the brackets of (52) represents gyroscopic forces due to the fact that qTKT bT JKq = VT bT JV equiv 0 since b (V ) V equiv 0 Hence one can separate a gyroscopic potential from the Lagrangian Λ which contributes to gyroscopic forces

42 The Lagrangian motion equations of a constrained MBS

Using relative coordinates an MBS with kinematic loops is a tree-topology system subject to a system of r holonomic constraints (13) More generally an MBS is subject to further geometric andor non-holonomic velocity constraints Denote the overall system of m geometric constraints by f k (q) = 0 and with f a1(q) qa = 0 the overall system of r = |a1| ge m (linear) velocity constraints The geometric constraints define the configuration space of the constrained MBS V= q isin|f k (q) = 0 For simplicity we presume rank (partqa f k) = m and rank (f a1 ) = r on all V Then V is a n minus m-dimensional smooth manifold

There exists a partitioning (f b1 ) = f b1 | f b1 ) with rank (f b1 ) = r and thus an orthogonal complement Fa of f a1 with rank (Fa )= n minus r ie f a1 F a = 0 Hence the system of kinematic constraints has a solution

qa = F a (q) qaprime2 q isin V (54)

(54) is a parametrization of the nminusr-dimensional vector space defined by the velocity constraints - a subspace of the nminusm-dimensional tangential space TqV Ie the vector space of admissible velocities may have a lower dimension than the base manifold V We use primed indices to indicate reference to this subspace The solution (54) is not unique and a different choice of the partitioning yields a different chart

The motion equations for the constrained MBS in terms of qaprime2 follow by substitution of qa using (54) [13]

ga2b2qbprime2 + Λarsquo2brsquo2crsquo2

qbprime2qcprime2 = Qarsquo2 (55)

with the metric garsquo2brsquo2= gabFa Fb (56)

the generalized Christoffel symbols of first kind

Λarsquo2brsquo2crsquo2 = (Γabc minus gac1

f c1 partqc f a1 )Fa Fb Fc (57)

where (f c1 ) = (f a1 )minus1 and the generalized forces Qaprime2= Fa Qa

The equations (55) are the equations of Woronetz Assuming regularity of Fa (56) defines a metric on V

The Woronetz equations can be written in matrix form

G(q) q2 + C (q q) q2 + N (q) = Q (58)

where

G=FTGF C=FT (CF + GF ) N=FTN Q=FTQ (59)

The system (55) together with the kinematic constraints f a1 (q) qa = 0 yield n differential equations in q isin which completely describe the MBS dynamics

The constraints are holonomic if and only if the integrability conditions Ab1 equiv 0 are satisfied where

Ab1 = Fb Fc (partqb f c1 minus partqc f c1) (60)

is called the object of non-holonomy

5 Numerical integration of MBS Dynamics

51 ODE integration

As explained the LME (45) yield an ODE system on the state space T where n ODE is assumed T is a tangential bundle that comprises configuration manifold and union of all tangential spaces Tq It is a 2n-dimensional manifold covered by the coordinates q q T = (q q) q isin q isin Tq By adopting the generalized mass matrix G(q) as a Riemannian metric on the configuration manifold the tangential space Tq (ldquothe fiber of the tangential bundle at point qrdquo) becomes a locally Euclidean vector space spanned by a covariant basis ĝqi

The LME can be writen in the shorter matrix form asG(q)q = Q(q q t) (61)and following the standard procedure it can be turned into the 2n ODE form

v = Gndash1 (q) Q (q v t) q = v q = qv

x f x f xv

G Q= =

minus( ) ( )

1 (62)

The solution of (62) is the integral curve of the vector field f ( x ) on tangential bundle (ldquovelocity phase spacerdquo) for a set of Cauchy data (t0 x0)

52 DAE integration

If additional holonomicnonholonomic constraints are imposed on the system instead of reducing the system to the constrained LME form as explained in chapter 42

middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2

ndash ndash ndashndash

ndash ndash ndash ndashmiddot

a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash


one can model the system using a mathematical model in descriptor form This is straightforward modelling procedure that can be easilly applied for a wide class of MBS (thus being a basis for many general MBD packages) but it leads to DAE integration (numerical integration of system of differential-algebraic equations) that is generally more formidable differential-algebraic than standard ODE integration routine

To avoid high-index DAE formulations the mathematical model for dynamic simulation of multibody systems with imposed holonomic constraints Φ (q t) = 0 and Φq

(q t) x = 0 where Φ(q t) = (f k (q t)) and Φq

= (partqa f k) is often shaped as a differential-algebraic system (DAE) of index 1 (redundant coordinates formulation ideal constraintsrsquo reaction forces are assumed)

(63)

where the Lagrangian equations of the first type

G (q) q + ΦT (q t) λ = Q (q q t) (64) q isin n Φq

T isin mtimesn rank [Φq]= m

and the constraint equations at the acceleration levelΦq

(q t) q = ξ (65)are put together System (63) is uniquely solvable for the set of consistent initial values It can be integrated in time to obtain the motion of the system as well as the constraint reactions Although constraints at the acceleration level will be immanently satisfied since (65) is included in mathematical model (63) and will be explicitly solved during integration the numerical non-stability of (65) can induce constraints violation at both the position and velocity level

As explained holonomic constraints restrict system configuration space and impose constraints at velocity level a trajectory Tqi = qi (t) is ldquoforced to moverdquo on the n minus m dimensional constraint manifold nminusm(t) = q isin nminusm Φ(q t) = 0 t ge 0 q(t0) isin

nminusm (t0) and linear constraint equation (66) is imposed at system velocities

Φq (q t) x = ndash Φt (66)Although analytical solution curves for consistent

initial conditions will move on a tangential bundle T nminusm

nminusm (having satisfied all constraints imposed on the system) a numerical solution will tend to drift away from constraint manifold

If beside holonomic constrains additional non-holonomic constraints given in linear (Pfaffian) form are imposed on the system (further reducing dimensionality of admissible velocities by forming subspace at Tq ) a mathematical model similar to (63) can be introduced [38] allowing the same integration procedure as for the holonomic systems

53 Stabilization of constraint violation

If the governing equations of the system are based on a mathematical model in descriptor form leading to time integration of redundant coordinates a constraint violation stabilization method has to be applied during the integration procedure Alternatively a well-known algorithm described in [39] can be used It is based on a coordinate partitioning procedure prior to integration followed by the time integration of the selected independent coordinates only Another pojective stabilization procedure is described in [3]

The stabilization algorithm proposed in [38] is based on projection of the steppost-integration results (system current state point) to the constraint manifold in the course of simulation An advantage of the procedure is straightforward integration of dynamic equations while stabilization algorithm is to be applied only after intolerable constraint violation is detected The procedure can be briefly described as follows

If constraint violation (specified by given numerical tolerances) occurs after step-integration phase the ldquopositionrdquo stabilization step is to be performed by correcting dependent coordinates sub-vector qdep via solving system constraint equation providing thus shifting of the system configuration-point q back to constraint manifold nminusm The procedure is then repeated at the velocity level by correcting qdep to bring q in accordance with (66) This means that as stabilization step final result time-integration values q q are projected to the constraint manifold tangential bundle T thus completely satisfying constraints of the system As will be seen later a crucial point of the algorithm is appropriate selection of sub-vectors qdep and qdep to provide the optimal stabilization effect

Criteria for coordinates selection can be expressed geometrically basically every selection that returns sub-vector of dependent coordinates qdep whose basis vectors have non-zero projections on the constraint subspace (corresponding mtimesm sub-matrix of constraint matrix Φq

is non-singular) is a correct one and can be used for stabilization procedure Consequently basis vectors of independent coordinates qind must have projections on tangential space of constraint manifold Tq

nminusm that complement If the extracted sub-vectors do not satisfy specified conditions selection is not a valid one and calculation will fail

531 Numerical errors along configuration manifold

The main problem that may occur during stabilization procedure is an inadequate coordinate selection that may have a negative effect on integration accuracy along the constraint manifold Although as was explained every

middot

middotmiddot

middotmiddot

G ΦqT

Φq 0


partitioning that returns acceptable sub-vectors can be used for stabilization a non-optimal choice of coordinate sub-vectors may cause an increase of numerical errors along the manifold during stabilization part of the integration procedure If this happens a correction of constraint violation will be accomplished at the expense of ldquokinetic motionrdquo accuracy that comes as a result of ODE integrators of system variables q q

The ldquomechanismrdquo of emerging of numerical errors along configuration manifold because of an inadequate partitioning during the stabilization procedure is outlined in Figure 3 For illustrative simplicity holonomic (as well as scleronomic) example q = x isin equiv 2 rank [Φx

] = 1 nminusm = is discussed Fundamentally nothing is changed if more general case is analysed

Assuming that starting from an integration of ODE gives result instead of exact position a projection on the constraint manifold by adjusting coordinate x1 (solving constraint equation along x1 curve) yields result

which is consistent to the constraint If instead of x1 the variable x2 was chosen to be a dependent coordinate an adjustment of the integration result along x2 curve would yield solution which is also consistent which the constraint but contains considerable error along the manifold A remedy for the problem of an inadequate selection of dependent coordinates has been described in [2] where a projective criterion to the coordinate partitioning method is discussed

Figure 3 Numerical correction of the constraint violation at the configuration (ldquogeneralized positionrdquo) levelSlika 3 Numerička korekcija povreda kinematičkih ograničenja na nivou poopćenih položaja

532 Criterion for optimized coordinates partitioning

In the framework of constraint violation stabilization algorithm proposed in [38] the main idea of ldquocontrollingrdquo the numerical error that is eventually introduced during stabilization projection step is to ldquominimizerdquo the artificial numerical shift along the manifold by projecting integration point ldquoas direct as possible using

coordinate curvesrdquo to the constraint manifold This can be accomplished by utilizing criterion for optimized partitioning that allows for determination of those coordinates whose direction vectors ĝqi

are directed most orthogonally toward constraint manifold By selecting these coordinates as dependent ones during projection procedure the constraint equation would be solved along the selected ldquoorthogonally directedrdquo curves providing

thus optimized projection and ldquocontrolledrdquo numerical error along the manifold The basic idea of the criterion is to determine those m coordinates whose direction vectors ĝqi

deliver the biggest relative projections to the

(ie ldquosmallrdquo value of αp Figure 3) and select them as dependent variables which will be adjusted during the stabilization procedure By correcting coordinates whose direction vectors align well with the constraint gradients it is ensured that correction procedure will shift a state-point of the system ldquoas direct as possible using coordinate curvesrdquo to constraint ldquohyper-surfacesrdquo minimizing thus an error along constraint manifold Alternatively the coordinates that have ldquogoodrdquo projections on the local tangential space Tq

nminusm can be extracted and treated as independent coordinates [37]

The main ldquomeasurerdquo for expressing the criterion is relative length of the projection of the coordinates direction vectors ĝqi

to the constraint subspace (or tangent subspace Tq

nminusm) The relative lengths of 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 ĝqi

on the subspace and subspace Tq

nminusm respectively) and is the squared length of ĝqi

in the natural basis G Those coordinates 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) should be calculated with respect to the system manifold Riemannian metric G while expressions in nominator must be calculated via sub-metrices associated with the constraint subspace and its orthogonal counterpart as explained in [2 39])

54 Numerical example

541 Mechanical system with non-holonomic constraint

In the framework of the first numerical example that presents numerical forward dynamics of the satellite motion while its panel system develops the effect of

middot


the described constraint stabilization procedure will be illustrated without details on mathematical modeling of the satellite dynamics (for more details refer to [38])

Figure 4 Satellite multibody modelSlika 4 Mehanički model satelita

The considered satellite is based on the INTELSAT V satellite [35] it consists of a main body and two panels of four parts (Figure 4) A planar model is used and no gravitational force is incorporated in the simulation Since angular momentum of the system is conserved the additional constraint equation that express conservation of the angular momentum is imposed on the system during part of the analysis (it can be treated as non-holonomic constraint as it involves kinematic constraint at the velocity level which cannot be directly transposed to the generalized positions level)

The equations of motion of the satellite were obtained symbolically starting from (45) using Mathematica programming package Forward dynamics of the satellite was performed for a duration time of 10 s with a variable step Runge-Kutta method of mixed order (4th and 5th) using an absolute tolerance of 1e-18 The purpose of this simulation is to obtain a reference trajectory which can be treated as the ldquoexactrdquo solution The maximum error on the total energy which should be constant is 11e-9 J and the maximum error on the total angular momentum is 16e minus 9 kgmiddotm2s Note that no constraint stabilization was performed in the process of obtaining reference trajectory After obtaining reference solution dynamic simulation was performed using a Euler first order numerical integration scheme and a fixed timestep of 001 s As Figure 5 shows the total angular momentum of the satellite (about the center of mass) is not constant and the error grows steadily These errors were eliminated during subsequent stabilized integration that utilized described projection algorithm by considering system constraint manifold defined via additional nonholonomic constraint equation of angular momentum conservation (Figure 5)

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

542 Mechanical system with mixed holonomic and non-holonomic constraints

The second numerical example focuses on the numerical forward dynamics of a snakeboard The mathematical model of the snakeboard is synthesized using descriptor formulation resulting in a mix of holonomic and nonholonomic constraints To investigate characteristics of optimized generalized coordinates partitioning procedure when holonomic and non-holonomic constraints are present in the system the separate partitioning at position level and velocity level is performed and analyzed It is shown that optimal selection of independent coordinates is not necessarily the same as the best set of independent velocities

The snakeboard is illustrated in Figure 6 It is modelled as a planar multibody system with 4 bodies connected to each other by means of pin-joints There is one coupler (B) two small boards with wheels (A and C) and one rotor (D) on the coupler to model dynamical excitation of human body torso motion The two pairs of wheels cannot slide and therefore impose two non-holonomic constraints on the system At the configuration level the snakeboard has 6 DOF

The snakeboard is mathematically modelled in descriptor form ie at the configuration level each component has 3 degrees of freedom and holonomic constraints are added to model the joints Therefore the mathematical model is synthesised using 12 coordinates and 6 holonomic constraints supplemented by additional 2 non-holonomic constraints

The chosen coordinates are (xi yi θi) representing the position of the center of mass and the global orientation of body i = A B C or D

During performing forward dynamic analysis the following scenario was considered The snakeboard is actuated by a force with constant amplitude acting on


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 05 rad by means of PD-controllers (P = 10 Nm D = 01 Nmiddotsm) 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 6 Snakeboard multibody modelSlika 6 Mehanički model lsquosnakeboard-arsquo

Figure 7 System reference generalized coordinatesSlika 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 and presented in Figure 8 (configuration coordinates projections are presented left and velocities are right)

tend

t=0

Figure 8 Relative length of projection for configuration coordinates and velocity coordinatesSlika 8 Relativna duljina projekcija baznih vektora na višestrukost ograničenja


As one can see there are clear differences between the lengths of relative projections at both levels Since the coordinate which has smaller projection at the respective constraint manifold would be a more appropriate choice for dependent integration parameter than one with longer projection it is clear that optimal choices for dependent position coordinates would differ from those for velocities

In the sequel the performance of constraint violation stabilization procedure based on optimal coordinates partitioning is discussed in the framework of the presented example For this purpose only constraint stabilization at velocity level is considered In order to determine and compare local integrationstabilization velocity errors the integration procedure was performed as follows The state vector of the reference trajectory has been integrated using fixed step Euler numerical integrator at any given time for one single step (001 s) with and without projective stabilization step and the difference between the stabilized and unstable solution has been calculated This procedure was followed twice with a different choice of dependent coordinates in the case of stabilized solution once with optimal stabilization (as shown in Figure 8) and once with a suboptimal set of dependent coordinates (the coordinate with smallest projection is replaced with the 7th smallest one)

The errors ε were calculated by integrating the weighted squared difference (using system metric M) in velocity between both sets of trajectories

ε = int (ΔqTMΔq)dt

The obtained result shows that error between the unstabilized fixed step solution and the optimal solution is 000003e-5 while the difference between unstabilized and the suboptimal solution is significantly larger and yields 047836e-5 The comparison between the errors shows that during integration step the distance from solution before (ie after unstable ODE step that violates constraints) and after constraint stabilization is smaller when dependent coordinate is chosen according to projective criterion As one could have expected this means that the additional stabilization step based on optimized partitioning shifts unstable ODE numerical solution back to the constraint manifold with controlled error along the manifold (see [38] for details) However as has been shown previously for non-holonomic systems the optimized coordinates partitioning should be done separately at position and velocity level

AcknowledgmentsThe authors acknowledge dr Joris Naudet (University

of Brussels VUB) for performing computations in the framework of presented numerical example

REFERENCES

[1] ANDERSON K An Order n Formulation for the Motion Simulation of General Multi-Rigid-Body Constrained Systems Computers amp Structures 43 (1992) 3 565-579

[2] BLAJER W SCHIEHLEN W and SCHIRM W A Projective Criterion to the Coordinate Partitioning Method for Multibody Dynamics Archive of Applied Mechanics 64 (1994) 86-98

[3] BLAJER W Elimination of Constraint Violation and Accuracy Aspects in Numerical Simulation of Multibody Systems Multibody System Dynamics 7 (2002) 265-284

[4] BROCKETT R W STOKES A PARK F C Geometrical Formulation of the Dynamical Equations Describing Kinematic Chains IEEE Int Conf on Robotics and Automation (1993) 637-642

[5] BULLO F LEWIS A D Geometric Control of Mechanical Systems Springer 2005

[6] DAI J S An historical review of the theoretical development of rigid body displacements from Rodrigues parameters to the finite twist Mech Mach Theory 41 (2006) 41-52

[7] FEATHERSTONE R The Calculation of Robot Dynamics Using Articulates-Body Inertias Int J Robotics Research 2 (1983) 1 13 - 30

[8] FIJANY A SHARF I DrsquoELEUTERIO G parallel O(log N) Algorithms for Computation of Manipulator Forward Dynamics IEEE Trans Rob and Automation 11 (1995) 3 389-399

[9] FUNDA J TAYLOR R H PAUL R P On homogenous transformations Quaternions and Computational efficiency IEEE Trans on Robotics and Automation 6 June (1990) 3 382ndash388

[10] HAIRER E Lubich C Wanner G Geometric Numerical Integration Springer 2002

[11] HERVE J M Intrinsic formulation of problems of geometry and kinematics of mechanisms Mech Mach Theory 17 (1982) 3 179-184

[12] LEWIS D SIMO J C Conserving Algortihms for the Dynamics of Hamiltonian Systems on Lie Groups J Nonlinear Sci 4 (2004) 253-299

[13] MAISSER P A Differential Geometric Approach to the Multibody System Dynamics ZAMM Z angew Math Mech 71 (1991) 4 116ndash119

[14] MCCARTHY J M Geometric Design of Linkages SpringerndashVerlag New York 2000

[15] MORAWIEC A Orientations and Rotations Springer 2004

[16] MUumlLLER A A Conservative Elimination Procedure for Redundant Closure Constraints in MBS Models Multibody Systems Dynamics Springer 16 (2006) 4 309-330

[17] MUumlLLER A Maisser P Lie group formulation of kinematics and dynamics of constrained MBS and its application to analytical mechanics Multibody System Dynamics 9 (2003) 311-352

tend

t=0


[18] MUumlLLER A Geometric characterization of the configuration space of rigid body mechanisms in regular and singular points 29th Mechanisms amp Robotics Conference ASME 2005 International Design Engineering Technical Conferences September 22-28 2005 Long Beach California USA

[19] MUumlLLER A On Non-Smooth Phenomena in the Kinematics of Rigid Body Mechanisms Multibody Dynamics 2007 ECCOMAS Thematic Conference 25-28 June 2007 Milano Italy

[20] MURRAY R M LI Z SASTRY S S A mathematical Introduction to robotic Manipulation CRC Press 1993

[21] MURRAY R M SASTRY S S Nonholonomic motion planing steering using sinusoids IEEE Trans Autom Control 38 (1993) 5 700ndash716

[22] ORTEGA R et al Passivity-based Control of Euler-Lagrange-Systems Springer 1998

[23] OSTROWSKI J Geometric Perspectives on the Mechanics and Control of Robotic Locomotion 7th Int Symp Robotics Research Munich Germany Oct 1995 in GGiralt G Hitzinger (Eds) Robotics Research SpringerndashVerlag 1996

[24] PARK F C BOBROW J E PLOEN S R A Lie Group Formulation of Robot Dynamics Int J Robotics Reserch 14 (1995) 6 609ndash618

[25] PARK F C Distance metrics on the rigid-body motions with applications to mechanism design ASME Journal of Mechanical Design 117 (1995) 1 48-54

[26] PARK F C CHOI J PLOEN S R Symbolic formulation of closed chain dynamics in independent coordinates Mech Mach Theory 34 (1999) 731-751

[27] PARK J CHUNG W-K Geometric Integration on Euclidean Group With Application to Articulated Multibody Systems IEEE Trans Robotics Vol 21 No 5 2005 pp 850-863

[28] PLOEN S R PARK F C Coordinate-Invariant Algorithms for Robot Dynamics IEEE Trans Robotics and Automation 15 (1999) 6 1130-1135

[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

[31] RICO J M RAVANI B On mobility analysis of linkages using group theory ASME Journal of Mechanical Design 125 March (2003) 70-80

[32] RODRIGUEZ G KREUTZ-DELGADO K Spatial operator factorization and inversion of the manipulator mass matrix IEEE Trans Robotics and Automation 8 (1992) 1 65 - 76

[33] SAHA S K SCHIEHLEN W O Recursive Kinematics and Dynamics for Parallel Structured Closed-Loop Multibody Systems Mech Struct amp Mach 29 (2001) 2 143-175

[34] SELIG J M Geometrical methods in robotics Springer 1996

[35] WIE B Space Vehicle Dynamics and Control AIAA 1998

[36] WOLF A SHOHAM M Screw theory tools for the synthesis of the geometry of a parallel robot for a given instantaneous task Mech Mach Theory 41 (2006) 656-670

[37] TERZE Z LEFEBER D Dynamic Simulation of Multibody Systems With no Constraints Violation Transactions of FAMENA 24 (2000) 1-9

[38] TERZE Z NAUDET J Geometric Properties of Projective Constraint Violation Stabilization Method for Generally Constrained Multibody Systems on Manifolds Multibody System Dynamics 20 (2008) 85-106

[39] TERZE Z et al Null Space Integration Method for Constrained Multibody System Simulation with no Constraint Violation Multibody System Dynamics 6 (2001) 229-243

[40] WEHAGE R A and HAUG E J Generalized Coordinate Partitioning for Dimension Reduction in Analysis of Constrained Dynamic Systems Journal of Mechanical Design 104 (1982) 247-255

[41] ZEFRAN M KUMAR V CROKE C Choice of Riemannian Metric for Rigid Body Kinematics Proc ASME 1996 Inte Design Engineering Technical Conferences August 19-22 1996 Irvine California USA 29


group of isometric orientation preserving transformations of the three-dimensional Euclidean space In essence the parameterization of screws with line coordinates gives rise to geometric treatment of differential-geometric entities Screw theoretic considerations also gave rise to the development of advanced methods for the design of mechanisms [14 25 36] Besides these geometric and kinematic applications explicit use of the Lie group structure admits to expressed motion equations of mechanisms in a very compact and elegant way [4 11 17 24 26 28] and also admits application of integration schemes on Lie groups [25] The difference of classical matrix formulations and recursive formulations [1 7-8 32-33] is that these formulations are entirely formulated in terms of screw quantities corresponding to the mechanismrsquos joint geometry (by virtue of the parameterization with line coordinates) Furthermore the coordinate invariance allows for a formulation without body-fixed reference frames and hence for the incorporation of CAD data This fact will be emphasized throughout the paper

The paper is organized as follows Section 2 deals with the geometry and summarizes the representation of configurations of rigid MBSs After recalling the graph representation of the MBS topology configurations are expressed as elements of the 3-dimensional special Euclidean group The configuration space is then parameterized using canonical (axisangle) and non-canonical parameters A formulation is given that does not 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 ie 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

2 MBS geometry

21 The topological graph of an MBS

The MBS topology is represented as an oriented graph Γ = (B J) where the set of vertices B represents the bodies and the set of edges J represents the joints of the

MBS It is assumed hereafter that Γ has one connected component In this case Γ has γ = m minus N fundamental loops (FLs) where m= |J| is the number of joints and N= |B| minus 1 Joints and bodies are respectively denoted by Jα α = 1 m and Bk k = 0 N Body B0 stands for the ground An edge JαisinΓ is an unordered pair Jαequiv (Bk Bl) (for brevity we write αisinΓ or (k l) isinΓ if Jα equiv (k l))

A spanning tree of Γ is denoted by G = (B JG) and the corresponding cotree by H (ΓG) = (B JH) or simply with H Vertex B0 is chosen as the root of the tree JG is the set of tree-joints and JH = J JG is the set of cut-joints G comprises N tree joints The term cut-joint indicates that if we rsquocutrsquo these joints we get a tree topology

Figure 1 An example for the graph description of a MBSSlika 1 Primjer graf-opisa diskretnog mehaničkog sustava s kinematičkim vezama

Let Gd be the directed tree wrt to the root B0 which is the directed subgraph of G such that to every vertex there is a path starting from the root of G (Figure 1) The directed tree induces the following ordering relation of joints and bodies

J1

J2

J3

J4

J5

J6

J7

J8

B1

B2

B3

B4

B5

B6B7J9











k

1

N

∆

α











C = C R α = J (k) (4)


α

α l

α k

α

α

α


α α

α

k k-1 α

1 N

0 1
















macr

macr

macr

α

α1 αv

α

Zqα1and

Zqανand

α

Xqα1and

Xqανand

αiS

α k αi

α α

Xqα1and

Xqανand

αi

αi


= eα1 αν

Q (qα) = eα1 αν (8)







(9)




Zqα1++and

Zqανand

Xqα1++and

Xqανand

α

αi α

α

α





Y = Adm X (10)



α = J (k) β = Jroot (k) (11)







k k

k β α-1 α

αi kαi

αi

k

kk

Yqβ1and

Yqβνand

Yqα1and

Yqανand

k

αi

αi

α α


S-1 C-1 C S = N (12)






g (q) = 0 (13)


V = q isin | g (q) =0 (14)


3 MBS Kinematics






H Q Qi iα

α αα= partminus1

(17)

kα1 l αk α

α

α

k

and

k k k

k

alen k

k

k m



DOF

SSG


(vrsta zgloba)


Subgroup Subgrupa

1


e12 = e2

3 = e13 = 0

p1 = p2 = 0times


e12 = e2

3 = 0p1 = p2 = p3 = 0

times


e12 = e2

3 = e13 = 0

p3 = 0times

2


e13 = e2

3 = 0p1 = p2 = 0

times


e12 = 0

p1 = p2 = p3 = 0-


2 = e23 = e1

3 = 0 times

3


e13 = e2

3 = 0p3 = 0

times


p1 = p2 = p3 = 0 times

4


e13 = e2

3 = 0 times



(18)



(19)


(20)





-1 X are the





(21)





k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv



(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0




































k

1

N

∆

α











C = C R α = J (k) (4)


α

α l

α k

α

α

α


α α

α

k k-1 α

1 N

0 1
















macr

macr

macr

α

α1 αv

α

Zqα1and

Zqανand

α

Xqα1and

Xqανand

αiS

α k αi

α α

Xqα1and

Xqανand

αi

αi


= eα1 αν

Q (qα) = eα1 αν (8)







(9)




Zqα1++and

Zqανand

Xqα1++and

Xqανand

α

αi α

α

α





Y = Adm X (10)



α = J (k) β = Jroot (k) (11)







k k

k β α-1 α

αi kαi

αi

k

kk

Yqβ1and

Yqβνand

Yqα1and

Yqανand

k

αi

αi

α α


S-1 C-1 C S = N (12)






g (q) = 0 (13)


V = q isin | g (q) =0 (14)


3 MBS Kinematics






H Q Qi iα

α αα= partminus1

(17)

kα1 l αk α

α

α

k

and

k k k

k

alen k

k

k m



DOF

SSG


(vrsta zgloba)


Subgroup Subgrupa

1


e12 = e2

3 = e13 = 0

p1 = p2 = 0times


e12 = e2

3 = 0p1 = p2 = p3 = 0

times


e12 = e2

3 = e13 = 0

p3 = 0times

2


e13 = e2

3 = 0p1 = p2 = 0

times


e12 = 0

p1 = p2 = p3 = 0-


2 = e23 = e1

3 = 0 times

3


e13 = e2

3 = 0p3 = 0

times


p1 = p2 = p3 = 0 times

4


e13 = e2

3 = 0 times



(18)



(19)


(20)





-1 X are the





(21)





k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv



(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0









































macr

macr

macr

α

α1 αv

α

Zqα1and

Zqανand

α

Xqα1and

Xqανand

αiS

α k αi

α α

Xqα1and

Xqανand

αi

αi


= eα1 αν

Q (qα) = eα1 αν (8)







(9)




Zqα1++and

Zqανand

Xqα1++and

Xqανand

α

αi α

α

α





Y = Adm X (10)



α = J (k) β = Jroot (k) (11)







k k

k β α-1 α

αi kαi

αi

k

kk

Yqβ1and

Yqβνand

Yqα1and

Yqανand

k

αi

αi

α α


S-1 C-1 C S = N (12)






g (q) = 0 (13)


V = q isin | g (q) =0 (14)


3 MBS Kinematics






H Q Qi iα

α αα= partminus1

(17)

kα1 l αk α

α

α

k

and

k k k

k

alen k

k

k m



DOF

SSG


(vrsta zgloba)


Subgroup Subgrupa

1


e12 = e2

3 = e13 = 0

p1 = p2 = 0times


e12 = e2

3 = 0p1 = p2 = p3 = 0

times


e12 = e2

3 = e13 = 0

p3 = 0times

2


e13 = e2

3 = 0p1 = p2 = 0

times


e12 = 0

p1 = p2 = p3 = 0-


2 = e23 = e1

3 = 0 times

3


e13 = e2

3 = 0p3 = 0

times


p1 = p2 = p3 = 0 times

4


e13 = e2

3 = 0 times



(18)



(19)


(20)





-1 X are the





(21)





k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv



(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0






























Y = Adm X (10)



α = J (k) β = Jroot (k) (11)







k k

k β α-1 α

αi kαi

αi

k

kk

Yqβ1and

Yqβνand

Yqα1and

Yqανand

k

αi

αi

α α


S-1 C-1 C S = N (12)






g (q) = 0 (13)


V = q isin | g (q) =0 (14)


3 MBS Kinematics






H Q Qi iα

α αα= partminus1

(17)

kα1 l αk α

α

α

k

and

k k k

k

alen k

k

k m



DOF

SSG


(vrsta zgloba)


Subgroup Subgrupa

1


e12 = e2

3 = e13 = 0

p1 = p2 = 0times


e12 = e2

3 = 0p1 = p2 = p3 = 0

times


e12 = e2

3 = e13 = 0

p3 = 0times

2


e13 = e2

3 = 0p1 = p2 = 0

times


e12 = 0

p1 = p2 = p3 = 0-


2 = e23 = e1

3 = 0 times

3


e13 = e2

3 = 0p3 = 0

times


p1 = p2 = p3 = 0 times

4


e13 = e2

3 = 0 times



(18)



(19)


(20)





-1 X are the





(21)





k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv



(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0




























DOF

SSG


(vrsta zgloba)


Subgroup Subgrupa

1


e12 = e2

3 = e13 = 0

p1 = p2 = 0times


e12 = e2

3 = 0p1 = p2 = p3 = 0

times


e12 = e2

3 = e13 = 0

p3 = 0times

2


e13 = e2

3 = 0p1 = p2 = 0

times


e12 = 0

p1 = p2 = p3 = 0-


2 = e23 = e1

3 = 0 times

3


e13 = e2

3 = 0p3 = 0

times


p1 = p2 = p3 = 0 times

4


e13 = e2

3 = 0 times



(18)



(19)


(20)





-1 X are the





(21)





k

k Adα k-1

α α αi

α α αi

α k

αi

Xqα1and

Xqανand

k k m αi

k

and and and

αi

k k αi

Yqα1and

Yqανand Yqβ1

andYqβνand

α1αv



(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0




























(22)


) and

(23)




V = Kq (24)



(25)





if α=β


(26)

1

k

2 1

1kndash1

kndash11 2k k k

2 1

kndash1k

i indash1









] = AdCndash1C

[KαiHβj



(29)



(31)

(32)



k k m

k k

k k k

and and

kk k k

b b c

k k k m

m k m m

and

and and

1

k

1i

ki

d




(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0





























(34)




(36)







middot

and and and

eα2 eαv α1


αvndash1eαv

middot

α k klk

l

middot

middotmiddotmiddot



DOF SSG


(vrsta zgloba)


1


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0

11

1

0 00 0 0 0 01


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

110

0 0 0 0 0


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

0 1

2


1 0 0 0 0 00 0 0 0 00 0 0 0 00 0 0 0

11

1 0


1 0 0 0 0 00 0 0 0 00 0 0 0 0

1

1


0 0 1 0 0 00 0 0 1 0 00 0 0 0 10


0 0 0 0 00 0 0 0 00 0 0 0 0

11

1

4


0 0 0 1 0 00 0 0 0 10

middot middot

=



4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0




























4 MBS Dynamics




(39)



(40)


(41)

where

(42)





mdashk

mdashminusk k

k k

kk


mdashminus minus

k kk

k

mdashk

mdashk

k

form

with the symmetric

(43)




(44)





first kind

(46)




(48)



G(q) q+ C (q q) q = Q (49)

where




k k k

middot

middot middot



k

~k k k k

minus minus


middot middot

middot

g

g

g

g

gg g

g

~

=

=

=

=






















G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0















































G(q) q2 + C (q q) q2 + N (q) = Q (58)

where







51 ODE integration




x f x f xv

G Q= =

minus( ) ( )

1 (62)


52 DAE integration


middot

middot1 k

minus minus minus

k

1 N

middot middot

amiddot

a

a a1 a2

a1 a2 aa2

a2a

middot a2middot

middot middot


a2 a2

~a1 b a2 b2 c2

~a1 c1

aprime2



a middot

b2b2 c2c2


middotmiddot middot

middot middot ndash

ndashndash

~

middot

ndashndash






(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0































(63)





















middot

middotmiddot

middotmiddot

G ΦqT

Φq 0



















(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0












































(67)

(68)








middot


















tend

t=0










REFERENCES


















tend

t=0











































tend

t=0










REFERENCES


















tend

t=0


































REFERENCES


















tend

t=0


























Differential-Geometric Modelling and Dynamic Simulation of ... · Kinematika sustava...

Documents

Transcript of Differential-Geometric Modelling and Dynamic Simulation of ... · Kinematika sustava...