Vrije Universiteit Brussel - pdfs. · PDF fileProf. dr. ir. Herman Bruyninckx (Katholieke...

Vrije Universiteit Brussel

Faculteit Ingenieurswetenschappen

Vakgroep Toegepaste Mechanica

Forward Dynamics

of Multibody Systems:

A Recursive Hamiltonian Approach

Joris Naudet

Proefschrift voorgelegd tot het behalen van de academische graad

van Doctor in de Toegepaste Wetenschappen

September 2005

Vrije Universiteit Brussel

Faculteit Ingenieurswetenschappen

Vakgroep Toegepaste Mechanica

Forward Dynamics

of Multibody Systems:

A Recursive Hamiltonian Approach

Joris Naudet

September 2005

Jury:Prof. dr. ir. Dirk Lefeber (Vrije Universiteit Brussel), promotor

Prof. dr. ir. Patrick Kool (Vrije Universiteit Brussel), co-promotorProf. Zdravko Terze (University of Zagreb)

Prof. dr. ir. Herman Bruyninckx (Katholieke Universiteit Leuven)Prof. Jean-Claude Samin (Universite catholique de Louvain )

Prof. dr. ir. Jean Vereecken (Vrije Universiteit Brussel), vice-chairmandr. ir. Bjorn Verrelst (Vrije Universiteit Brussel), secretary of the chairman

Prof. dr. ir. Patrick De Wilde (Vrije Universiteit Brussel), chairman

to Mum

Preface

The increase in processing power and the theoretical breakthroughs achieved inmultibody systems dynamics have improved the usefulness of dynamic simulationsto such an extent that the development of a whole range of applications has beentriggered. Dynamic simulations are used for the analysis of mechanisms, for virtualprototyping, simulators, computer animation, advanced control, etc. and are gain-ing in popularity. This thesis proposes a method for formulating the equations ofmotion of multibody systems with the purpose of further improving the efficiencyof dynamic simulations. The proposed algorithm is recursive and is based on a setof Hamiltonian equations.

Contents

The core of the thesis describes a recursive algorithm to obtain a set of Hamiltonianequations of motion of multibody systems. The reader who is not familiar withmultibody system analysis, and the more experienced reader who would like torefresh his memory on the matter, can take a look at the appendix. It compriseschapters about kinematics (A), dynamics (B) and numerical integration methods(C). The last part of the appendix (D) contains the heavy calculations that wereskipped in the main text.

The thesis is divided into 7 chapters. Chapter 1 is an overview of the applicationsand the state of the art in the field of multibody dynamics. Chapter 2 introducesHamilton’s equations of motion. In chapter 3, the Hamiltonian equations of motionof a single rigid body are described using the concept of momentum vector. Chap-ter 4 is about open-loop multibody systems. Using the Hamiltonian equations ofmotion of each individual body, it is explained how the Hamiltonian equations ofmotion of the full system can be obtained recursively. This method is extended toconstrained systems in chapter 5. Two different approaches are discussed: the aug-mented Lagrangian method and a method based on coordinate reduction. Chapter

i

6 discusses the use of the recursive algorithm in combination with impact prob-lems and systems with changing topology. In chapter 7, numerical examples arediscussed to demonstrate the efficiency of the algorithm. Finally, conclusions aredrawn in chapter 8.

Acknowledgements

It is not an easy task to thank all the persons who, in one way or another, con-tributed to this thesis: there are so many I am afraid to forget one. There is ofcourse the person it all started with: my promotor. When I asked him whetherhe had some ideas for a PhD-thesis at the end of my studies, he answered: “Well,I was working on something”. He opened a drawer, took a piece of paper withsome scribbles on it and handed it over. Today, the scribbles have grown to thiswork. I would hereby like to thank Prof. Dirk Lefeber for providing me the basicidea of this thesis, giving me the opportunity to work on it for four years and forhis constructive comments on the text. I would like to thank the colleagues fromthe research group of Robotics and multibody dynamics. Special thanks to MichaelVan Damme, for the interesting discussions, for answering a lot of questions abouta lot of things and for reading the thesis; Bjorn Verrelst and Bram Vanderborghtfor their everlasting enthusiasm and for detecting an error in my code, Bjorn alsofor his comments on the text; Ronald Van Ham and Jimmy Vermeulen for puttinglife in the brewery and making work more fun. I would like to thank Prof. FrankDaerden and my co-promotor Prof. Patrick Kool as well, for constructive discus-sions and good advice, Prof. Kool for corrections on the thesis too. I would alsolike to thank all the other colleagues from the department of applied mechanics, itwas a real pleasure working with them. Especially Gert De Sitter, whom I’d liketo thank for constantly emptying my snacks provisions so I had regularly to go tothe store across the street, which permitted me to have some exercise and breathsome fresh air, which supposedly had some good influence on my mental health.

I’d also like to thank Prof. Patrick De Ryck, Maarten Ottoy and Raf Godart fromGroep-T in Leuven, for showing interest in my research and developing a graphicaluser interface for my code. I would like to thank Prof. Zdravko Terze for invitingme to stay for two months in Zagreb and teaching me the concept of manifolds anddifferential-geometric approaches. I am grateful for the interesting discussions onnumerical integrations methods I had with Prof. Pieter De Groen.

I would further like to thank the Institute for the Promotion of Innovation byScience and Technology in Flanders (IWT) for funding this research. My computerfor not crashing at the wrong moments (not at all actually, at least not yet). Andlast but not least: my mum, my brothers, and all my friends and family for helpingme not to think about canonical momenta and all these awkward equations all thetime. And Ploefke of course.

Brussels, Joris NaudetJune 2006

Contents

Preface i

Nomenclature xv

1 INTRODUCTION 1

1.1 STATE OF THE ART IN MULTIBODY SYSTEM ANALYSIS . . . 1

1.2 CONTRIBUTIONS OF THIS THESIS . . . . . . . . . . . . . . . . . 3

1.3 APPLICATIONS: NEED FOR SPEED . . . . . . . . . . . . . . . . 5

1.3.1 SIMULATIONS . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.3.2 VIRTUAL PROTOTYPING . . . . . . . . . . . . . . . . . . 7

1.3.3 CONTROL . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.3.4 ANIMATION MOVIES . . . . . . . . . . . . . . . . . . . . . 10

1.3.5 PHYSICS BASED COMPUTER GAMES . . . . . . . . . . . 12

2 HAMILTONIAN EQUATIONS 15

3 RIGID BODY AND CANONICAL MOMENTA 21

3.1 MOMENTUM VECTOR . . . . . . . . . . . . . . . . . . . . . . . . 21

3.2 KINETIC ENERGY . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.3 CANONICAL MOMENTA . . . . . . . . . . . . . . . . . . . . . . . 23

3.3.1 EXAMPLES . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.4 NEWTON-EULER EQUATIONS FOR ASINGLE RIGID BODY IN LOCAL AXES . . . . . . . . . . . . . . . 29

3.5 JOINT VELOCITIES . . . . . . . . . . . . . . . . . . . . . . . . . . 30

v

3.6 HAMILTONIAN EQUATIONS . . . . . . . . . . . . . . . . . . . . . 30

3.7 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4 OPEN-LOOP MULTIBODY SYSTEMS 33

4.1 TERMINOLOGY OF AN OPEN-LOOP MULTIBODY SYSTEM . 33

4.2 CANONICAL MOMENTA . . . . . . . . . . . . . . . . . . . . . . . 34

4.3 TRANSFORMATION OPERATORS . . . . . . . . . . . . . . . . . 36

4.4 ARTICULATED MOMENTUM VECTOR . . . . . . . . . . . . . . 37

4.4.1 EXAMPLE . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.5 EQUATIONS OF MOTION . . . . . . . . . . . . . . . . . . . . . . . 37

4.5.1 NEWTON-EULER FORMALISM . . . . . . . . . . . . . . . 38

4.5.2 PRINCIPLE OF VIRTUAL POWER . . . . . . . . . . . . . 41

4.5.3 JOINT FORCES . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.6 JOINT VELOCITIES . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.7 JOINT ACCELERATIONS AND JOINT REACTION FORCES . . 44

4.7.1 TIME DERIVATIVE OF THE ARTICULATED MOMEN-TUM VECTOR . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.7.2 JOINT REACTIONS . . . . . . . . . . . . . . . . . . . . . . 47

4.8 ALGORITHM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.9 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5 CONSTRAINED MULTIBODY SYSTEMS 51

5.1 TERMINOLOGY OF A CLOSED-LOOPMULTIBODY SYSTEM . . . . . . . . . . . . . . . . . . . . . . . . . 52

5.2 METHOD 1: AUGMENTED LAGRANGIAN . . . . . . . . . . . . . 52

5.2.1 AUGMENTED MOMENTUM VECTOR . . . . . . . . . . . 53

5.2.2 JOINT AND SPATIAL VELOCITIES . . . . . . . . . . . . . 55

5.2.3 RESOLVING THE CONSTRAINTS . . . . . . . . . . . . . . 58

5.2.4 GENERAL APPROACH . . . . . . . . . . . . . . . . . . . . 59

5.2.5 SUMMARY OF THE METHOD . . . . . . . . . . . . . . . . 60

5.3 METHOD 2: MINIMAL COORDINATES . . . . . . . . . . . . . . . 62

5.3.1 EXAMPLE 1: DEPENDENT COORDINATES AT ENDOF UNDERLYING OPEN-LOOP STRUCTURE . . . . . . . 63

5.3.2 EXAMPLE 2: ARBITRARY SELECTION OF INDEPEN-DENT COORDINATES . . . . . . . . . . . . . . . . . . . . . 71

5.3.3 GENERAL APPROACH . . . . . . . . . . . . . . . . . . . . 78

5.4 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

6 IMPACTS, CONTACTS AND TIME-VARYING TOPOLOGIES 83

6.1 COLLISION DETECTION . . . . . . . . . . . . . . . . . . . . . . . 83

6.2 IMPACT THEORY . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

6.2.1 IMPACT EQUATIONS AND RECURSIVE ALGORITHM . 86

6.3 CONTACTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

6.4 TIME-VARYING TOPOLOGY . . . . . . . . . . . . . . . . . . . . . 89

6.5 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

7 NUMERICAL EXAMPLES 93

7.1 HARDWARE DESCRIPTION . . . . . . . . . . . . . . . . . . . . . 93

7.2 SIMULATION SPEEDS . . . . . . . . . . . . . . . . . . . . . . . . . 94

7.3 CANONICAL MOMENTA VERSUS ACCELERATIONS . . . . . . 99

7.3.1 MODEL DESCRIPTION . . . . . . . . . . . . . . . . . . . . 99

7.3.2 NUMERICAL INTEGRATION WITH FIXED STEP . . . . 100

7.3.3 NUMERICAL INTEGRATION WITH ADAPTIVE STEPS . 103

7.4 TOPOLOGY CHANGE . . . . . . . . . . . . . . . . . . . . . . . . . 106

7.5 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

8 GENERAL CONCLUSIONS 109

A KINEMATICS 113

A.1 POSITION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113

A.2 ORIENTATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114

A.2.1 ROTATION MATRIX . . . . . . . . . . . . . . . . . . . . . . 115

A.2.2 EULER ANGLES . . . . . . . . . . . . . . . . . . . . . . . . 116

A.2.3 QUATERNIONS . . . . . . . . . . . . . . . . . . . . . . . . . 118

A.2.4 CONCLUSION . . . . . . . . . . . . . . . . . . . . . . . . . . 122

A.3 HOMOGENEOUS TRANSFORMATION . . . . . . . . . . . . . . . 123

A.4 CONFIGURATION SPACE . . . . . . . . . . . . . . . . . . . . . . . 123

A.4.1 CARTESIAN COORDINATES . . . . . . . . . . . . . . . . . 124

A.4.2 JOINT OR RELATIVE COORDINATES . . . . . . . . . . . 124

A.4.3 KINEMATIC CONSTRAINTS . . . . . . . . . . . . . . . . . 125

A.5 FORWARD KINEMATICS . . . . . . . . . . . . . . . . . . . . . . . 126

A.6 INVERSE KINEMATICS . . . . . . . . . . . . . . . . . . . . . . . . 127

A.7 VELOCITIES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128

A.7.1 VELOCITY FIELD IN A RIGID BODY . . . . . . . . . . . 128

A.7.2 JOINT MATRIX . . . . . . . . . . . . . . . . . . . . . . . . . 129

A.8 ACCELERATIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

A.9 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

B DYNAMICS 135

B.1 STATE SPACE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135

B.2 DIFFERENT FORMALISMS . . . . . . . . . . . . . . . . . . . . . . 136

B.2.1 NEWTON-EULER EQUATIONS . . . . . . . . . . . . . . . 137

B.2.2 D’ALEMBERT’S PRINCIPLE . . . . . . . . . . . . . . . . . 141

B.2.3 LAGRANGIAN EQUATIONS . . . . . . . . . . . . . . . . . 142

B.2.4 HAMILTONIAN EQUATIONS . . . . . . . . . . . . . . . . . 146

B.2.5 GIBBS-APPELL . . . . . . . . . . . . . . . . . . . . . . . . . 147

B.3 CHOICE OF STATE COORDINATES . . . . . . . . . . . . . . . . 147

B.4 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148

C NUMERICAL INTEGRATION OF THE EQUATIONS OF MO-

TION 149

C.1 EXPLICIT METHODS . . . . . . . . . . . . . . . . . . . . . . . . . 150

C.1.1 RUNGE-KUTTA METHODS . . . . . . . . . . . . . . . . . . 150

C.2 STIFF SYSTEMS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152

C.3 IMPLICIT METHODS . . . . . . . . . . . . . . . . . . . . . . . . . 153

C.3.1 BACKWARD EULER METHOD . . . . . . . . . . . . . . . 153

C.3.2 OTHER IMPLICIT METHODS . . . . . . . . . . . . . . . . 154

C.4 MULTISTEP METHODS . . . . . . . . . . . . . . . . . . . . . . . . 154

C.4.1 ADAMS-BASHFORTH/MOULTON METHODS . . . . . . . 155

C.5 ERROR CONTROL . . . . . . . . . . . . . . . . . . . . . . . . . . . 155

C.5.1 EMBEDDED METHODS . . . . . . . . . . . . . . . . . . . . 156

C.5.2 STEP DOUBLING METHOD . . . . . . . . . . . . . . . . . 156

C.6 SOLVING DAEs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156

C.6.1 DAE SOLVERS . . . . . . . . . . . . . . . . . . . . . . . . . 157

C.7 CONSTRAINT VIOLATION ERRORS . . . . . . . . . . . . . . . . 158

C.7.1 COORDINATE PARTITIONING METHOD . . . . . . . . . 158

C.7.2 PENALTY FORMULATIONS . . . . . . . . . . . . . . . . . 159

C.7.3 PROJECTIVE METHODS . . . . . . . . . . . . . . . . . . . 160

C.8 CONSIDERATIONS FOR REALTIME SIMULATIONS . . . . . . . 161

C.9 CONCLUSION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161

D ADDITIONAL CALCULATIONS 163

D.1 CALCULUS OF VARIATIONS . . . . . . . . . . . . . . . . . . . . . 163

D.2 BASIC VECTOR FORMULAS . . . . . . . . . . . . . . . . . . . . . 164

D.2.1 PROPERTIES OF SKEW-SYMMETRIC MATRICES . . . . 164

D.2.2 DOUBLE VECTOR PRODUCTS . . . . . . . . . . . . . . . 164

D.3 DERIVATION IN MOVING REFERENCE FRAMES . . . . . . . . 165

D.4 DERIVATION OF VECTORS AND MATRICES . . . . . . . . . . . 165

D.4.1 DERIVATION OF TRANSFORMATION OPERATORS . . 165

D.4.2 DERIVATION OF TRANSFORMED QUANTITIES . . . . 166

D.4.3 EQUATIONS OF MOTION OF (CONSTRAINED) ARTI-CULATED BODY . . . . . . . . . . . . . . . . . . . . . . . . 167

D.5 REDUCING THE CONSTRAINTEQUATIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168

D.6 PARTIAL DERIVATIVES OF THE SPATIAL VELOCITIES . . . . 170

D.7 CONSTRAINED MOMENTUM VECTOR . . . . . . . . . . . . . . 173

Bibliography 175

List of Figures

1.1 The Canadarm2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.2 NADS vehicle simulation . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.3 Liebherr’s TI 272 ultra-class haul truck . . . . . . . . . . . . . . . . 8

1.4 Boo and Sully in Pixar’s Monsters Inc. movie . . . . . . . . . . . . . 11

1.5 Screenshot from Valve’s Half-Life2 . . . . . . . . . . . . . . . . . . . 12

2.1 Sir William Rowan Hamilton (1805-1865) . . . . . . . . . . . . . . . 16

3.1 Rigid body connected to the environment by means of a pin-joint . . 25

3.2 Rigid body connected to the environment by means of a sliding pin-joint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.3 Floating rigid body . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.1 Terminology of a tree structure . . . . . . . . . . . . . . . . . . . . . 35

4.2 Planar double pendulum . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.3 Reaction forces and torques on body K . . . . . . . . . . . . . . . . 40

4.4 Two systems with same total mass and length . . . . . . . . . . . . . 45

5.1 Terminology of a closed-loop multibody system . . . . . . . . . . . . 53

5.2 Chain with both ends fixed . . . . . . . . . . . . . . . . . . . . . . . 54

5.3 Four-bar mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

5.4 Selection of coupled dependent bodies . . . . . . . . . . . . . . . . . 82

6.1 Different hierarchical levels of representation. . . . . . . . . . . . . . 84

6.2 Impact of two bodies A and B . . . . . . . . . . . . . . . . . . . . . 85

xi

6.3 Breaking of a joint. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

7.1 Chain with 100 links. . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

7.2 Tree system with 150 links. . . . . . . . . . . . . . . . . . . . . . . . 95

7.3 Closed loop with 100 links. . . . . . . . . . . . . . . . . . . . . . . . 95

7.4 Simulation time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

7.5 Number of function evaluations . . . . . . . . . . . . . . . . . . . . . 97

7.6 Formulation time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

7.7 3D-chain with 20 links . . . . . . . . . . . . . . . . . . . . . . . . . . 98

7.8 Model of the biped. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

7.9 Generalized Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . 100

7.10 Coordinates velocities . . . . . . . . . . . . . . . . . . . . . . . . . . 101

7.11 Canonical momenta . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

7.12 Constraint violation errors for δt=0.001s . . . . . . . . . . . . . . . . 103

7.13 Error on the total energy . . . . . . . . . . . . . . . . . . . . . . . . 104

7.14 Number of iterations, full error control . . . . . . . . . . . . . . . . . 105

7.15 Number of iterations, partial error control . . . . . . . . . . . . . . . 106

7.16 Simulation of a breaking chain with 100 links. . . . . . . . . . . . . . 107

A.1 Global and local reference frames . . . . . . . . . . . . . . . . . . . . 114

A.2 Leonhard Euler (1707-1783) . . . . . . . . . . . . . . . . . . . . . . . 116

A.3 Decomposition of an arbitrary rotation into three primitive rotationsusing Euler angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118

A.4 Brougham bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118

A.5 Planar double pendulum . . . . . . . . . . . . . . . . . . . . . . . . . 124

A.6 Serial robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127

A.7 Velocity distribution of a rigid body . . . . . . . . . . . . . . . . . . 129

B.1 Sir Isaac Newton (1643-1727) . . . . . . . . . . . . . . . . . . . . . . 136

B.2 Jean Le Rond D’Alembert (1717-1783) . . . . . . . . . . . . . . . . . 141

B.3 Joseph-Louis Lagrange (1736-1813) . . . . . . . . . . . . . . . . . . . 143

C.1 Gradient projective methods . . . . . . . . . . . . . . . . . . . . . . 160

List of Tables

4.1 Number of required operations . . . . . . . . . . . . . . . . . . . . . 49

7.1 Fixed step integration with RKF45 method . . . . . . . . . . . . . . 102

7.2 Adaptive step integration with two different solvers . . . . . . . . . . 104

7.3 Adaptive step size with RKF45 method (tol.=10−6) . . . . . . . . . 105

7.4 Distribution of truncation errors among the state coordinates . . . . 106

7.5 Processing times. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

xiii

Nomenclature

Acronyms

AOB Adjacent Outboard Bodies (children)CJ Constraint JointDAE Differential Algebraic EquationsDOF Degrees Of FreedomIRK Implicit Runge-KuttaMBS MultiBody SystemOCJ Outboard Constraint JointODE Ordinary Differential EquationsRK Runge-KuttaRKF Runge-Kutta-Fehlberg

Greek

ǫ quaternion of Euler parametersλ Lagrange multiplier (associated to reaction forces)π impulseρ pure quaternionσ Lagrange multiplier (associated to reaction impulses)Ω spatial velocityω angular velocity

xv

xvi NOMENCLATURE

Roman

1k Unity vector along axis kAsc(i) set of ascendants of body iAsc set ascendants of body i, including body iC Constraint matrixE Joint matrixEC Joint constraint matrixEr Rotational part of the joint matrixEt Translational part of the joint matrixf force vectorg gravitational fieldCh(i) set of children of body idAsc(i) set of dependent ascendants of body iDep set of all dependent coordinatesDes(i) set of all descendants of iDes set descendants of body i, including body iG Center of mass of rigid bodyH homogeneous transformation matrixI Unity dyadic, dimension depending on contextiDes(i) set of all independent descendants of body iInd set of all independent coordinatesJ Tensor of inertiaK Tag of arbitrary bodyL Tag of (auxiliary) arbitrary bodyM Mass matrix of a rigid body

or generalized mass matrix of multibody system (see context)M∗ Articulated mass matrixm Mass

or number of constraint equationsN Tag of leaf bodyn number of degrees of freedom0 Zero dyadic, size depending on contextO Reference point and origin of local reference frameP momentum vectorp vector of canonical momentapd dependent canonical momentaPa(i) Parent of body ipa angular momentum vectorpl linear momentum vectorq vector of system coordinatesq generalized velocities, also called coordinate or joint velocitiesR rotation matrix

NOMENCLATURE xvii

RS spatial rotation matrixv linear velocityT Kinetic energyT spatial forceT F force shift dyadicT V velocity shift dyadict torque vector

Subscript

PK variable P associated to body K

Superscript

∗ related to articulated body+ related to augmented form(+) denotes the time just after an impact(−) denotes the time just before an impact′ intermediate quantityc related to constrained bodyKr column vector containing the components of vector r projected

in reference frame KdK

dt time derivative with respect to reference frame KT transposed vector/matrixv virtual (joint or spatial) velocity

Other

ω ω×

Chapter 1

INTRODUCTION

“Canonical:reduced to the simplest

and most significant form possiblewithout loss of generality”

-Collaborative International Dictionary of English

Giving the definition of a multibody system is only a matter of due diligence, thename really says it all. As soon as a mechanical system comprises more than onecomponent, it is said to be a multibody system (Mehrkorpersysteme in German,systemes multicorps in French, sistemas multicuerpo in Spanish). The componentsof a multibody system can be rigid or flexible, although only rigid bodies are con-sidered in this thesis. The bodies are usually interconnected by means of jointssuch as pin-joints, sliders or spherical joints, but it is not a prerequisite to qualifyfor the title. The term multibody encompasses a whole set of systems rangingfrom simple mechanisms to cars and other ground vehicles, airplanes, heavy ma-chinery, the human body, robots, spacecrafts and the like. The forward dynamicsof multibody systems deals with the study of the motion of mechanisms under in-fluence of external forces. Fast methods for solving the forward dynamics problemare gaining importance in applications such as virtual prototyping, animation andsimulators. There exists a plethora of methods, but the algorithm proposed in thisthesis distinguishes itself from the others by the use of canonical momenta, whicheventually results in a set of Hamiltonian equations.

1.1 STATE OF THE ART IN MULTIBODY SYS-TEM ANALYSIS

The simulation of multibody systems comprises two essential steps: formulating theequations of motion and solving the equations of motion. Both steps are obviously

1

2 CHAPTER 1

connected and the efficiency of the simulation method will not only depend on theefficiency of both individual steps but also on the way they interact. A third stepthat is required for most practical cases is taking care of events. For instance, whena collision occurs between two objects, the numerical integration process must behalted and the impact equations need to be solved. Thereafter, the numericalintegration can be restarted. Similarly, the equations of motion may need to bealtered after a contact is broken.

The formulation of the equations of motion can be done based on the Newton-Euler equations, the principle of d’Alembert, the Lagrangian or Hamiltonian equa-tions, the Gibbs-Appell equations and many other variants. One of the most crucialchoices is whether to use independent or dependent coordinates. In the case of de-pendent coordinates, an additional choice imposes itself about the way to treat theconstraints. Expressing the equations of motion in a minimal set of independentcoordinates results in less differential equations, which are however more coupledand usually exhibit stronger non-linearities [de Jalon & Bayo, 1994], compared tothe non-minimal formulations. The advantage is that no constraint equations arerequired and a set of ordinary differential equations (ODE) must be solved. Thisis convenient because the ODE-theory is well understood and a wide range ofsolvers is at disposition. Non-minimal formulations on the other hand, result inmixed differential algebraic equations (DAE), but are in general much easier toestablish. Differential-algebraic equations can be solved directly using specializedDAE-solvers or can be reduced to ODEs by differentiating the constraint equations.The latter choice requires stabilization methods to make sure the constraints arenot violated. Further considerations that will affect the efficiency of the simulationinvolve the choice between a purely numerical method and a symbolic method, andbetween a global and a recursive (topological) approach.

The literature about the formulation of the equations of motion of multibodysystems is well established and many good reference books are available. Thereader in search for a comprehensive work on the matter may find satisfactionin [Wittenburg, 1977], [Kane & Levinson, 1985], [Roberson & Schwertassek, 1988],[Haug, 1989] or [Shabana, 1989]. The composite inertia method is one of the mostefficient ones for serial systems with few degrees of freedom [Walker & Orin, 1982].It is a method of order 3, also denoted O(n3), which means that the numberof calculations required to obtain the equations of motion is proportional to thethird power of the number of degrees of freedom. For systems with a largeramount of bodies, recursive approaches are a better choice [Vereshchagin, 1974,Armstrong & Green, 1985]. A widely used recursive method is the articulated in-ertia method [Featherstone, 1987]. It is a O(n) method, so the cost is linearlyproportional to the complexity. Featherstone showed that the articulated inertiamethod outperforms the composite inertia method as soon as the system com-prises at least 9 bodies. Moreover, the composite inertia method is prone to for-mulation stiffness, which means it may not behave very well during numericalintegration [Ascher et al., 1997]. After the introduction of the articulated iner-

INTRODUCTION 3

tia method, a myriad of different O(n)-algorithms emerged [Bae & Haug, 1987,Rosenthal, 1990, Rein, 1993, Vukobratovic et al., 1994]. However, these methodswere tailored for serial systems (including branched systems). Closed-loop sys-tems seemed more difficult to handle. The classic recursive approaches couldbe extended to constrained systems, but at the expense of the O(n)-efficiency[Lilly & Orin, 1994, Bae et al., 1987]. Baraff introduced a method based on La-grange multipliers [Baraff, 1996]. His method solves open-loop systems with O(n)-efficiency, but closed-loop systems with O(nm + m2 + m3), m being the number ofloop constraints. More sophisticated methods using coordinate reductions are ableto tackle closed-loop systems in a O(n)-way [Stejskal & Valasek, 1996]. Certainprecautions are needed however, because the coordinate reductions are prone tosingularities [Critchley & Anderson, 2003].

Besides designing more efficient methods for formulating and solving the equationsof motion, there is an obvious third way of increasing the simulation speed: hard-ware. Faster processors, memory chips, graphics processing units and so on havedirect effects on simulation times. Software implementation is also important: opti-mization of the code and (cache) memory usage are important factors. Significantperformance boosts can be obtained by implementing the simulation algorithmon a parallel processing architecture. This can be a computer with several pro-cessors, specialized hardware such as FPGAs (Field Programmable Gate Array)or a network of computers [Bae et al., 1988, Zomaya, 1992, Featherstone, 1999,Critchley & Anderson, 2004].

One of the most important challenges nowadays is to combine phenomena fromdifferent fields of physics into one simulator (multiphysics): multibody mechanics,thermodynamics, solid mechanics, aerodynamics, electronics,... Many real multi-body systems are influenced by several phenomena and require such a simulator toobtain accurate results.

1.2 CONTRIBUTIONS OF THIS THESIS

Most methods for formulating the equations of motion of multibody systems arebased on accelerations: whether the Newton-Euler equations, the Lagrangian equa-tions or the principle of virtual work or virtual power are used, these meth-ods provide a set of second order differential equations and the simulation al-gorithms come down to calculating and integrating accelerations [Jerkovsky, 1978,Kane & Levinson, 1985, Featherstone, 1987, Haug, 1989, Vukobratovic et al., 1994,Baraff, 1996].

A set of Hamiltonian equations does not contain accelerations, but is expressed interms of canonical momenta and their time derivatives instead, resulting in twice asmany first order differential equations. Although Hamiltonian equations are well-known amongst physicists, they are not widespread in the multibody community.The lack of interest can be attributed to several reasons. First of all: canonical

4 CHAPTER 1

momenta are often deprived of an intuitive physical interpretation. It is much morecomfortable to work with accelerations. Secondly, there exist very efficient methodsto formulate acceleration-based equations and it may seem pointless to use a less ef-ficient algorithm for the mere pleasure of handling Hamiltonian equations. Indeed,why would one bother to use a formulation based on canonical momenta anyway?The motivation for using Hamiltonian equations can be found in the numericalintegration of constrained multibody systems. Adding algebraic constraints tothe differential equations results in a set of differential-algebraic equations (DAE),which is characterized by a so-called differential index. This differential index isrelated to the number of times the constraint equations need to be differentiatedin order to eliminate the Lagrange multipliers. Hamilton’s equations have an in-dex of 2. Acceleration-based algorithms have index 3. It is a well-establishedfact that index 3 DAEs are more difficult to solve than systems with a lowerindex [Brenan et al., 1989, Baumgarte, 1983]. Moreover, the benefits of Hamil-ton’s equations for constrained multibody systems have been reported repeatedlyin the multibody literature [Lankarani & Nikravesh, 1988, Bayo & Avello, 1994,Ruhoff et al., 1996].

In this thesis, a recursive O(n) algorithm to obtain a set of Hamiltonian equationsis presented. It allows to compete with O(n) acceleration-based algorithms, whileexploiting the benefits of the lower differential index. It even outperforms thecomparable acceleration-based methods for formulating the equations of motion:counting the number of arithmetical operations needed to establish the equations ofmotion reveals that the proposed algorithm requires less computations than otheracceleration based methods described in the literature.

Before getting to the description of the algorithm, the spatial momentum vectorof a single rigid body is defined (chapter 3). It is a concatenation of the linear andangular momentum vector of that body and it allows one to establish a link withthe canonical momenta: the latter are the projection of the spatial momentumvector into the joint subspace. Furthermore, the Newton-Euler equations in termsof the momentum vector result in a concise spatial form of the equations of motion.

The recursive O(n) algorithm is first explained for open-loop systems and isheavily based on Featherstone’s articulated inertia method [Featherstone, 1987](chapter 4). Combining the concept of spatial momentum vector and the philoso-phy of the articulated inertia, a set of equations of motion can be obtained for thearticulated bodies. These equations are of the exact same form as for a single rigidbody, but are expressed in terms of the so-called articulated momentum vector, acombination of the momentum vectors of all the bodies contained in the consid-ered articulated body. Projecting the articulated momentum vector into the jointsubspace reveals the canonical momenta associated with that articulated body.

The extension of the open-loop algorithm to closed-loop system is discussed inchapter 5. Two different methods to deal with constraints are proposed. First amethod using Lagrange multipliers is discussed. The constraint forces and momentsare expressed in terms of the unknown multipliers and the constraint equations are

INTRODUCTION 5

used to solve them. The method has O(n + nm + m2 + m3)-efficiency. It can beused for lightly constrained systems, but the cubic relationship rapidly decreasesthe performance as more constraints are added.

The second algorithm proposed in this thesis is based on a coordinate reduction.Adding constraints to an open-loop system creates a relationship between the jointcoordinates, a partitioning in independent and dependent coordinate is thereforerequired. The principle of virtual power is formulated and the dependent virtualvelocities are expressed in terms of the independent ones. This allows to introducethe so-called constrained articulated momentum vector, a modification of the arti-culated momentum vector embedding the constraints. It is then possible to designa O(n)-algorithm in a way similar to the open-loop method. A singular behaviorcan occur when the sets of independent and dependent coordinates are inconsistentor poorly conditioned, this however can easily be fixed by switching to other setsof coordinates.

Additionally, some attention is paid to impacts and topology changes. It is ex-plained how the impact equations can be obtained recursively using the algorithmfor the forward dynamics. A topology change occurs when a contact is made orbroken or when several subsystems are connected or disconnected. They are veryeasily dealt with using the recursive algorithm.

Finally, several numerical examples are given to illustrate the theory. The compu-tation speed of multibody models with a different amount of bodies are comparedto show the O(n)-efficiency for the formulation of the equations of motion. Aswas said earlier, the global efficiency is dependent on both the formulation and thesolution of the equations of motion. Therefore, the fixed step numerical integrationof canonical momenta and acceleration based equation is compared. No significantdifference in accuracy can be detected for unconstrained systems. On the otherhand, the constraint violation errors behave better using the Hamiltonian algo-rithm. Hence, the simulation of constrained multibody systems using the proposedalgorithm combines a more efficient formulation and a more efficient solution. Onthe third hand, the example demonstrates an interesting phenomenon that appearswhen adaptive integrators are used on unconstrained systems. The canonical mo-menta based equations seem to behave much better than the acceleration basedones. When relaxing the error tolerance on the coordinate velocities however, bothformalisms display the same behavior.

1.3 APPLICATIONS: NEED FOR SPEED

Complex multibody systems are quite often simplified to enable realtime simula-tions, or at least to obtain solutions in a reasonable amount of time. The simplifi-cations relate to model reductions and dedicated optimizations. Some applicationshowever need a high degree of fidelity for the results to be usable and simplifi-cations are ruled out. In that case, it is important to revert to fast simulation

6 CHAPTER 1

methods. The performance of simulations relies on multiple aspects. Obviously,computer hardware is an important factor: the evolution of CPUs, memory, graph-ics acceleration cards and so on all contribute directly to the simulation speed. Asparallelization is gaining in popularity, the development of parallel computing ar-chitectures is getting increasingly interesting. Similarly, the communication speedbetween computers improves simulations run on clusters of computers. Further im-provements can be obtained by using faster algorithms to establish the equationsof motion and by designing adequate numerical integrators that allow one to uselarger time steps without loss of accuracy or without the solution becoming unsta-ble. Efficient methods for detecting contacts between bodies are also important, asthat part of the simulation may require a significant amount of time.

Applications can be found in a myriad of domains: virtual reality, virtual proto-typing, animation, spacecraft simulations, tele-surgery or tele-operation in general,design of control architectures, computer games, biomechanics and so on. Some ofthese topics are discussed in more detail in the following sections. It is importantto realize that different applications may need different requirements to be met:robot control needs real-time calculations and accurate results, computer graphicsand animation also require real-time computations, but only realistically lookingsimulations, making accuracy less stringent. The modelling of cars, trains, andother requires a high amount of bodies and fair accuracy, but realtime criteria arenot an issue.

1.3.1 SIMULATIONS

Figure 1.1: The Canadarm2

Many industries use simulations of mechanical systems to support research andtesting. For example, simulations enable NASA to construct a range of scenariosand procedures for the space missions to account for as many eventualities as pos-sible. The safety of the missions is thus increased. Simulations are also used totrain airplane or spacecraft pilots. In that case the pilot must be able to interact

INTRODUCTION 7

with the system in real-time, which puts high requirements on the system. Suchsimulations are called man-in-the-loop simulations. A nice example is the Cana-dian Space Agency’s Mobile Servicing System Operations and Training Simulator(MOTS) in Quebec. It is designed to train astronauts and cosmonauts to operatethe Mobile Servicing System (MSS) from the Space Station. The MSS consists ofthree components: the well-known Canadarm2 (fig.1.1), the not yet operationaltwo-armed robot called Special Purpose Dexterous Manipulator (SPDM) and theMobile Base System (MBS). It is hard to simulate zero-gravity in real life, under-water experiments and temporary weightlessness on a parabolic trajectory are notsuitable for training on robot operation. A computer can do a much better job andcritical scenarios can be replayed without risk of damage.

For more realistic training, the simulator may control a physical platform thatmimics the motion of the virtual airplane or spacecraft. This kind of simulationsare called hardware-in-the-loop simulations. An example of such a simulator isillustrated in figure 1.2. The National Advanced Driving Simulator (NADS) at theUniversity of Iowa aims at simulating ground vehicles to conduct safety researchand to test virtual prototypes.

The NADS consists of a dome with a diameter of over seven meters in which (partof) the bodywork of a car, truck or bus can be mounted. The dome itself is fixed toa motion platform with six degrees of freedom, which enables large rotations andtranslations of about ten meters. Add visual and auditive subsystems and a controlinterface through the bodywork’s steering wheel, pedals, transmission system andso on, and a driver installed in the dome gets the impression of driving a real vehicle(although he may get nausea afterwards).

1.3.2 VIRTUAL PROTOTYPING

Virtual prototyping is a special case of simulations, but it is so important it de-serves its own section. It is a tool used to aid the design of a machine. Buildingand testing a real prototype is so expensive and time-consuming that the numberof possible design alternatives is strongly limited. Moreover, one may not be ableto obtain all desired measurements due to inaccessibility of certain spots or otherpractical issues. Instead of using real prototypes, one may prefer to revert to avirtual prototype by transferring the design model into a multibody analysis soft-ware package. The virtual prototype can be simulated using realistic environmentsand scenarios. All possible measurements can be made without the usual practicalproblems, which allows a much better analysis of the system. It is also possible tocombine the dynamic simulation with some optimization techniques to perform au-tomatic tweaking of specific variables. Virtual prototyping reduces time-to-marketand suppresses the cost of real prototypes. A limited number of real prototypesmay of course still be needed, at least for validation of the multibody model.

Liebherr’s TI272 (fig.1.3)[LMSnews, 2003] is a good example to illustrate the con-cept. The TI272 is an ultra-class haul truck designed for the mining industry. These

8 CHAPTER 1

Figure 1.2: NADS vehicle simulation

Figure 1.3: Liebherr’s TI 272 ultra-class haul truck

huge haul trucks need a high payload/empty weight ratio to increase mining pro-ductivity and to reduce operational cost. A higher payload reduces the number of

INTRODUCTION 9

trips, a lighter truck reduces the consumed fuel. Haul trucks indeed travel half oftheir time empty. One of the ideas behind the TI272 was to switch from a singleaxle design to an improved design with two independent axles, with the inten-tion to reduce the high bending loads and consequently, enable higher payloads.The prototypes built to test the design exhibited fatigue, so a redesign of criticalcomponents was needed. Applying strain gauges on structural elements could givean idea of the internal stresses, but it was not possible to cover the whole truckwith these. It would have been an option to redesign a whole series of componentsusing finite element tools and try these on new prototypes, but this would havehad a very high cost, both in money and in time. Therefore Liebherr reverted tothe DADS (LMS Virtual.Lab Motion) multibody package for simulating the TI272.They succeeded to validate a model with 30 rigid bodies and one flexible bodyfor the frame, using the data from the strain gauges. Much more data was madeavailable and all required internal load could be computed, which greatly helpedto localize and redesign the critical components.

There are numerous other examples of virtual prototypes: BMW and Harley-Davidson use multibody packages to ensure the stability of their motorcycles, Daim-lerChrysler to improve their valve trains, Toyota Racing Developments (TRD) tooptimize their racing vehicles, and the ESA to aid the design of launch vehicles andsatellites. Observing the growing popularity of virtual prototyping, it is not sur-prising that the number of multibody codes is increasing as well: DADS, ADAMS,RecurDyn, Robotran, SD/Fast, Pro/Mechanica Motion, NEWEUL, SIMPACK,MBDyn are just a few examples out of the plethora of choices.

The automotive and aerospace industries are reaching a new balance between thestandard physical tests and virtual prototyping. Improved speed and reliabilityincrease the popularity of simulations. Real-time simulations can further improvethe efficiency of virtual prototyping by enabling the designers and analysts to inter-act directly with the system. However, virtual prototyping is not only a matter ofmultibody dynamics, but also of thermodynamics, fluid dynamics, vibration anal-ysis, electromagnetism and so on. The integration of multidisciplinary simulationcodes remains the exception rather than the rule. In order to deploy the full capa-bilities of computational modelling, the synergy of different fields must be achieved.This is probably the most important challenge nowadays.

1.3.3 CONTROL

The use of multibody dynamics for control purposes is twofold. First simulationsmay be used to design and test control strategies. Testing controllers is a delicateissue, as the system may display an unpredictable or even unstable behavior. Usinga real prototype may be expensive and dangerous. Reverting to a virtual prototypewith embedded controller is certainly a better idea.

On the other hand, multibody dynamics may be incorporated in the controlleritself to improve its efficiency. The computed torque method for example calculates

10 CHAPTER 1

which torques or forces must be applied by the actuators to obtain a desired tra-jectory. This method uses the inverse dynamics. Simulations may be performed topredict the motion. This can be helpful for jumping or falling robots for instance,as they may adapt their configurations to the trajectory prediction. Simulationscan also be used in vehicles to warn the driver in case of imminent critical situ-ations. Tele-operated systems with time-delay can also benefit from simulations.Typical examples are robots which are operated from a great distance (it takesseveral seconds for a signal to travel from earth to moon and back) or underwatervehicles controlled by acoustic waves. The delay caused by the limited travellingspeed of the signals puts a bound on the overall action speed of the robot or vehicle.Using simulations and correcting these with the delayed data may ameliorate thecontroller’s performance and as such the operation of the robot.

1.3.4 ANIMATION MOVIES

Dynamic simulations are being used in animation movies to obtain richer, morecomplex scenes and to reduce cost compared to the traditional methods. Thebiggest challenge is probably to provide the animator with appropriate controlla-bility to enable him to meet the director’s desires. One could for example want tosimulate a human character realistically. For the character to move, muscle forceshave to be specified. It may be possible to apply arbitrary forces in the muscles,simulate the character and look whether the animation is satisfactory or not. Ifnot, the forces can be tuned till satisfaction. This trial-and-error approach doesn’tseem like a sensible thing to do. A better approach would be to give the animatorsome tools to design approximate trajectories and let the simulator calculate a re-alistic motion taking these trajectories into account. This is called active motion:the simulator is required to add physics into the animation while fulfilling certaingiven intermittent configurations [Laszlo et al., 2000]. It should, for example, alsomake sure that the character does not take a configuration in which it would nor-mally loose its balance and fall down [Shin et al., 2003]. This is not an easy task.Furthermore, real dynamics may give a mechanical touch to the animation whilethe director would prefer a cartoony look. As Frank Thomas and Ollie Johnston,two of Disney’s Nine Old Men, once said: “Most people think the word ‘animation’means movement. But it doesn’t. It comes from ‘animus’ which means ‘life’ or ‘tolive’. Making it move is not animation, but just the mechanics of it”. A method forresolving the mechanical feeling is to add randomness to the simulation and tryingto achieve plausible motion rather than real motion, resulting in the right experi-ence rather than the right physics [Barzel et al., 1996, Chenney & Forsyth, 2000].Or, to use Stephen Chenney’s (University of Wisconsin) words: “Reality is a messything we can’t hope to accurately model” but “People are poor observers and canbe easily fooled”. Instead of introducing randomness, one may also revert to fakedynamics to simulate complex systems such as wires and ropes [Barzel, 1997]. An-imation artists however will always be needed to give life to the characters and the

INTRODUCTION 11

environment.

Figure 1.4: Boo and Sully in Pixar’s Monsters Inc. movie

For certain scenes, dynamic simulations are undoubtedly the better choice. Thisis certainly the case for so-called passive motions such as the swinging of a chainor rope, falling objects, grass or hair waving in the wind, the dispersion of smokeor the flow of water. These effects are difficult to obtain manually, but are pieceof cake for the computer. Take Pixar’s animation movie Monster Inc. for example(fig.1.4). One of the main characters is a big fluffy monsters called Sully. He hasbeen modelled with 2, 320, 413 blue hairs, each one being individually simulated toachieve a realistic animation. Pixar developed its own dynamic simulator calledFitz for that purpose. Fitz had not only to calculate the motion of each hair, butalso to make sure they wouldn’t intersect with each other or with other objects.It also had to untangle them when necessary. Figure 1.4 also shows Boo, a littlegirl with an oversized T-shirt and a pigtail. The movement of both the T-shirtand the pigtail were dynamically simulated as well [Baraff et al., 2003]. Althoughsimulation time is important to speed up the production of an animation movie—and Pixar uses state-of-the-art methods and relies on an impressive computerfarm for that purpose (3500 Sun Microsystems processors at the time of MonstersInc.)— it must be placed into its context: it cost ten minutes to obtain one secondof animation for Sully, while up to 80 hours were needed for the graphical renderingof one single frame.

12 CHAPTER 1

Figure 1.5: Screenshot from Valve’s Half-Life2

1.3.5 PHYSICS BASED COMPUTER GAMES

An ever increasing amount of computer games incorporates a physics engine togive a more lifelike feel to the environment and to increase interactivity with thecharacters and objects that inhabit it. Unreal Tournament 2004 for example, usesthe Karma physics engine. It allows for visually convincing effects, such as vehicledynamics or ragdoll physics that makes characters bodies behave realistically. TheSource engine used in Half-Life2 is another example. Figure 1.5 is a screenshotfrom Half-Life2, it shows a buggy being carried by a crane. The swinging motionis obtained through dynamic simulation.

The physics engines are still in an early phase though. So far, fairly simplemultibody systems are simulated. Take a new look at figure 1.5, the cable carryingthe buggy is actually simulated with one single body, which results in a constantlystraight line. Much progress is to be expected in the near future: higher number ofbodies simulated at the same time, more complex and realistic multibody systems,deformable objects and so on. The company AEGIA [AEGIA, 2005] recently tookan important step by announcing the release of the first so-called PPU (PhysicsProcessing Unit): the PhysX chip. A PPU is much like a graphics accelerationcard, but it aims at accelerating dynamic computations. The PhysX chip should

INTRODUCTION 13

be able to handle up to 40000 bodies in real-time, involving of course the motion ofthe objects, but also the collision detection. It is apparently not restricted to rigidbodies: soft bodies and fluids can also be simulated by the PhysX chip. The rigidbody dynamics part is tackled by the Novodex engine [Novodex, 2004]. While thetechnology will certainly improve the gaming experience dramatically, it’s successwill depend on the initial breakthrough, as hardware tends not to sell so easily.

14 CHAPTER 1

Chapter 2

HAMILTONIAN EQUATIONS

“He used to carry on, long trains of algebraic and arithmetical cal-culations in his mind, during which he was unconscious of the earthlynecessity of eating; we used to bring in a snack and leave it in his study,but a brief nod of recognition of the intrusion of the chop or cutlet wasoften the only result, and his thoughts went on soaring upwards.”

-William Edwin Hamilton (his elder son)

There are many ways to describe the equations of motion of multibody systems.The reason why this thesis is based on Hamilton’s equations is twofold. On theone hand, there is an advantage related to the stability of constraints during thenumerical integration of constrained systems. On the other hand, Hamilton’s equa-tions can be formulated very efficiently using a recursive algorithm, but this willbe discussed later in chapter 4. For the moment, the objective is to obtain anexpression for these Hamiltonian equations. The best way to introduce them isto start from the well-known Lagrangian equations [Lagrange, 1853]. When La-grange’s equations are expressed for n dependent coordinates q related by meansof m constraints Φ = 0 and in terms of the nonconservative forces Qnc, they takethe following form (see appendix B for more detail)

d

dt(∂L

∂q)T − (

∂L

∂q)T + ΦT

qλ = Qnc (2.1)

with

L = T − V (2.2)

L is the Lagrangian function. T is the kinetic energy of the system and V thepotential energy arising from the conservative forces. Φq represents the Jacobianof the constraint equations and λ is a set of Lagrange multipliers. In multibodydynamics, these equations are often written as [de Jalon & Bayo, 1994]:

15

16 CHAPTER 2

Mq + ΦTqλ = Qnc + Lq − Mq (2.3)

with Lq = (∂L/∂q)T . M is called the generalized mass matrix, it comes from thedefinition of the kinetic energy: T = 1/2qT Mq. The Lagrangian equations aresecond order differential equations. They are not convenient to integrate directlyand it is therefore common to convert them into a more suitable set of first or-der differential equations. This can be achieved by introducing new independentcoordinates v = q:

q = v (2.4)

v = M−1(Qnc + Lq − Mq − ΦTqλ) (2.5)

Figure 2.1: Sir William Rowan Hamilton (1805-1865)

It was Hamilton’s1 idea to switch from a description of the system’s state usingcoordinates q and velocities v to a description using the same coordinates q andthe so-called canonical momenta p, defined as [Goldstein, 2002]

1Sir William Rowan Hamilton(1805-1865) —not to confound with his contemporary SirWilliam Hamilton (1788-1856), Scottish philosopher and educationalist— is born in Dublin anddied in Dublin. Much happened in the meantime. He grew up with his uncle, Reverend JamesHamilton, who took care of young Hamilton’s linguistic education. William was a child prodigy.At the age of 5 he already mastered Latin, Greek and Hebrew. At the age of 13 he had consid-erable knowledge of 13 languages, including French, Italian, German, Spanish, Syriac, Persian,

HAMILTONIAN EQUATIONS 17

p = (∂L(q, q, t)

∂q)T (2.6)

This definition is not just a mathematical trick, the canonical momenta appearexplicitly in Lagrange’s equations (2.1). The full name for p takes a while to pro-nounce: generalized momenta canonically conjugate to q. The canonical momentaare an extension of the concept of linear and angular momenta to generalized coor-dinates. Substituting definition (2.6) into Lagrange’s equations (2.1) produces thefollowing form of the equations of motion:

p = Qnc + (∂L(q, q, t)

∂q)T − ΦT

qλ (2.7)

The Lagrangian is still expressed in terms of q and q. It is the objective to obtain anexpression in q and p. From now on, the coordinate velocities q will be consideredas dependent state coordinates : q = q(q,p, t). Following equality is then true:

∂L(q,p, t)

∂q=

∂L(q, q, t)

∂q+

∂L(q, q, t)

∂q

∂q

∂q(2.8)

∂q

∂qis generally not equal to zero, because q is treated as a coordinate depending

on q. Expression (2.8) can be further developed:

Arabic, Sanskrit, Hindustani and Malay. Reverend James Hamilton was an estimable man and agood teacher, but an eccentric one. An anecdote tells that he used to tie a string around William’stoe and pass it through a hole in the wall into his own room, just before going to sleep. In theearly morning he would then pull on the string to wake his nephew so they could start the lessonsright away.

Young Hamilton appeared to be excellent in mental arithmetics. Aged 12, he would challengelegend Zerah Colburn (1804-1840), who was able to perform amazing mental feats. Asked whether4294967297 was prime, Colburn replied it had 641 as a divisor after just a short mental calculation.Hamilton usually lost the competitions, but that would only enforce his interest in mathematics.According to Hamilton, his ability was attributable to nothing more impressive than an unusuallygood memory. Aged 15, Hamilton studied the works of Newton and Laplace, he found an errorin Laplace’s Mecanique Celeste. In 1823 he entered Trinity College (in Dublin, not Cambridgelike Newton). Four years later he was already appointed Professor of Astronomy, which broughta lot of controversy considering his lack of experience. Hamilton made important contributionsto optics. He introduced his characteristic function, a function he would later apply to mechanics[Hamilton, 1834]. Hamilton dedicated the last part of his life to the study of quaternions. Al-though he claimed the discovery of quaternions to be his biggest achievement, many argue thathe did a much better job in optics and dynamics. Hamilton was the person who introduced theterms vector and scalar, and the sign ∇ that was named after the shape of an old Egyptian harp(nabla). Hamilton showed that Newton’s equations and Lagrange’s formalism were equivalentand extended Maupertuis’s principle of least action. His work would later be quite significant forthe development of quantum mechanics, it was the starting point for Schodingers development ofthe wave mechanics. In his private life, Hamilton was less successful. He was tormented by hisfrustrated love for Catherine Disney and was driven to alcoholism.

18 CHAPTER 2

∂L(q, q, t)

∂q=

∂L(q,p, t)

∂q−

∂L(q, q, t)

∂q

∂q

∂q(2.9)

=∂L(q,p, t)

∂q− pT ∂q

∂q(2.10)

=∂

∂q(L(q,p, t) − pT q) (2.11)

= −∂H(q,p, t)

∂q(2.12)

Such a transformation is known as a Legendre transformation2. Equation (2.12)introduces the Hamiltonian function H, which is defined as

H(q,p, t) = pT q − L(q,p, t) (2.13)

Partial differentiating the Hamiltonian (2.13) with respect to the canonical mo-menta reveals another important equation:

∂H

∂p= qT + pT ∂q

∂p−

∂L

∂q

∂q

∂p= qT (2.14)

Combining (2.7), (2.12), (2.14) and the constraint equations, a set of 2n + m firstorder differential-algebraic equations (DAE) is formed:

q = (∂H(q,p, t)

∂p)T (2.15a)

p = −(∂H(q,p, t)

∂q)T + Qnc − ΦT

qλ (2.15b)

Φ = 0 (2.15c)

In analogy with equation (2.3) and using the definition of the canonical momenta,a set of Hamiltonian equations can also be written as:

p = Mq (2.16)

2Another well-known application of Legendre transformations can be found in thermodynam-ics, where the enthalpy and entropy of a system are dependent on two coordinates, but can bedescribed arbitrarily as functions of pressure, specific volume or temperature[Goldstein, 2002].

HAMILTONIAN EQUATIONS 19

p = Lq + Qnc − ΦTqλ (2.17)

Unlike for the Lagrange equations, the multipliers λ in the set of Hamiltonianequations (2.15) cannot be eliminated by differentiating the constraint equations(see again appendix B.2.3). To solve this problem, the canonical momenta and theLagrange multipliers need to be coupled. This can be achieved by introducing theconstraint equations at the velocity level instead of the position level. ApplyingHamilton’s principle on a new3 augmented Lagrangian L

+ = L + ΦTσ results in:

d

dt(∂L + Φ

Tσ

∂q)T − (

∂L + ΦTσ

∂q)T = Qnc (2.18)

which can be turned into4

d

dt(∂L

∂q)T − (

∂L

∂q)T + ΦT

q σ = Qnc (2.19)

Identification with (2.1) yields σ = λ. While the Lagrange multipliers λ representa measure for the reaction forces and moments, the multipliers σ are a measure forthe reaction impulses and impulse moments. Using the augmented Lagrangian, aset of augmented canonical momenta p+ can be defined as:

p+ =∂L+

∂q= p + ΦT

qσ = Mq + ΦTqσ (2.20)

Time derivation of the augmented canonical momenta gives

p+ = p + ΦT

qσ + ΦTqλ (2.21)

Substitution in (2.15b) finally results in the new equations of motion:

p+ = Lq + Qnc + ΦT

qσ (2.22)

The Lagrange multipliers can now be solved by differentiating the constraint equa-tions once with respect to time. In matrix notation, this results in

(M ΦT

q

Φq 0

)(q

σ

)=

(p+

−Φt

)(2.23)

To find the multipliers λ in the Lagrangian formulation, the constraint equationsneeded to be differentiated twice with respect to time. To find σ, the constraintequations are differentiated only once, resulting in smaller constraint violation er-rors [Baumgarte, 1983]. The augmented formulation is used in chapter 5.

3Instead of L+ = L + ΦTλ used in appendix B.2.3.

4Note that Φ = Φqq + Φt.

20 CHAPTER 2

Chapter 3

RIGID BODY AND CANONICAL

MOMENTA

“The sciences do not try to explain, they hardly even try to inter-pret, they mainly make models. By a model is meant a mathematicalconstruct which, with the addition of certain verbal interpretations, de-scribes observed phenomena. The justification of such a mathematicalmodel is solely and precisely that it is expected to work.”

-John Von Neumann

In this section, the concept of spatial momentum vector is introduced. This mo-mentum vector works as a leverage point between the well-known Newton-Eulerequations and the Hamiltonian equations. It is easy to obtain the canonical mo-menta from the momentum vector and the resulting equations of motion can beformulated in a quite elegant way.

3.1 MOMENTUM VECTOR

Canonical momenta often tend to lack an intuitive and easy physical interpreta-tion and are therefore bothersome or uncomfortable to work with. Most people(scientists/engineers) however are familiar with the concepts of linear (pl) and an-gular (pa) momentum vectors. Both vectors have dimension 3 and can be broughttogether in a 6-dimensional spatial momentum vector P:

P =

(pl

pa

)(3.1)

There is no unique angular momentum of a (rigid) body, as it is calculated withrespect to an arbitrary reference point. But it would be cumbersome to mention

21

22 CHAPTER 3

that point whenever the angular momentum is used. This is why the referencepoint is always chosen as the origin O of the local reference frame.

Expressing the linear momentum as function of the velocity of point O and theangular velocity of the body gives:

pl = mvG = m(vO + ω × OG)

= mvO − mOG × ω (3.2)

Doing the same for the angular momentum yields

pa = JOω + m(OG × vO) (3.3)

The last term is needed because the reference point O may be moving. The con-catenation of both momentum vectors results in following spatial form:

P = MΩ (3.4)

with the mass matrix

M =

(mI mGO

mOG JO

)(3.5)

and the spatial velocity vector Ω defined as

Ω =

(vO

ω

)(3.6)

The mass matrix of the body is symmetrical and positive definite [Featherstone, 1987].I is a unity dyadic, its dimensions are dependent on the context (3×3 in this case).

3.2 KINETIC ENERGY

Before jumping to the derivation of the canonical momenta, it is convenient toconsider the expression for the kinetic energy T of a single rigid body.

T =1

2mvT

GvG +1

2ωT JGω (3.7)

The form expressed in terms of the center of mass is often used, as the contributionsfrom the linear and the angular motions are decoupled. Plugging in the linearvelocity and the inertia tensor with respect to origin O however, results in

RIGID BODY AND CANONICAL MOMENTA 23

T =1

2m(vO + ω × OG)2 +

1

2ωT (JO − mOG OG)ω (3.8)

which, after rearranging the terms, can be identified with

T =1

2ΩT MΩ =

1

2ΩT P (3.9)

3.3 CANONICAL MOMENTA

As was seen in chapter 2, the canonical momenta of a mechanical system aredefined as the partial derivation of the Lagrangian function L with respect to thegeneralized velocities q. Let’s assume that the potential energy V of the system isindependent of the system’s velocities. In that case, only the kinetic energy T isneeded to perform the calculations.

L = T − V =1

2ΩT MΩ − V (q) (3.10)

The spatial velocity can be written as

Ω = Eq + Ωt (3.11)

E is called the joint matrix and describes the joint subspace. Its components arethe partial derivatives of the spatial velocity to the joint velocities q. Ωt is a termwhich appears when kinematic drives are present, it is independent of the jointvelocity (see appendix A.7 for more detail). Partial derivation of the Lagrangiangives

p = (∂L

∂q)T = (

∂T

∂q)T = ET MΩ = ET P (3.12)

The canonical momenta of a rigid body can be interpreted as the projection ofthe momentum vector P on the joint subspace E. As will be shown in chapter4, this notion can be extended to multibody systems. From (3.9), the well-knownHamiltonian expression for the kinetic energy can be found:

T =1

2ΩT P =

1

2qT ET P =

1

2qT p (3.13)

Note that the momentum vector can formally be obtained using:

∂L

∂Ω= PT (3.14)

24 CHAPTER 3

3.3.1 EXAMPLES

EXAMPLE 1: PIN-JOINT

As soon as real computations must be performed and hard data is needed, allphysical vectors and tensors need to be projected into reference frames. While,for instance, P is used to denote the spatial momentum vector, KP represents acoordinate vector containing the components of P when projected on the main axesof reference frame K. If K is a local reference frame attached to body K, the massmatrix KM of body K is constant and the momentum vector of that body can bewritten explicitly as:

KP =

KM

KΩ (3.15)

=

0BBBBB m 0 0 0 mOGz −mOGy

0 m 0 −mOGz 0 mOGx

0 0 m mOGy −mOGx 00 −mOGz mOGy Jxx Jxy Jxz

mOGz 0 −mOGx Jxy Jyy Jyz

−mOGy mOGx 0 Jxz Jyz Jzz

1CCCCCA0BBBBBvx

vy

vz

ωx

ωy

ωz

1CCCCCA (3.16)

=

0BBBBB mvx + mOGzωy − mOGyωz

mvy − mOGzωx + mOGxωz

mvz + mOGyωx − mOGxωy

−mOGzvy + mOGyvz + Jxxωx + Jxyωy + Jxzωz

mOGzvx − mOGxvz + Jxyωx + Jyyωy + Jyzωz

−mOGyvx + mOGxvy + Jxzωx + Jyzωy + Jzzωz

1CCCCCA (3.17)

Figure 3.1 depicts a rigid body connected to the environment by means of a pin-joint. The system has one degree of freedom θ describing the rotation angle aboutthe joint axis. Using definition (3.12), one finds:

p =∂L

∂θ=

1

2

∂(JO θ2)

∂θ= JO θ (3.18)

This is nothing else than the angular momentum about the joint axis, as one wouldexpect. JO = Jzz is the moment of inertia about the joint axis. The spatial velocityvector can be expressed as

KΩ = KEθ =(0 0 0 0 0 θ

)T(3.19)

The joint matrix associated with θ and written with respect to frame K attachedto the body is thus

KE =(0 0 0 0 0 1

)T(3.20)


Figure 3.1: Rigid body connected to the environment by means of a pin-joint

Projecting the momentum vector P on E gives (see (3.15))

p = ET P = KET KP = JO θ (3.21)

EXAMPLE 2: SLIDING PIN-JOINT

In figure 3.2, the body gains an additional degree of freedom as it is allowed toslide along a rod in the z-direction. There are thus 2 DOF and the joint matrixhas dimensions 6 × 2:

KE =

(0 0 1 0 0 00 0 0 0 0 1

)T

(3.22)

The canonical momenta can then again be obtained using the definition or byprojection

26 CHAPTER 3

Figure 3.2: Rigid body connected to the environment by means of a sliding pin-joint

p =

(12

∂(JO θ2+mz2)∂z

12

∂(JO θ2+mz2)

∂θ

)=

(mz

JO θ

)(3.23)

p = ET P =

(mz

JO θ

)(3.24)

which, of course, leads to the same result. The vector of canonical momenta p

is now an aggregation of the linear momentum along the z-axis and the angularmomentum about that same axis. The canonical momenta of a single rigid bodyoften have an intuitive physical meaning, this is not always the case for generalmultibody systems.

The momentum vector P and the joint matrix E are dependent on the referenceframe. Indeed, if the origin of the reference frame is chosen to be the center ofmass G, the spatial velocity ΩG associated with the new reference frame becomes:


GΩG = GEG

(z

θ

)=

−OGy θ

OGxθz00

θ

(3.25)

which implies the following expression for the joint matrix

GEG =

0 −OGy

0 OGx

1 00 00 00 1

(3.26)

The mass matrix is also dependent on the chosen reference frame. MG is given by

MG =

(mI 0

0 JG

)(3.27)

The canonical momenta on the other hand, are associated with the generalized co-ordinates and are independent of the chosen reference frames. Choosing a differentreference frame does not influence the canonical momenta:

p = ETGPG = GET

GGPG =

(mz

(JG + mOG2x + mOG2

y)θ

)=

(mz

JO θ

)(3.28)

This is the same result as (3.23).

EXAMPLE 3: UNCONSTRAINED RIGID BODY

The third and last example is a freely moving body (floating body). The exampleis restricted to the planar case for simplicity (fig. 3.3). XOYO is the global, inertialframe, XKYK is the local frame attached to floating body K. x and y are thecoordinates denoting the position of the origin OK of reference frame K. θ denotesthe orientation of the body with respect to XO. The components of the spatialvelocity ΩK with respect to the global reference frame are easy to find:

28 CHAPTER 3

Figure 3.3: Floating rigid body

OΩK = 0EK

xy

θ

=

xy000

θ

(3.29)

Hence, the associated joint matrix EK is

0EK =

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

(3.30)

If one needs the components of this joint matrix with respect to the local referenceframe, a simple rotation must be performed and one ends up with


KEK =

cos θ sin θ 0− sin θ cos θ 0

0 0 00 0 00 0 00 0 1

(3.31)

Note that the time derivative of the joint matrix with respect to the local referenceframe is not zero:

KEK =

− sin θ cos θ 0− cos θ − sin θ 0

0 0 00 0 00 0 00 0 0

θ (3.32)

The importance of this observation will become apparent in the next section.

3.4 NEWTON-EULER EQUATIONS FOR ASINGLE RIGID BODY IN LOCAL AXES

The Newton-Euler equations are discussed in appendix B. Expressed in movingaxes, they take the following form

pl + ω × pl = f + fr (3.33)

pa + v × pl + ω × pa = t + tr (3.34)

where the total forces and torques are split into an external part (f , t) and a reactionpart (fr, tr). Remember that the angular momentum is taken with respect to theorigin of the local reference frame, hence the extra term v×pl in (3.34). pl standsfor the time derivative of pl relative to the local axes. For the local reference frameK, this is also written as pl = dKpl/dt. Above form of the Newton-Euler equationscan be written in matrix notation:

(pl

pa

)+

(ω 0

v ω

)(pl

pa

)=

(f

t

)+

(frtr

)(3.35)

For the sake of conciseness, a 6-dimensional vectorial product can be defined:

30 CHAPTER 3

Ω× =

(v

ω

)× ,

(ω 0

v ω

)(3.36)

This is an operator encountered in the Lie algebra. Defining the spatial force vector

T =(f t

)Tand introducing the momentum vector, the descriptor form of the

equations of motion for a single rigid body becomes

P + Ω × P = T + Tr (3.37)

The story is not completely finished yet, as the reaction forces Tr are unknown.(3.37) forms a set of 6 equations however, and the motion has only n ≤ 6 DOF.By projecting the equations into the joint subspace E, the reaction forces can beeliminated and (3.37) reduces to following Hamiltonian equations

p = ET (T − Ω × P) + ET P (3.38)

The last term results from the fact that p = ET P+ ET P (from definition 3.12). Itvanishes for certain types of joint, like pin-joints or sliders. Remember: E standsfor the time derivation of vector E with respect to the local (moving) referenceframe.

3.5 JOINT VELOCITIES

To complete the set of Hamiltonian equations, the joint velocities q are required.These can easily be found by application of the definition of the momentum vector:

P = MΩ = MEq (3.39a)

⇒ q = (ET ME)−1ET P = M−1j p (3.39b)

Mj = ET ME is the mass matrix projected into the joint space.

3.6 HAMILTONIAN EQUATIONS

The general form for the Hamiltonian equations of a single rigid body is thus

q = M−1j p (3.40)

p = ET (T − Ω × P) + ET P (3.41)


The joint constraints are embedded in the constraint matrix E. Note that the spa-tial velocity Ω is dependent on the joint velocities, so (3.40) needs to be calculatedprior to (3.41).

3.7 CONCLUSIONS

In this chapter, a Hamiltonian approach was used to describe the dynamics of asingle rigid body. It resulted in the introduction of the concept of momentumvector that helped to provide an interpretation of the canonical momenta and tocreate an elegant theoretical framework. As will be seen in the next chapters, thatframework can easily be extended to describe open-loop and closed-loop multibodysystems.

32 CHAPTER 3

Chapter 4

OPEN-LOOP MULTIBODY SYSTEMS

“More computing sins are committed in the name of efficiency (with-out necessarily achieving it) than for any other single reason - includingblind stupidity.”

-W.A. Wulf

“On the other hand, we cannot ignore efficiency.”

-Jon Bentley

This section is probably the most important one in this book. It shows how aHamiltonian set of equations of motion of an open-loop multibody systems can beobtained efficiently using momentum vectors. As the proposed method is heavilybased on Featherstone’s articulated inertia method [Featherstone, 1987], the dis-cussion will naturally lead to the concept of articulated bodies and their associatedarticulated momentum vectors. The section starts with a description of the differ-ent elements of a generic open-loop mechanism. It is quite important for readerswho are not familiar with the terminology of multibody systems, as the introducedterms are extensively used throughout the text. The next sections have roughlythe same structure as in previous chapter: expressions for the canonical momenta,the articulated momentum vectors, the equations of motion and the joint velocitiesare derived subsequently. The remainder of the chapter deals with the calculationof the accelerations and the reaction forces.

4.1 TERMINOLOGY OF AN OPEN-LOOP MULTI-BODY SYSTEM

Open-loop multibody systems can contain a huge amount of bodies and it is there-fore important to have a convenient method for numbering them. An example of

33

34 CHAPTER 4

an open-loop multibody system is depicted in fig. 4.1. The fixed environment iscalled the ground and its associated (inertial) reference frame gets the identificationnumber 0. The body that is directly connected to the ground is called the basebody and is tagged with the number 1. Note that only one body can be connectedto the ground, additional connections would introduce kinematical loops. Considernow body 4, in black on the figure. All the bodies laying between that body andthe ground are said to be inboard bodies or ancestors. The first ancestor is calledthe parent or adjacent inboard body. Bodies 1, 2 and 3 are all ancestors of body 4,body 3 is its parent. Conversely, all the bodies encountered when following a chainin the opposite direction are called outboard bodies or descendants. Descendantsdirectly connected to the body are children or adjacent outboard bodies. Bodies 5,6, 7 and 8 are descendants of body 4, 5 is a child. A body can have several children(like bodies 3 and 5), but only one parent. If at least one body possesses morethan one child, the system is said to have a tree structure. Different bodies sharingone same parent are called siblings. Bodies 4 and 9 are siblings. The extremities(like 6, 8 and 11) are named leaves or terminal bodies. The tagging of the bodiesis actually quite simple, there are only two rules: two distinct bodies have two dif-ferent identification numbers and the number of a body is always higher than thatof its parent. Besides these restrictions, the numbering is arbitrary. Joints alsoget a tag. As one can observe, there are always as many joints as bodies and it issensible to give the same identification number to a body and to the joint directlypreceding it (see body 10 on the figure). Throughout the text Des(K) is used todenote the set of all descendants of body K, Ch(K) is the set of all children of Kand MBS is the set of all bodies of the considered multibody system. Note thatMBS = Des(0). Similarly, Asc(K) is the set of all ascendants of K and Pa(K) isthe body’s parent.

4.2 CANONICAL MOMENTA

The canonical momenta of an open-loop multibody system can again be calculatedusing the definition:

pTK =

∂L

∂qK=

∂

∂qK(∑

i∈MBS

Ti) =1

2

∂

∂qK(∑

i∈MBS

ΩTi MiΩi) (4.1)

Ti being the contribution of body i to the total kinetic energy of the multibodysystem. The spatial velocity ΩL of an arbitrary body L is a combination of threecontributions: one part of the total motion originating from the motion of theparent body, another part induced by the joint motion and a last part resultingfrom contingent kinematic drives. This can be formulated as:

ΩL = LT VPa(L)ΩPa(L) + ELqL + ΩtL

(4.2)

OPEN-LOOP MULTIBODY SYSTEMS 35

Figure 4.1: Terminology of a tree structure

with LT VPa(L) the spatial velocity shift dyadic. This operator can be written explic-

itly as

LT VPa(L) =

(I ˜OPa(L)OL

0 I

)(4.3)

In open-loop systems, the spatial velocity ΩL is thus dependent on its own jointvelocity and the joint velocities of the inboard bodies. This can easily be shown byinspection of (4.2):

∂ΩL

∂qK= 0 ∀K ∈ Des(L) (4.4)

∂ΩL

∂qK= LT V

K EK ∀K /∈ Des(L) (4.5)

Hence, the kinetic energy of body K is dependent on its own motion and the motionof the ancestors. For the canonical momenta conjugated to leaf body N , one getsthe same expression as (3.12):

36 CHAPTER 4

pN = (∂T

∂qN)T =

∑

i∈MBS

∂ΩTi

∂qNPi = ET

NPN (4.6)

For an arbitrary body K, one obtains

pK =[∂(TK +

∑i∈Des(K) Ti)

∂qK

]T=

∂ΩTK

∂qKPK +

∑

i∈Des(K)

∂ΩTi

∂qKPi (4.7)

= ETK [PK +

∑

i∈Des(K)

(iT VK )T Pi] (4.8)

Note the difference between pK , which denotes the canonical momenta associatedwith the joint coordinates of body K and p, which represents the complete set ofcanonical momenta of the multibody system.

p =

p1

p2

...pN

(4.9)

4.3 TRANSFORMATION OPERATORS

This short section is a brief discussion on the two transformation operators thatare used in this thesis. The first operator, called velocity shift dyadic LT V

K wasintroduced in the previous section. In equations (4.8), the transpose of the operatorwas used. This transposed operator is also written as:

(LT VK )T = KT F

L (4.10)

which is called the spatial force shift dyadic. If a spatial force vector is known withrespect to a reference point L, this vector can be reduced to a different referencepoint K by the mere use of the force operator KT F

L . Forces are transmitted withoutchange, while torques are transmitted according to tK = tL + OKOL × fL. Thiscan readily be recognized in the explicit form of the force operator:

KT FL =

(I 0

OKOL I

)(4.11)

From equation (4.8) it appears that the momentum vectors transform according tothe same rules as spatial forces.


4.4 ARTICULATED MOMENTUM VECTOR

Equation (4.7) gently compels to define the articulated momentum vector as

P∗

K = PK +∑

i∈Des(K)

KT Fi Pi (4.12)

This can also be turned into a recursive formula:

P∗

K = PK +∑

i∈Ch(K)

KT Fi P∗

i (4.13)

As said before, Des(K) and Ch(K) are respectively the sets of descendants andchildren of body K. To obtain all articulated momentum vectors, the formulaneeds to be processed backwards from the leaves to the ground. It was shownin chapter 3 that the projection of the momentum vector of a rigid body on thejoint subspace results in a set of canonical momenta. Similarly, (4.7) clearly showsthat the projection of an articulated momentum vector of a multibody system onits joint subspace results in a set of canonical momenta conjugated to the jointcoordinates:

pK = ETKP∗

K (4.14)

If an articulated body is defined to be a set comprising a given body and all itsdescendants, the articulated momentum vector P∗

K of body K is a measure for theamount of motion of the associated articulated body.

4.4.1 EXAMPLE

As an example, consider the double pendulum depicted in fig.4.2. It may be a bittedious to get an expression for the canonical momenta starting from the definitionand calculating the partial derivatives. This would certainly be the case for a highernumber of links. Using the articulated momentum vectors however results in anextremely simple formulation:

p1 = ET1 P∗

1 (4.15)

p2 = ET2 P2 (4.16)

4.5 EQUATIONS OF MOTION

In chapter 2, a derivation of the Hamiltonian equations has been given. For un-constrained systems, they are formulated as follows:

38 CHAPTER 4

Figure 4.2: Planar double pendulum

q = (∂H

∂p)T (4.17a)

p = −(∂H

∂q)T + Qnc (4.17b)

These equations can be established straightforwardly by working out the partialderivatives of the Hamiltonian function, but this results in lengthy expressions. Inthis section, the Hamiltonian equations are obtained by virtue of the Newton-Eulerequations (3.37) that were derived for single rigid bodies. Additionally it is shownhow the same equations can be obtained using the principle of virtual power.

4.5.1 NEWTON-EULER FORMALISM

Using equation (3.37) for each body separately and eliminating the reaction forceswill naturally lead to new equations of motion in terms of the articulated momen-tum vectors. Projecting these equations on the joint subspaces will provide an


alternative way of obtaining equations (4.17b).

Consider first an arbitrary leaf body N . Its equations of motion can be writtenas

PN + ΩN × PN = TN + TrN(4.18)

in which the spatial forces are split in an (known) external part TN and an (un-known) reaction part TrN

induced by the parent body. The subspace of the reactionforces is orthogonal to the joint subspace determined by EN . In the case of a pin-joint for example, this means that no reaction torque is present in the joint. Hence,the spatial reaction force can be eliminated by projection of (4.18) onto the jointsubspace:

pN = ETN (TN − ΩN × PN ) + ET

NPN (4.19)

The story is different for an arbitrary body K. Take a look at figure 4.3, whichrepresents a part of a multibody chain. The total spatial force that influences thebody can now be split in three distinct parts: the external contribution TK , thereactions TrK

impressed by the parent body and the reactions −KT Fi Tri

originatingfrom the child or children.

PK + ΩK × PK = TK + TrK−

∑

i∈Ch(K)

KT Fi Tri

(4.20)

The parent forces could again be eliminated by projection into the joint subspace,but the projection of the children’s contributions would not vanish. If the articu-lated momentum vectors are put into play however, the influence of the childrendisappears. Presume that following equation holds for the children of body K :

P∗

i + Ωi × P∗

i = T∗

i + Tri(i ∈ Ch(K)) (4.21)

with the so-called accumulated force vector

T∗

i = Ti +∑

j∈Ch(i)

KT Fj T∗

j (4.22)

(4.21) can be proved by induction. Therefore, these equations must first of all holdfor the leaf bodies. This is clearly the case as P∗

N = PN and T∗

N = TN for any leafbody N . The second part of the proof by induction is to show that the expressionholds for body K if it is valid for its children. Substitution of (4.21) into (4.20)gives:


∑

i∈Ch(K)

KT Fi (P∗

i + Ωi × P∗

i − T∗

i ) (4.23)

40 CHAPTER 4

Figure 4.3: Reaction forces and torques on body K

Regrouping the terms and remembering to derive with respect to the correct coor-dinate system using (D.12) gives

dK

dt

24PK +X

i∈Ch(K)

KT

Fi P

∗

i

35+ΩK×

24PK +X

i∈Ch(K)

KT

Fi P

∗

i

35 = TK+TrK+

Xi∈Ch(K)

KT

Fi T

∗

i

(4.24)

Following two properties were used for the derivation:

dK

dt(KT F

i P∗

i ) = KT Fi (P∗

i + Eiqi × P∗

i ) (4.25)

KT Fi (iT V

K ΩK × P∗

i ) = ΩK × KT Fi P∗

i (4.26)

Proofs are provided in appendix D.4.3. Identification of the articulated momentumvector and the accumulated force vector in (4.24) eventually yields the desiredoutcome:

P∗

K + ΩK × P∗

K = T∗

K + TrK(4.27)


T∗

K is nothing else than the resultant of all forces applied on the articulated bodyK. Note the similarity with the expression for the articulated momentum vector.The projection on the joint subspace finally leads to set (4.17b) of Hamiltonianequations:

pK = ETKT∗

K − ETK(ΩK × P∗

K) + ETKP∗

K (4.28)

Note that the joint velocities are needed to calculate the spatial velocities. Theformer thus need to be obtained first. It is shown in section 4.6 how this can beachieved.

4.5.2 PRINCIPLE OF VIRTUAL POWER

Expression (4.28) can also be obtained by using the principle of virtual power. Itis not really necessary to use that principle, so the reader may just want to skipthis part, but a brief discussion will probably help to understand how to deal withclosed-loop systems, as explained in the next chapter. The principle of virtualpower states that the reaction forces provide no power during a virtual motion(appendix B):

∑

i∈MBS

ΩvT

i (Pi + Ωi × Pi − Ti) = 0 (4.29)

The superscript v denotes a virtual velocity. The goal now is to rewrite aboveexpression in the following form:

∑

i∈MBS

qvT

i Ai = 0 (4.30)

In the case of (unconstrained) open-loop systems, all joint coordinates qi and hencethe virtual joint velocities qv

i are independent of each other. Equations (4.29) thusdegenerate into a set of N equations

Ai = 0 (i ∈ MBS) (4.31)

An arbitrary factor AK can be identified as:

AK =∂

∂qvK

(∑

i∈MBS

Aiqvi ) (4.32)

=∑

i∈MBS

∂ΩvT

i

∂qvK

(Pi + Ωi × Pi − Ti) (4.33)

=∑

i∈Des(K)

KT Fi ET

i (Pi + Ωi × Pi − Ti) (4.34)

42 CHAPTER 4

= ETK(P∗

K + ΩK × P∗

K − T∗

K) (4.35)

which is not surprisingly the same result as was obtained in previous section.Des(K) denotes the set of bodies containing K and all its descendants, i.e. Des(K) =K ∪ Des(K). To perform the step from (4.34) to (4.35), appendix D.4.3 needs tobe applied repeatedly till all the bodies are combined.

4.5.3 JOINT FORCES

Joint forces are forces coupling two consecutive bodies along the degrees of free-dom. If the generalized forces associated with coordinates qK are given by τK ,the resulting spatial force is represented by EKτK . Joint forces can be induced byactuators, springs and dampers for instance. Joint forces always influence two bod-ies: if body K feels a force EKτK , then its parent feels a force −(P a(K)T F

K EKτK).This is nothing but the law of action and reaction. Looking at the expression ofthe accumulated force vector (4.22), one can see that all the outboard joint forcesvanish. The only remaining joint force is that of the considered body itself. Thismeans that the joint forces need not to be taken into account to calculate theaccumulated forces, provided an extra term is added to the equations of motion:

pK = ETKT∗

K − ETK(ΩK × P∗

K) + ETKP∗

K + τK (4.36)

Making a classification into external and joint forces is not necessary, but it mayslightly simplify the equations.

4.6 JOINT VELOCITIES

To complete the set of Hamiltonian equations, an expression for the joint velocities(4.17a) is required. Again, the partial derivatives of the Hamiltonian function tothe canonical momenta could be worked out explicitly, but this would be cumber-some. Instead, one can use (3.39a) as inspiration to devise a recursive method toobtain these joint velocities. Presume that the articulated momentum vector canbe written under following form:

P∗

K = M∗

KΩK + DK (4.37)

M∗

K being the articulated mass matrix and DK the momentum remainder vector.This expression is certainly true for a leaf body N and boundary conditions DN = 0

and M∗

N = MN . It will be proved by induction that it holds for an arbitrary bodyK as well. An expression for joint velocity qK can be obtained by projecting (4.37)onto the joint subspace


pK = ETKP∗

K = ETK(M∗

KΩK + DK) (4.38)

= ETK [M∗

K(KT VPa(K)ΩPa(K) + EK qK + ΩtK

) + DK ] (4.39)

⇒ qK = M∗−1

jK[pK − dK − ET

KM∗

K(KT VPa(K)ΩPa(K) + ΩtK

)](4.40)

with d = ET D and M∗

j being the joint mass matrix defined as

M∗

j = ET M∗E (4.41)

The joint mass matrix is nothing else than the articulated mass matrix projectedinto the subspace of the degrees of freedom. Provided that the articulated massmatrices and the remainder momentum vectors are known, one can see from (4.40)that the joint velocities can be obtained using a forward recursion. Indeed, the spa-tial velocity of the ground Ω0 is zero and q1 can therefore be calculated directlyusing (4.40). This enables to compute the spatial velocity of body 1. Again revert-ing to (4.40), the joint velocities of the children can be obtained and so on. Let’snow prove equation (4.37) and find an expression for the articulated mass matrixM∗

K and the remainder momentum vector DK . This can be done by eliminatingqK from (4.37) by means of (4.40) and rearranging the terms:

P∗

K = M∗

K(KT VPa(K)ΩPa(K) + EK qK + ΩtK

) + DK (4.42)

= M′

K(KT VPa(K)ΩPa(K) + ΩtK

) + D′

K (4.43)

with

M′

K = M∗

K − M∗

KEKM∗−1

jKET

KM∗

K (4.44)

D′

K = M∗

KEKM∗−1

jK(pK − dK) + DK (4.45)

M′ is called the reduced mass matrix. D′ is a reduced remainder term. Supposethat expression (4.43) holds not only for body K, but for all its siblings as well.Substitution of (4.43) in the definition of the articulated momentum vector (4.13)and writing Pa instead of Pa(K) for the sake of clarity gives

P∗

Pa = PPa +∑

i∈Ch(Pa)

P aT Fi P∗

i (4.46)

= M∗

PaΩPa + DPa (4.47)

44 CHAPTER 4

with

M∗

Pa = MPa +∑

i∈Ch(Pa)

P aT Fi M′

iiT V

Pa (4.48)

DPa =∑

i∈Ch(Pa)

P aT Fi (M′

iΩti+ D′

i) (4.49)

This is precisely of the shape presumed in equation (4.37). Hence it is shown thatequation (4.48) holds for a body Pa if it holds for all its children. It was shownearlier that the equation is true for the leaf bodies as well: the proof by induction isthus completed. As (4.46) shows, two terms contribute to the total momentum ofan articulated body. The last term D takes the additional motion induced by theoutboard joints into account. The first term accounts for the momentum associatedwith the spatial velocity of the whole articulated body. The articulated mass matrixis not the combination of all mass matrices of the descendant bodies, but ratherthe combination of all inertia components that are transmitted through the joints.Consider two different mechanism: one is a double pendulum (fig. 4.4) with eachlink having mass m and length l, the other is a single pendulum (fig. 4.4(a)) withmass 2m and length 2l. It is intuitively clear that less effort is needed to obtaina small rotation about joint axis Z of the double pendulum. This is because theinertia associated with the joint axis of the second body is not transmitted throughthe pin-joint. On the other hand, performing a rotation about a perpendicular axiswill have the same effect in both cases: inertia properties are fully transmittedalong the constrained dimensions of a joint. The mass matrix one would obtain ifall joints of the considered articulated body were replaced by rigid connections iscalled the composite inertia matrix. It is used in the method of the same name[Walker & Orin, 1982].

4.7 JOINT ACCELERATIONS AND JOINT RE-ACTION FORCES

Accelerations and reaction forces are needed in certain applications. ModellingCoulomb friction and stiction requires the joint reaction forces to be known. Somesimulations may require the monitoring of reaction forces in critical parts. Thesimulator may even perform a structural change of the multibody model to complywith part failures due to the reaction forces exceeding a certain threshold. Unfor-tunately, joint accelerations and reaction forces cannot be calculated easily usingthe Hamiltonian formulation and two additional recursion steps are required. Thegood news is that these recursion steps are very similar to the ones described inthe previous section.


(a) Single pendulum (b) Double pendulum

Figure 4.4: Two systems with same total mass and length

4.7.1 TIME DERIVATIVE OF THE ARTICULATED MO-MENTUM VECTOR

The method for obtaining the joint accelerations q and the time derivative of themomentum vectors P is quite similar to the algorithm for the forward dynamics.First take a look at the momentum vector of an arbitrary leaf body N .

PN = MNΩN = MN (NT VN−1ΩN−1 + EN qN ) (4.50)

Taking the time derivative and using (D.21) and (D.22) results in:

PN = MN [NT VN−1ΩN−1 + (EN qN )T × NT V

N−1ΩN−1 + EN qN + EN qN ] (4.51)

from which the expression for the joint accelerations follows

qN = M−1jN

ETN [PN − MN (EN qN )T × NT V

N−1ΩN−1 − MNNT V

N−1ΩN−1 − EN qN ](4.52)

Substitution in (4.51) finally gives:

PN = M′

N [NT VN−1ΩN−1 + (EN qN )T × NT V

N−1ΩN−1 + EN qN ] + MNENM−1jN

pN

= M′

NNT V

N−1ΩN−1 + D′

N (4.53)

with

D′

N = M′

N (EN qN )T × NT VN−1ΩN−1 + M′

N EN qN + MNENM−1jN

pN (4.54)

46 CHAPTER 4

The only unknown in this expression is the spatial acceleration ΩN−1. It is com-puted in a forward recursion step, using

ΩK = KT VK−1ΩK−1 + (EK qK)T × KT V

K−1ΩK−1 + EK qK + EK qK (4.55)

Note that the reduction of the spatial velocity of body N−1 to its outboard body Nis already calculated in the derivation of the equations of motion. The articulatedmomentum vector of body K = N − 1 is given by

P∗

K = PK + KT FK+1P

∗

K+1 (4.56)

Thus, its time derivative is (use appendix (D.18)):

P∗

K = PK + KT FK+1

dKP∗

K+1

dt+

dKKT FK+1

dtP∗

K+1

= MKΩK + KT FK+1(EK+1qK+1 × P∗

K+1) + KT FK+1P

∗

K+1

= MKΩK + KT FK+1(EK+1qK+1 × P∗

K+1) + KT FK+1(M

′

K+1K+1T V

K ΩK + D′

K+1)

= M∗

KΩK + DK (4.57)

with

DK = KT FK+1D

′

K+1 + KT FK+1(EK+1qK+1 × P∗

K+1) (4.58)

One can use the same procedure as above to find the other joint accelerations:

P∗

K = M∗

K [KT VK−1ΩK−1 + (EK qK)T × KT V

K−1ΩK−1 + EK qK ] + DK (4.59)

from which the expression for the joint acceleration follows

qK = M∗−1jK

ETK [P∗

K − DK − M∗

K(EK qK)T × KT VK−1ΩK−1 − M∗

KKT V

K−1ΩK−1](4.60)

Substitution in the articulated momentum vector for further recursions gives:

P∗

K = M′

K [KT VK−1ΩK−1 + (EK qK)T × NT V

K−1ΩK−1]

+M∗

KEKM∗−1jK

(pK − ETKDK) + DK (4.61)

= M′

KKT V

K−1ΩK−1 + D′

K (4.62)


with

D′

K = M′

K(EK qK)T × KT VK−1ΩK−1

+ M∗

KEKM∗−1jK

(pK − ETK dK) + DK (4.63)

This procedure can be carried on until the base body is reached, whose accelerationis known.

4.7.2 JOINT REACTIONS

The equations of motion for a rigid body interconnected with other bodies is givenby:


∑

i∈Ch(K)

KT Fi Tri

(4.64)

When using articulated bodies, the reaction forces and torques introduced by theoutboard bodies are eliminated, as said before:

P∗

K + ΩK × P∗

K = T∗

K + TrK(4.65)

If the time derivative of the articulated momentum vector is calculated using themethod described previously, the spatial joint reaction force can be computed:

TrK= P∗

K + ΩK × P∗

K − T∗

K (4.66)

4.8 ALGORITHM

In a few special cases, the algorithm for formulating the equations of motion canbe made of two recursion steps, but in most practical cases, three are needed. Firstthe multibody system is swept through starting from the utmost bodies down tothe base body. During this backward recursion process, the articulated mass prop-erties and the remainder momentum vectors are calculated. Both these quantitiesare only dependent on the joint coordinates and their conjugated momenta, i.e.the state coordinates. In the second, forward recursion step, the velocities andmomentum vectors are propagated from the base to the tip(s). Additionally, thehomogeneous transformation matrices are calculated. These are solely needed toexpress the external forces with respect to the local frames. Indeed, most externalforces, like gravitational forces, are given in the inertial frame. In the last, backwardrecursion step, the accumulated forces are derived and the remaining Hamiltonianequations can then be computed. The algorithm is presented in pseudo-code (al-gorithm 1). The recursive nature of the algorithm is emphasized by the recursive

48 CHAPTER 4

calls in the functions 2,3 and 4. Note that the structure of the algorithm is fairlysimple, the more difficult part is the implementation and optimization of the func-tions. In case no external forces are present or if they are given in the local axes,the algorithm can be reduced to two recursion steps: the calculation of the accu-mulated force vectors can be moved to the first backward recursion step and thecomputation of the time derivative of the canonical momenta to the forward recur-sion step. Gravitational forces can be added by applying a constant accelerationg(gravitational field) to the ground.

Algorithm 1 Calculate equations of motion for open-loop MBS

set i = baseBodyfirst backward recursion step(i)forward recursion step(i)second backward recursion step(i)

Function 2 First backward recursion step (body i)

for j = firstChild to lastChild do

first backward recursion step(j)end for

compute articulated mass matrix(i)compute remainder momentum vector(i)compute reduced mass matrix(i)compute reduced remainder vector(i)

Function 3 Forward recursion step (body i)

compute coordinate velocity(i)compute spatial velocity(i)compute homogeneous transformation matrix(i)for j = firstChild to lastChild do

forward recursion step(j)end for

In the case only pin-joints are used, a thorough inspection of the algorithm revealsa maximum of 363 arithmetical operations are needed for each body. Due tosimplifications at the first and last bodies, this amount is reduced with at least475 operations for the complete mechanism. This can be written as: 363n − 475,with n the number of bodies (degrees of freedom). This formula is applicable forn ≥ 3. For comparison, a list of acceleration based algorithms and their amountof operations is shown table 4.1. One should be careful with the interpretation ofthese results. The figures in the table are often given without any specification on


Function 4 Second backward recursion step (body i)


second backward recursion step(j)end for

compute accumulated force vector(i)compute time derivative of canonical momenta(i)

Algorithm Add. Mult. Total[Featherstone, 1987] 275n − 18 336n − 220 611n − 238[Vukobratovic et al., 1994] 231n − 294 249n − 272 480n − 566[Stejskal & Valasek, 1996] 206n − 345 226n − 343 432n − 688[Rein, 1993] 195n − 247 216n − 317 411n − 669[Naudet et al., 2003] 178n − 230 185n − 245 363n − 475

Table 4.1: Number of required operations

which optimizations are performed, further reduction of the number of requiredoperations may be possible. The figures are thus upper-bounds. Furthermore, thenumber of operations to formulate the equations of motion is not the only factoraffecting the simulation speed. Different methods may display a different behaviorduring numerical integration. A few examples are provided in chapter 7.

4.9 CONCLUSIONS

It is possible to devise a recursive algorithm to establish a set of Hamiltonian equa-tions of motion for open-loop multibody systems. The proposed algorithm is basedon the well-known articulated body approach and introduces the concept of articu-lated momentum vectors. A comparison with similar acceleration-based methodsshowed that less arithmetical operations are needed to obtain the equations, whichis an important step towards faster simulations. Furthermore, the accelerationsand joint reaction forces can be derived at a moderate extra computational cost,by appending two additional recursion steps.

50 CHAPTER 4

Chapter 5

CONSTRAINED MULTIBODY

SYSTEMS

“Algebra is generous: she often gives more than is asked for.”

-Jean Le Rond D’Alembert

When dealing with generic mechanical systems, one is almost inevitably con-fronted with constraints. As was shown in the previous chapter, it is possible totreat open-loop systems as unconstrained systems by using relative or joint coor-dinates. However, when kinematical loops are present, a relationship is createdbetween those coordinates. The same applies for nonholonomic (Pfaffian) con-straints: these will introduce a relationship between the canonical momenta.

A straightforward method to tackle additional constraints would be to replacethe constraints by spring-damper systems. Choosing high stiffness coefficients andadequate damping may provide satisfying accuracy to the simulation. Unfortu-nately, these very stiff springs wreak havoc to the numerical integration, requiringintolerably small time steps.

The additional constraints lead to a set of differential-algebraic equations (DAE) ofindex 2 (see appendix C.6). These equations may be directly integrated using DAE-solvers, but it is often preferable to convert them into a set of ordinary differentialequations (ODE). There exist two popular ways of performing this conversion. Thefirst way is to reduce the differential index of the DAE by differentiating the con-straint equations once. This allows one to eliminate the Lagrange multipliers fromthe equations and to do the numerical integration using the classical ODE-solvers.This method has two main disadvantages: it requires stabilization methods for theconstraint violations, and a recursive approach leads to O(n + nm + nm2 + m3)efficiency, m being the number of constraints [Lilly & Orin, 1994, Bae et al., 1987].The other method consists in reducing the number of coordinates describing the sys-tem, such as to obtain a formulation in terms of independent coordinates only. Itis possible to design such a method with O(n) efficiency [Stejskal & Valasek, 1996,

51

52 CHAPTER 5

Anderson & Critchley, 2003]. However, this method is prone to singularities whenthe choice of independent coordinates is inadequate.

In this chapter, the recursive algorithm to obtain a set of Hamiltonian equationsof motion is extended from open-loop to constrained multibody systems. Twomethods are proposed: one based on Lagrange multipliers, the other based oncoordinate reduction. It is shown that the first method is a good choice for lightlyconstrained systems, while the second method keeps its O(n)-efficiency even forheavily constrained mechanisms. It is assumed that the additional constraints arenot redundant.

5.1 TERMINOLOGY OF A CLOSED-LOOPMULTIBODY SYSTEM

For closed-loop systems, it is not evident to speak about children and parents. Inorder to keep this open-loop terminology, joint cuts1 can be performed to obtainan underlying open-loop tree structure. An example will clarify this. Figure 5.1shows a closed-loop system with two kinematic loops. There are two connectionswith the ground (0 and 12) and one inner loop (bodies 3 to 9). One can obtain anopen-loop system by breaking certain joints: the joint between bodies 9 and 3 andthe joint between bodies 11 and 12 for instance. In general, this can be achievedin many different ways (e.g. any joint in the inner loop can be removed). Theremoved joints are called constraint joints. When sweeping through the tree in theoutboard direction, the first element of an inner loop that is encountered is calledthe loop base. In the figure, body 3 is a loop base. It has only one child (4), body9 is only a descendant.

5.2 METHOD 1: AUGMENTED LAGRANGIAN

As was seen in chapter 2, constrained multibody systems can be solved by conside-ring a variational problem with boundary conditions. This led to the definition ofthe so-called augmented Lagrangian function. The resulting equations of motioncan also be obtained recursively by introducing an augmented momentum vector.The method will be explained by means of a relatively simple example. The generalapproach will be discussed afterwards.

1As a matter of fact, body cuts can be performed as well. The drawback is that they introducesix extra degrees of freedom. Body cuts are not considered in this thesis.

CONSTRAINED MULTIBODY SYSTEMS 53

Figure 5.1: Terminology of a closed-loop multibody system

5.2.1 AUGMENTED MOMENTUM VECTOR

Consider a planar chain (fig.5.2). Its links are connected by pin-joints and onekinematical loop is created by means of an additional pin-joint. The equations ofmotion of the last body N are

PN + ΩN × PN = TN + TrN− NT F

N+1TrN+1(5.1)

= TN + TrN− NT F

N+1EcN+1λ (5.2)

N + 1 denotes the fixed ground at the position of the loop closure. λ is a set ofLagrangian multipliers. In the general case, it would be denoted by λN+1, but theindex is dropped now for the sake of clarity. The constraint matrix Ec representsthe subspace of reaction forces and torques. It is obviously orthogonal to the jointsubspace. Hence, following properties hold:

ETc E = 0 (5.3)

ET Ec = 0 (5.4)

54 CHAPTER 5

Figure 5.2: Chain with both ends fixed

The union of the constraint and the joint subspaces spans the whole 6-dimensionalCartesian space. In this specific example concerning a pin-joint, EcN+1

is given by:

N+1EcN+1=

1 00 10 00 00 00 0

(5.5)

and thus EcN+1= 0. Let’s now define the augmented momentum vector P+ as

follows:

P+N = PN + NT F

N+1EcN+1σ (5.6)

with σ a set of Lagrange multipliers as introduced in chapter 2. Time derivationresults in


P+N = PN + NT F

N+1EcN+1σN+1 + NT F

N+1

dNEcN+1

dtσ + NT F

N+1EcN+1σ

= PN + NT FN+1EN+1qN+1 × EcN+1

σ + NT FN+1EcN+1

λ (5.7)

with σ = λ. In other words, as λ is a measure for the reaction forces and moments,σ is a measure for the reaction impulses and impulse moments. Note that T F andEcN+1

are zero for pin-joints. Substitution in (5.1) gives

P+N + ΩN × P+

N = TN + TrN+ ΩN × NT F

N+1EcN+1σ

+ NT FN+1EN+1qN+1 × EcN+1

σ (5.8)

= TN + TrN+ NT F

N+1(ΩN+1 × EcN+1σ) (5.9)

= TN + TrN(5.10)

The last term in (5.9) is zero, because the constraint equation ΩN+1 = 0 must befulfilled. Projection into the joint subspace leads to:

p+N = −ET

N (ΩN × P+N − TN ) (5.11)

The equations of motion for the remaining bodies can be found using a backwardrecursion, just as for open-loop systems (4.27). The only difference is that the aug-mented momentum vectors are used instead of the articulated momentum vectors:

P+K = PK + KT F

K+1P+K+1 (5.12)

= P∗

K + KT FN+1EcN+1

σ (5.13)

Projection on the joint subspace gives the canonical momenta associated to theaugmented formulation:

p+K = ET

KP+K = pK + ET

KKT F

N+1EcN+1σ (5.14)

The equations of motion thus become:

p+K = −ET

K(ΩK × P+K − T∗

K) (5.15)

5.2.2 JOINT AND SPATIAL VELOCITIES

The Lagrange multipliers σ are unknown. To find them, the constraint equationsmust be solved, but they first need to be expressed in terms of the known quantities:

56 CHAPTER 5

the generalized coordinates q and the (augmented) canonical momenta p+. It ispossible to conceive a method similar to the open-loop method. The problemresides in the calculation of the joint velocities q. The forward recursion steps canstill be applied, but only symbolically: the real values of the joint velocities cannotbe computed, because they are dependent on the unknown σ. Once a symbolicexpression is obtained for the spatial velocity ΩN , one can solve for the Lagrangemultipliers σ and substitute their values in the symbolical expression of the jointvelocities q.

Let’s now find an expression for the joint and spatial velocities by using thedefinition of the augmented momentum vector (5.6). Starting from the leaf body,one gets

p+N = ET

N (MNΩN + NT FN+1EcN+1

σ) (5.16)

= ETNMN (NT V

N−1ΩN−1 + EN qN ) + ETN

NT FN+1EcN+1

σ (5.17)

Further developments lead to

qN = M−1jN

(p+N − ET

NMNNT V

N−1ΩN−1 − ETN

NT FN+1EcN+1

σ) (5.18)

Plugging this last equation into the expression for the augmented momentum vector(5.6) produces

P+N = M

′

NNT V

N−1ΩN−1 + D′

N + Dσ′

N σ (5.19)

with

M′

N = MN − MNENM−1jN

ETNMN (5.20)

D′

N = MNENM−1jN

p+N (5.21)

Dσ′

N = (I − MNENM−1jN

ETN )NT F

N+1EcN+1(5.22)

Note that the reduced momentum remainder vector D′

N is now expressed in termsof p+ instead of p, but it does not formally differ from the open-loop expression(4.44). Performing a backward propagation, the next body encountered is N − 1.Its augmented momentum vector can be written as:

P+N−1 = PN−1 + N−1T F

N P+N


= MN−1ΩN−1 + N−1T FN (M

′

NNT V

N−1ΩN−1 + D′

N−1 + Dσ′

N σ)

= M∗

N−1ΩN−1 + DN−1 + Dσ

N−1σ (5.23)

with

M∗

N−1 = MN−1 + N−1T FN M′

NNT V

N−1 (5.24)

DN−1 = N−1T FN D′

N (5.25)

Dσ

N−1 = N−1T FN Dσ

′

N (5.26)

The expressions for the articulated mass matrix and the remainder momentumvector are unchanged. The joint velocities are readily found:

qN−1 = M∗−1

jN−1(p+

N−1 − ETN−1DN−1 − ET

N−1M∗

N−1N−1T V

N−2ΩN−2

− ETN−1D

σ

N−1σ) (5.27)

Substitution into the augmented momentum vector yields

P+N−1 = M

′

N−1N−1T V

N−2ΩN−2 + D′

N−1 + Dσ′

N−1σ (5.28)

and

M′

N−1 = M∗

N−1 − M∗

N−1EN−1M∗−1

jN−1ET

N−1M∗

N−1 (5.29)

D′

N−1 = M∗

N−1EN−1M∗−1

jN−1(pN−1 − ET

N−1DN−1) + DN−1 (5.30)

Dσ′

N−1 = (I − M∗

N−1EN−1M∗−1

jN−1ET

N−1)Dσ

N−1 (5.31)

This procedure can be followed till the base body is reached. The next step toundertake is to establish an expression for the joint and the spatial velocities interms of the Lagrange multipliers. Consider base body 1. Its joint velocities can besplit into two terms: a contribution qu coming from the underlying unconstrainedsystem and an additional term to account for the constraints:

q1 = qu1 + γσ

1 σ (5.32)

58 CHAPTER 5

wherein

qu1 = M∗

−1

j1 (p+1 − ET

1 D1) (5.33)

γσ = −M∗−1

j1 ET1 Dσ

1 (5.34)

γσ denotes the coefficient associated to the Lagrange multipliers σ. Similarly,the spatial velocity of body 1 can be formulated with an unconstrained and aconstrained part.

Ω1 = E1q1 = Ωu1 + Γσ

1 σ (5.35)

with

Ωu1 = E1q

u1 (5.36)

Γσ

1 = E1qσ

1 (5.37)

Further outwards propagation gives the general expressions:

qK = quK + γσ

Kσ (5.38)

ΩK = ΩuK + Γσ

Kσ (5.39)

with

quK = M∗

−1

jK(p+

K − ETKDK − ET

KM∗

KKT V

K−1ΩuK−1) (5.40)

ΩuK = KT V

K−1ΩuK−1 + EK qu

K (5.41)

γσ

K = −M∗−1

jK(ET

KM∗

KKT V

K−1Γσ

K−1 + ETKDσ

K) (5.42)

Γσ

K = KT VK−1Γ

σ

K−1 + EKγσ

K (5.43)

5.2.3 RESOLVING THE CONSTRAINTS

The constraint equations can be formulated as

ΩN+1 = 0 (5.44)


The joint velocities qN+1 associated with the constraint joint are dependent on thespatial velocity ΩN . One can write

ΩN+1 = N+1T VN ΩN + EN+1qN+1 = 0 (5.45)

⇒ qN+1 = −(ETN+1EN+1)

−1ETN+1

N+1T VN ΩN (5.46)

One can recognize the Moore-Penrose pseudo-inverse (ETN+1EN+1)

−1ETN+1. Sub-

stitution of (5.46) in (5.45) gives

CN+1N+1T V

N ΩN = 0 (5.47)

with

CN+1 = I − EN+1(ETN+1EN+1)

−1ETN+1 (5.48)

Note that, taking (5.3) into account,

ETcN+1

CN+1 = ETcN+1

[I − EN+1(ETN+1EN+1)

−1ETN+1] = ET

cN+1(5.49)

Premultiplying (5.47) by ETcN+1

thus yields

ETcN+1

N+1T VN ΩN = 0 (5.50)

which is a set of m equations in the n unknowns q. m is the dimension of theconstraint subspace (2 in this example). Substituting the expression for the spatialvelocity of the leaf body gives

ETcN+1

N+1T VN Γσ

Nσ = −ETcN+1

N+1T VN Ωu

N (5.51)

⇒ σ = −(ETcN+1

N+1T VN Γσ

N )−1ETcN+1

N+1T VN Ωu

N (5.52)

Observe that the inverted matrix has dimensions m × m = 2 × 2. A singularbehavior is possible in certain configurations.

5.2.4 GENERAL APPROACH

In generic multibody systems, several constraint joints may be present. In thatcase, the procedure needs only small modifications. Suppose there are m con-straints. The Lagrange multipliers associated to constraint i are denoted by σi.The definition of the augmented momentum vector must then be modified to con-tain a contribution of all outboard constraints:

60 CHAPTER 5

P+K = P∗

K +∑

i∈OCJ(K)

KT Fi Eci

σi (5.53)

= PK +∑

i∈Ch(K)

P+i (5.54)

= M∗

KΩK + DK +∑

i∈OCJ(K)

Dσi

K (5.55)

OCJ(K) stands for Outboard Constraint Joint. It represent the sets of all con-straints that lay outboard from body K. When the base body is reached, theforward recursion starts. The joint and spatial velocities are of the following form:

qK = quK +

∑

i∈CJ

γσi

K σi (5.56)

ΩK = ΩuK +

∑

i∈CJ

Γσi

K σi (5.57)

Note that CJ is used now. It stands for Constraint Joints and represents the setof all constraint joints in the system. All the constraint joints are propagated tothe base, and they must all be transmitted back to the constraints. After that,the constraint equations are joined together to produce a set of linear equationsAσ = b with σ the vector containing all the system’s Lagrange multipliers. Ittakes O(m2) operations to build the matrix A, O(m3) operations to inverse it andO(m2) operations for the multiplication A−1b. Add the O(n) operations for thebasic algorithm and the O(nm) additional ones to compute Dσm

n and Γσm

n and thetotal order of the algorithm becomes O(n + nm + m2 + m3).

It is now time to say a few words about inner-loops. An inner-loop is created byimposing a constraint between two links of the system. The reaction forces andmoments that appear on both links are opposite. These two opposite reactions canbe treated like multiple constraints and propagated towards the base body. Whenthe loop base is reached however, the two contributions of the constraint canceleach other and the dependence on the Lagrange multiplier disappears. From thatpoint on, the multibody system can be treated as an open-loop system.

5.2.5 SUMMARY OF THE METHOD

Three sweeps are needed to obtain the full equations of motion of the closed-loopsystem, just as for open-loop systems. In a first backward recursion step, thearticulated mass matrices M∗, the remainder momentum vectors D and Dσ are


calculated. In the subsequent forward recursion step, the unconstrained velocitiesΩu and qu and the constrained contributions Γσ and γσ are computed. Afterthe forward recursion step, the Lagrange multipliers σ are obtained by solving theconstraint equations. In the third, backward recursion step, the joint and spatialvelocities q and Ω are computed, together with the accumulated force vectorsT∗, the augmented momentum vectors P+ and the derivative of the augmentedcanonical momenta p+. The method is given in pseudo-code in algorithm 5. Thedifferences with the open-loop method are written in italic. The word list is usedto emphasize that one matrix is needed for each constraint.

As explained earlier the algorithm has O(n + nm + m2 + m3)-efficiency. Thecubic order makes it unsuitable for heavily constrained bodies. The nm-order canalso penalize systems with a moderate number of long constrained branches. Onthe other hand, the method is efficient for mechanisms with many independentinner-loops containing a small amount of elements.

Algorithm 5 Calculate equations of motion for closed-loop MBS using Lagrangemultipliers

set i = baseBodyfirst backward recursion step(i)forward recursion step(i)solve constraint equationssecond backward recursion step(i)

Function 6 First backward recursion step (body i)


first backward recursion step(j)end for

compute articulated mass matrix(i)compute remainder momentum vector(i)compute reduced mass matrix(i)compute reduced remainder vector(i)compute list of Dσ(i)compute list of Dσ′

(i)

62 CHAPTER 5

Function 7 Forward recursion step (body i)

compute unconstrained coordinate velocity(i)compute list of γσ(i)compute unconstrained spatial velocity(i)compute list of Γσ(i)compute momentum vector(i)compute homogeneous transformation matrix(i)for j = firstChild to lastChild do

forward recursion step(j)end for

Function 8 Second backward recursion step (body i)


second backward recursion step(j)end for

compute coordinate velocities(i)compute spatial velocity(i)compute accumulated force vector(i)compute augmented momentum vector(i)compute time derivative of canonical momenta(i)

5.3 METHOD 2: MINIMAL COORDINATES

The second method to obtain the equations of motion of closed-loop systems pro-posed in this thesis is based on a coordinate reduction. A set of independentcoordinates and their conjugated canonical momenta are chosen as state coordi-nates, resulting in a minimal formulation. To obtain a recursive algorithm usingthe principle of virtual power, the virtual spatial velocities of the dependent bodiesneed to be written in terms of the independent velocities. The method is quite com-plicated and revealing the general approach at once is likely to drown the reader inan overflow of equations. Therefore, a simple example is treated first. It is shownhow the equations of the planar four-bar mechanism depicted in figure 5.3 can bederived in a recursive way. It will become clear that the choice of independentcoordinates plays an important role. Choosing the dependent bodies at the end ofthe underlying open-loop structure is much more convenient than any other arbi-trary choice. Unfortunately, that choice is not always possible due to singularityissues. Two different sets of independent coordinates are considered, as shown onfigure 5.3.


(a) Two last links are dependent (b) Arbitrary selection

Figure 5.3: Four-bar mechanism

5.3.1 EXAMPLE 1: DEPENDENT COORDINATES ATEND OF UNDERLYING OPEN-LOOP STRUCTURE

The considered example is a planar2 chain with both ends fixed to the groundby means of pin-joints. There are four joint coordinates but only two degrees offreedom. Joint 5 is chosen to be the constraint joint. Removing it produces theunderlying open-loop structure. The last two bodies (3 and 4) are considered asdependent (the dark links on fig. 5.3(a)). Writing the principle of virtual powerfor the whole system yields (see also appendix B.2.2)

4∑

i=1

ΩvTi (Pi + Ωi × Pi − Ti) = 0 (5.58)

The major difference with the open-loop case is that the virtual joint velocitiesqv

i are not independent of each other. In this example qv1 and qv

2 are chosen asindependent coordinates and the principle of virtual power can thus be written as(see section 4.5.2)

AK =

4∑

i=1

∂ΩvTi

∂qvK

(Pi + Ωi × Pi − Ti) = 0 for K = 1, 2 (5.59)

Being able to express the dependent spatial velocities in terms of the independentones enables one to obtain the coefficients AK recursively, just as for open-loop

2The only reason why a planar system is used, is to restrict the number of constraints. Closingthe chain in the example introduces two constraints. In the 3-dimensional case, a pin-joint wouldcreate 5 constraints. The spatial formulation for both systems is the same though, only thenumber of dependent coordinates is different. This is why the 6-dimensional spatial notation isstill used in the example.

64 CHAPTER 5

systems. The two equations of motion will turn out to be of the form

ETK(P∗

c

K + ΩK × P∗c

K − T∗c

K ) = 0 (5.60)

with P∗c

and T∗c

the so-called constrained articulated momentum vector andconstrained accumulated force vector. To introduce vector P∗

c

, it will be necessaryto take a closer look at the expression of the canonical momenta. The constrainedarticulated momentum vector will also allow to compute the joint velocities.

Let’s summarize the procedure that needs to be followed to obtain the equationsof motion: Express the dependent spatial velocities in terms of the independent ones. Use the definition of the canonical momenta to find an expression for P∗

c

. Use the principle of virtual power to get equations of the form (5.60). Get the independent joint velocities from P∗c

.

The last step will be lengthy, but quite straightforward.

REDUCTION OF DEPENDENT SPATIAL VELOCITIES

To reduce the dependent spatial velocities Ω3 and Ω4 to the independent ones, onestarts with the first form3 of the constraint equations:

Ω5 = 0 (5.61)

Reference frame 5 represents the ground at the loop closure. It has an associated(dependent) constraint joint q5 that will be used as intermediate quantity only.Note that above expressions of the constraint equations are redundant: there aresix equations for only two constraints. The decomposition of the spatial velocitygives

Ω5 = 5T V4 Ω4 + E5q5 = 0 (5.62)

After projection on the subspace E5, an expression for the joint velocity of body 5is obtained:

q5 = −(ET5 E5)

−1ET5

5T V4 Ω4

3Some authors number the different forms of the constraint equations the other way aroundand would call this the 5th form [Anderson & Critchley, 2003].


= CTq5

5T V4 Ω4 (5.63)

Substitution of (5.63) in (5.62) results in

Ω5 = C55T V

4 Ω4 = 0 (5.64)

withC5 = I + E5C

Tq5

(5.65)

Matrix C is called the constraint matrix. Equation (5.64) is the second form of theconstraint equations. One can now start with the second form of the constraintequations and repeat the procedure to continue the reduction towards the inde-pendent bodies. Premultiplying equations (5.64) by ET

44T F

5 and decomposing thespatial velocity Ω4 results in

q4 = −(ET4

4T F5 C5

5T V4 E4)

−1(ET4

4T F5 C5

5T V4 )4T V

3 Ω3

= CTq4

4T V3 Ω3 (5.66)

Note that matrix (ET4

4T F5 C5

5T V4 E4) must not be singular. A case of singularity

would be the consequence of a bad partitioning in independent and dependentcoordinates. Ω4 can now be found as

Ω4 = 4T V3 Ω3 + E4q4 = C4

4T V3 Ω3 (5.67)

withC4 = I + E4C

Tq4

(5.68)

The third form of the constraint equations can also readily be derived by substitu-tion of (5.67) in (5.64) and premultiplication by 4T F

5 :

C44T V

3 Ω3 = 0 (5.69)

with

C4 = 4T F5 C5

5T V4 C4 (5.70)

C4 is a symmetrical constraint matrix. The procedure can be repeated once moreto attain the first independent body 2. This leads to following expressions:

q3 = CTq3

3T V2 Ω2 (5.71)

66 CHAPTER 5

Ω3 = C33T V

2 Ω2 (5.72)

with

CTq3

= −(ET3

3T F4 C4

4T V3 E3)

−1(ET3

3T F4 C4

4T V3 ) (5.73)

C3 = I + E3CTq3

(5.74)

The constraint matrices Ci and CTqi

are found during a backward recursion step, thejoint and spatial velocities during a forward recursion step. Note that CiCi = Ci,this means that the constraint matrix is a projection operator. Note also thatCiEi = 0, so Ci lies in the subspace orthogonal to the joint subspace Ei. Remem-ber that the objective of the velocity reductions was to eliminate the dependentvirtual spatial velocities from the principle of virtual power. This can now beachieved, the dependent spatial velocities can indeed be written as

Ω3 = C33T V

2 Ω2 (5.75)

Ω4 = C44T V

3 C33T V

2 Ω2 (5.76)

CANONICAL MOMENTA

Before applying the principle of virtual power, it is necessary to take a closer lookat the canonical momenta associated with the independent coordinates. It is im-portant to realize that no canonical momenta exist for the dependent coordinates,canonical momenta are only defined for the independent ones.

p2 = (∂L

∂q2)T = (

∂T

∂q2)T =

4∑

i=1

∂ΩTi

∂q2Pi (5.77)

= ET2 P2 + ET

22T F

3 CT3 P3 + ET

22T F

3 CT3

3T F4 CT

4 P4 (5.78)


The constrained articulated momentum vector4 of a body is defined as

P∗c

K = P∗

K +∑

i∈Dep

KT Fi Cqi

ETi P∗

i (5.79)

with

Cq3= Cq3

(5.80)

Cq4= Cq4

+ 4T F3 Cq3

ET3

3T F4 Cq4

(5.81)

Dep is the set of all the dependent bodies. The reader may develop equations (5.78)(using (5.74) and (5.70)) to check these expressions or take a look at appendix D.7for a general expression. The details of the expressions are of little importancethough, the important thing is to note that the constrained momentum vector P∗

c

is the sum of the articulated momentum vector P∗ and one additional contributionfrom each dependent body. The canonical momenta associated to body 2 are

p2 = ET2 P∗

c

2 (5.82)

The constrained momentum vectors for all the other bodies can also be found usingfollowing recursive relationship:

P∗c

K = P∗

K +∑

i∈Dep

KT Fi Cqi

ETi P∗

i (5.83)

= PK + KT FK+1P

∗

K+1 + KT FK+1

∑

i∈Dep

K+1T Fi Cqi

ETi P∗

i (5.84)

= PK + KT FK+1P

∗c

K+1 (5.85)

Similarly, one can readily find the canonical momenta for body 1:

p1 = ET1 P∗

c

1 (5.86)

4It may be tempting to define the constrained articulated momentum vector as

PcK = PK + KT F

K+1CTK+1P

cK+1

with C equal to the unity dyadic I for independent bodies. As shown in [Naudet & Lefeber, 2005],it is then possible to obtain the equations of motion for the specific choice of dependent coordinatesof this example. The definition does however not allow an arbitrary selection of dependentcoordinates. This is why the alternative definition (5.79) is adopted here.

68 CHAPTER 5

PRINCIPLE OF VIRTUAL POWER

Using the velocity reductions introduced in previous section, the coefficient A2 inthe principle of virtual power (5.59) can be found as (using (5.75) and (5.76)):

A2 =

4∑

i=1

∂ΩvTi

∂qv2

(Pi + Ωi × Pi − Ti) (5.87)

= ET2 (P2 + Ω2 × P2 − T2) (5.88)

+ ET2

2T F3 CT

3 (P3 + Ω3 × P3 − T3) (5.89)

+ ET2

2T F3 CT

33T F

4 CT4 (P4 + Ω4 × P4 − T4) = 0 (5.90)

One can recognize the same structure as in (5.78), which is not surprising since thecoefficients in both expressions arise from the partial derivatives ∂ΩT

i /∂qK . Thisallows one to use the same recursive relationships as for the constrained articulatedmomentum vector. I.e., the momentum vector Pi can formally be replaced byexpression Bi = Pi + Ωi × Pi − Ti in equations (5.79) and (5.85). Taking this inconsideration, A2 can be written as A2 = ET

2 B∗c

2 . Developing B∗c

4 yields

B∗c

4 = P4 + Ω4 × P4 − T4 + 4T F3 Cq3

ET3 (P∗

3 + Ω3 × P∗

3 − T∗

3)

+ Cq4ET

4 (P4 + Ω4 × P4 − T4) (5.91)

= P∗c

4 + Ω4 × P∗c

4 − T∗c

4 (5.92)

with (see appendix D.4.3)

T∗c

4 = T4 + 4T F3 Cq3

ET3 T∗

3 + Cq4ET

4 T4

+ 4T F3 [

˙Cq3

ET3 + (Ω3 × I)Cq3

ET3 − Cq3

ET3 (Ω3 × I)]P∗

3

+ [˙Cq4

ET4 + (Ω4 × I)Cq4

ET4 − Cq4

ET4 (Ω4 × I)]P4 (5.93)

As one can see, the constrained force vector T∗c

4 associated with the leaf body isquite involved. This is apparently the price to pay for adding constraints. Fortu-nately, the expressions are much simpler for the remaining bodies, they also remindof the open-loop equations. The expression for B∗

c

3 is:

B∗c

3 = P3 + Ω3 × P3 − T3 + 3T F4 (P∗

c

4 + Ω4 × P∗c

4 − T∗c

4 ) (5.94)


= P∗c

3 + Ω3 × P∗c

3 − T∗c

3 (5.95)

with

T∗c

3 = T3 + 3T F4 T∗

c

4 (5.96)

Finally, the expression for A2 can be found as:

A2 = ET2 B∗

c

2 = ET2 [P2 + Ω2 × P2 − T2 + 2T F

3 (P∗c

3 + Ω3 × P∗c

3 − T∗c

3 )]

= ET2 (P∗

c

2 + Ω2 × P∗c

2 − T∗c

2 ) (5.97)

with

T∗c

2 = T2 + 2T F3 T∗

c

3 (5.98)

Hence, A2 = 0 can be transformed into

p2 = ET2 (T∗

c

2 − Ω2 × P∗c

2 ) + ET2 P∗

c

2 (5.99)

Similarly for articulated body 1:

p1 = ET1 (T∗

c

1 − Ω1 × P∗c

1 ) + ET1 P∗

c

1 (5.100)

with

T∗c

1 = T1 + 1T F2 T∗

c

2 (5.101)

JOINT VELOCITIES

The independent coordinate velocities are needed to obtain the remaining Hamil-tonian equations. The dependent coordinate velocities q3, q4 and q5 were alreadycalculated in section 5.3.1. The independent joint velocities can be found by ma-nipulating the constrained articulated momentum vectors. Consider body 3 (see(5.78)):

P∗c

3 = CT3 (P3 + 3T F

4 CT4 P4) (5.102)

= CT3 (M3 + 3T F

4 CT4 M4C4

4T V3 )Ω3 (5.103)

= Mc33T V

2 Ω2 (5.104)

70 CHAPTER 5

M∗c

3 = CT3 (M3 + 3T F

4 CT4 M4C4

4T V3 )C3 (5.105)

M∗c

3 being the constrained mass matrix. For body 2, one subsequently gets:

P∗c

2 = P2 + 2T F3 P∗

c

3 (5.106)

= (M2 + 2T F3 M∗

c

33T V

2 )Ω2 (5.107)

= M∗c

2 Ω2 (5.108)

M∗c

2 = M2 + 2T F3 M∗

c

33T V

2 (5.109)

The joint velocity q2 can easily be derived from above equations:

p2 = ET2 P∗

c

2 = ET2 M∗

c

2 (2T V1 Ω1 + E2q2) (5.110)

q2 = (ET2 M∗

c

2 E2)−1(p2 − ET

2 M∗c

22T V

1 Ω1)

= M∗c−1

j2[p2 − ET

2 M∗c

22T V

1 Ω1] (5.111)

To obtain the joint velocity for body 1, one can substitute (5.111) in the expressionof the constrained momentum vector (5.106):

P∗c

2 = M∗c

2 (2T V1 Ω1 + E2q2) (5.112)

= M∗c

2 [2T V1 Ω1 + E2M

∗c−1

j2(p2 − ET

2 M∗c

22T V

1 Ω1)] (5.113)

= M′

22T V

1 Ω1 + D′

2 (5.114)

M′

2 = M∗c

2 − M∗c

2 E2M∗

c−1

j2ET

2 M∗c

2 (5.115)

D′

2 = M∗c

2 E2M∗

c−1

j2p2 (5.116)

Repeating the previous procedure gives:

P∗c

1 = P1 + 1T F2 P∗

c

2 (5.117)

= (M1 + 1T F2 M

′

22T V

1 )Ω1 + 1T F2 D

′

2 (5.118)

= M∗c

1 Ω1 + D1 (5.119)

M∗c

1 = M1 + 1T F2 M

′

22T V

1 (5.120)

D1 = 1T F2 D

′

2 (5.121)


The joint velocity is then:

p1 = ET1 P∗

c

1 = ET1 M∗

c

1 E1q1 + ET1 D1 (5.122)

q1 = (ET1 M∗

c

1 E1)−1(p1 − ET

1 D1)

= M∗c−1

j1(p1 − d1) (5.123)

The joint and spatial velocities can now be computed explicitly using a forwardrecursion.

CONCLUSION

The method based on coordinate reduction is very similar to the one designed foropen-loop systems. It is recursive and of O(n). It still comprises the 3 typicalrecursion sweeps: a backward recursion for computing the inertia properties, theremainder momentums and the constraint matrices C and Cq, followed by a for-ward recursion to solve the velocities and the constrained momentum vectors. Inthe final backward recursion, the constrained force vectors and the time derivativeof the canonical momenta are calculated.

5.3.2 EXAMPLE 2: ARBITRARY SELECTION OF INDE-PENDENT COORDINATES

The specific selection of independent coordinates in previous example had oneimportant consequence: the dependent spatial velocities could be reduced to theinboard independent bodies, and could therefore be treated during the first back-ward recursion step. If an arbitrary choice of independent coordinates is made,the dependent spatial velocities will influence outboard independent bodies as well,complicating the recursive relationships. This will become clear with the nextexample, where bodies 1 and 3 are chosen as dependent (fig.5.3(b)).

REDUCTION OF DEPENDENT SPATIAL VELOCITIES

The first (5.61) and second (5.64) forms of the constraint equations remain un-changed, but the third form becomes

C4(4T V

3 Ω3 + E4q4) = 0 (5.124)

with

C4 = 4T V5 C5

5T V4 (5.125)

72 CHAPTER 5

The premultiplication by 4T V5 is performed to obtain a symmetric expression of the

matrix. Joint velocity q4 is independent and should not be written as function ofother velocities. This is why it will remain explicitly in the constraint equations.The fourth form of the constraint equations involves a dependent joint velocity,which can be eliminated as before, resulting in:

q3 = CTq3

(3T V2 Ω2 + 3T V

4 E4q4) (5.126)

C3(3T V

2 Ω2 + 3T V4 E4q4) = 0 (5.127)

with

CTq3

= −(ET3

3T F4 C4

4T V3 E3)

−1ET3

3T F4 C4

4T V3

C3 = I + E3CTq3

C3 = 3T F4 C4

4T V3 C3

The next form of the constraint equations introduces another independent velocity:

C2(2T V

1 Ω1 + E2q2 + 2T V4 E4q4) = 0 (5.128)

with

C2 = 2T F3 C3

3T V2 (5.129)

Finally, equations (5.128) can be further developed into the last form

C1(1T V

2 E2q2 + 1T V4 E4q4) = 0 (5.130)

with

CTq1

= −(ET1

1T F2 C2

2T V1 E1)

−1ET1

1T F2 C2

2T V1

C1 = I + E1CTq1

C1 = 1T F2 C2

2T V1 C1

In summary, the reduction of the dependent spatial velocities yields:

Ω3 = C33T V

2 Ω2 + E3CTq3

3T V4 E4q4 (5.131)


Ω1 = E1CTq1

(1T V2 E2q2 + 1T V

4 E4q4) (5.132)

These velocities have roughly the same expression as in the previous example,except for the additional terms comprising the (outboard) independent joint velo-cities. The expressions for the constraint matrices are also very similar and canstill be calculated during the first backward recursion step.

CANONICAL MOMENTA

Using the definition of the canonical momenta for independent body 4, one obtains:

p4 =

4∑

i=1

∂ΩTi

∂q4Pi

= ET4

4T F1 Cq1

ET1 P1 + ET

44T F

1 Cq1ET

11T F

2 P2

+ ET4

4T F3 Cq3

ET3 P3 + ET

44T F

1 Cq1ET

11T F

3 CT3 P3

+ ET4 P4 + ET

44T F

3 Cq3ET

33T F

4 P4 + ET4

4T F1 Cq1

ET1

1T F3 CT

33T F

4 P4

= ET4 (P4 + 4T F

1 Cq1ET

1 P∗

1 + 4T F3 Cq3

ET3 P∗

3) (5.133)

= ET4 P∗

c

4 (5.134)

with

Cq1= Cq1

(5.135)

Cq3= Cq3

+ 3T F1 Cq1

ET1

1T F3 Cq3

(5.136)

A general proof of these expressions is given in appendix D.7. Similarly, the canon-ical momenta associated with body 2 are

p2 = ET2 P∗

c

2 (5.137)

wherein

P∗c

2 = P∗

2 + 2T F1 Cq1

ET1 P∗

1 + 2T F3 Cq3

ET3 P∗

3 (5.138)

The constrained articulated momentum vectors are now dependent on inboard andoutboard bodies, and can thus not be calculated in one sweep. On the other hand,one can observe that the projections of the dependent articulated momenta are

74 CHAPTER 5

used. The quantities ET1 P∗

1 and ET3 P∗

3 can be interpreted as dependent canonicalmomenta pd

1 and pd3. The constrained momentum vectors have the same recursive

relationship as in the previous example:

P∗c

K = PK + KT FK+1P

∗c

K+1 (5.139)

but now with

P∗c

4 = P4 + 4T F1 Cq1

ET1 P∗

1 + 4T F3 Cq3

ET3 P∗

3 (5.140)

as boundary condition.


The principle of virtual power can be used as previously:

4∑

i=1

ΩvTi (Pi + Ωi × Pi − Ti) = qvT

2 A2 + qvT

4 A4 = 0 (5.141)

The coefficient A4 is

A4 =

4∑

i=1

∂ΩvTi

∂qv4

(Pi + Ωi × Pi − Ti) (5.142)

= ET4 (P∗

c

4 + Ω4 × P∗c

4 − T∗c

4 ) = 0 (5.143)

with

T∗c

4 = T4 + 4T F1 Cq1

ET1 T∗

1 + 4T F3 Cq3

ET3 T∗

3

+ 4T F1 [

˙Cq1

ET1 + (Ω1 × I)Cq1

ET1 − Cq1

ET1 (Ω1 × I)]P∗

1

+ 4T F3 [

˙Cq3

ET3 + (Ω3 × I)Cq3

ET3 − Cq3

ET3 (Ω3 × I)]P∗

3 (5.144)

Projection on the joint subspace results in

p4 = −ET4 (Ω4 × P∗

c

4 − T∗c

4 ) (5.145)

The equations of motion associated to independent body 2 are easily found as

p2 = −ET2 (Ω2 × P∗

c

2 − T∗c

2 ) (5.146)


JOINT VELOCITIES

Finding the joint velocities becomes more complex when arbitrary independent co-ordinates are chosen. The velocities can be expressed as functions of the knownindependent canonical momenta and the unknown dependent canonical momenta.These dependent canonical momenta are propagated backwards and forwards,much like the Lagrange multipliers of method 5.2. When the dependent canon-ical momenta return to the leaf body, they can be determined using the constraintequations. Thereafter, the joint velocities can be calculated explicitly. The proce-dure starts with independent leaf body 4:

P∗c

4 = P4 + 4T F1 Cq1

ET1 P∗

1 + 4T F3 Cq3

ET3 P∗

3

= M4(4T V

3 Ω3 + E4q4) + 4T F1 Cq1

pd1 + 4T F

3 Cq3pd

3 (5.147)

⇒ q4 = M−1j4

(p4 − ET4 M4

4T V3 Ω3 − ET

44T F

1 Cq1pd

1 − ET4

4T F3 Cq3

pd3) (5.148)

Substituting (5.148) in (5.147) gives

P∗c

4 = M′

44T V

3 Ω3 + D′

4 + Dp′

14 pd

1 + Dp′

34 pd

3 (5.149)

Dp′

i

4 = (I − M4E4M−1j4

ET4 )4T F

i Cqi(i = 1, 3) (5.150)

Observe the similarity with remainder term Dσ′

(5.20) obtained in the augmentedLagrangian approach. The reduced mass matrix M

′

and the remainder momentumvector D

′

have the same definition as for open-loop systems. The constrainedmomentum vector of body 3 can be developed as

P∗c

3 = P3 + 3T F4 P∗

c

4

= M∗

3Ω3 + D3 + Dp1

3 pd1 + D

p3

3 pd3 (5.151)

Dpi

3 = 3T F4 D

p′

i

4 (i = 1, 3) (5.152)

Take a closer look at the projection of the constrained momentum vector on thejoint subspace:

ET3 P∗

c

3 = ET3 (P∗

3 + 3T F1 Cq1

pd1 + Cq3

pd3)

= pd3 + ET

33T F

1 Cq1pd

1 + ET3 Cq3

pd3 (5.153)

76 CHAPTER 5

Hence, it can be expressed in terms of the dependent canonical momenta solely.Combination of (5.151) and (5.153) leads to

q3 = M−1∗j3

(pd3 + ET

33T F

1 Cq1pd

1 + ET3 Cq3

pd3

− ET3 D3 − ET

3 Dp1

3 pd1 − ET

3 Dp3

3 pd3 − ET

3 M∗

33T V

2 Ω2) (5.154)

and

P∗c

3 = M′

33T V

2 Ω2 + D′

3 + Dp′

13 pd

1 + Dp′

33 pd

3 (5.155)

with

Dp′

33 = (I − M∗

3E3M∗−1j3

ET3 )Dp3

3 + M∗

3E3M∗−1j3

(I + ET3 Cq3

) (5.156)

Dp′

13 = (I − M∗

3E3M∗−1j3

ET3 )Dp1

3 + M∗

3E3M∗−1j3

3T F1 ET

1 Cq1(5.157)

D′

3 = (I − M∗

3E3M∗−1j3

ET3 )D3 (5.158)

The contribution of the dependent canonical momenta pd3 is not taken into D3, as

in open-loop systems. The expression of the latter is thus slightly different. Thenext body 2 was chosen to be an independent one. The recursion process is carriedon:

P∗c

2 = P2 + 2T F3 P∗

c

3 = M∗

2Ω2 + D2 + Dp1

2 pd1 + D

p3

2 pd3 (5.159)

= M′

22T V

1 Ω1 + D′

1 + Dp′

11 pd

1 + Dp′

31 pd

3 (5.160)

And finally:

P∗c

1 = P1 + 1T F2 P∗

c

2 = M∗

1Ω1 + D1 + Dp1

1 pd1 + D

p3

1 pd3 (5.161)

Just as for the other dependent body 3, one can calculate the projection of theconstrained momentum vector on the joint subspace to obtain

ET1 P∗

c

1 = pd1 + ET

1 Cq1pd

1 + ET1

1T F3 Cq3

pd3 (5.162)

The joint velocity of the first body can now be written as


q1 = M−1∗j1

(pd1 + ET

1 Cq1pd

1 + ET1

1T F3 Cq3

pd3

− ET1 D1 − ET

1 Dp1

1 pd1 − ET

1 Dp3

1 pd3) (5.163)

The next part of the procedure is to propagate the velocities from the base tothe tip. At that point, the constraint equations can be used to solve for thedependent canonical momenta. With knowledge of these momenta, all quantitiescan be calculated explicitly in a last sweep. So, the joint velocity of body 1 can bewritten formally as

q1 = q′

1 + γp1

1 pd1 + γ

p3

1 pd3 (5.164)

The matrices γpi

1 are coefficients of pdi . q

′

1 is the contribution from the independentbodies. The spatial velocity becomes

Ω1 = E1q1 = Ω′

1 + Γp1

1 pd1 + Γ

p3

1 pd3 (5.165)

The same applies for all other spatial velocities, in particular for body 4:

Ω4 = 4T V3 Ω3 + E4q4 = Ω

′

4 + Γp1

4 pd1 + Γ

p3

4 pd3 (5.166)

Substitution in the second form of the constraint equations yields

C55T V

4 (Γp1

4 pd1 + Γ

p3

4 pd3) = −C5

5T V4 Ω

′

4 (5.167)

This can be solved for the dependent canonical momenta:

(pd

1

pd3

)= −

[(Γ

pT1

4

ΓpT3

4

)4T F

5 C55T V

4

(Γ

p1

4 Γp3

4

)]−1(

ΓpT1

4

ΓpT3

4

)C5

5T V4 Ω

′

4 (5.168)

The inverted matrix has dimension 2× 2. Once the dependent canonical momentaare known, the joint velocities can be calculated explicitly.

CONCLUSION

For an arbitrary partition in dependent and independent coordinates, a little moreeffort is required to obtain the equations of motion. Unknown dependent canonical

78 CHAPTER 5

momenta are introduced in the equations and need to be carried along duringthe recursion process. As soon as all the dependent bodies have been processedhowever, the direction of the recursion can be reversed to determine the value ofthe dependent canonical momenta. After that, the system can be treated as anopen-loop system.

5.3.3 GENERAL APPROACH

The general approach is explained for chains with one single outer loop and nobranching, but with an arbitrary number N of bodies, interconnected by any kindof joint and with an arbitrary (valid) choice of dependent coordinates. Figure 5.2is an example of such a system. As the loop closure can be done by means of anytype of joint, the number of dependent bodies can range from 1 to 5.

CANONICAL MOMENTA

The canonical momenta conjugated to an independent body K are given by

pK = ETKP∗

c

K (5.169)

with the constrained articulated momentum vector

P∗c

K = P∗

K +∑

i∈Dep(K)

KT Fi Cqi

ETi P∗

i (5.170)

= PK +∑

i∈K+1

KT Fi P∗

c

i (5.171)

and the additional recursive relationships

CK = KT FK+1CK+1

K+1T VK CK (5.172)

CK = I ∀K ∈ Ind (5.173)

CK = I + EKCTqK

∀K ∈ Dep (5.174)

CTqK

= −(ETK

KT FK+1CK+1

K+1T VK EK)−1ET

KKT F

K+1CK+1K+1T V

K (5.175)

CTqd

= CTqd

(I +∑

i∈dAsc(d)

dT Vi EiC

Tqi

iT Vd ) (5.176)

Dep is the set of all dependent bodies. dAsc(d) stands for the set of all dependentascendants of body d. Equation (5.176) describes a recursive procedure betweenthe dependent bodies only. A proof is provided in appendices D.5 to D.7.



The equations of motion are obtained using the principle of virtual power. Thisresults in

pK = −ETK(ΩK × P∗

c

K ) + ETKT∗

c

K + ETKP∗

c

K (5.177)

with

T∗c

K = TK + KT FK+1T

∗c

K+1 (5.178)

The constrained force vector of the leaf body N is

T∗c

N = TN +∑

i∈Dep

[NT F

i CqiET

i T∗

i

+ NT Fi [

˙Cqi

ETi + (Ωi × I)Cqi

ETi − Cqi

ETi (Ωi × I)]P∗

i

](5.179)

A proof is obtained by combining the results of appendix D.7 and appendix D.4.3.

JOINT VELOCITIES

For the sake of clarity, the method to find the joint velocities is described withoutwriting all different coefficients explicitly. These coefficients are rather easily ob-tained by following the procedure of the two previous examples. The constrainedmomentum vector of body K can be expressed as

P∗c

K = M∗

KΩK + DK +∑

i∈Dep

Dpi

Kpdi (5.180)

from which the expression for the joint velocity can be extracted:

qK = M−1∗jK

[p

(d)K + ET

K

∑

i∈Dep

(KT Fi Cqi

pdi − D

pi

Kpdi )

− ETKDK − ET

KM∗

KKT V

K−1ΩK−1

](5.181)

The first dependent body outboard of the base body is called D1. During the firstbackward recursion, the articulated mass matrix M∗, the momentum remainder

80 CHAPTER 5

vector D and the coefficients Dpi are calculated, till body D1 is reached. Fromthat moment on, the backward recursion step is put on hold to solve the dependentcanonical momenta pd

i . The joint velocity of dependent body D1 can be writtenas:

qD1= q

′

D1+∑

i∈Dep

γpi

D1pd

i + γΩD1

ΩD1−1 (5.182)

This expression is dependent on the unknown spatial velocity ΩD1−1. γΩ is itsassociated coefficient. The joint and spatial velocities can be propagated in aforward recursion from D1 to the leaf body N :

qK = q′

K +∑

i∈Dep

γpi

Kpdi + γΩ

KΩD1−1 (5.183)

ΩK = Ω′

K +∑

i

Γpi

Kpdi + ΓΩ

KΩD1−1 (5.184)

ΩN eventually takes following form:

ΩN = Ω′

N +∑

i

Γpi

Npdi + ΓΩ

NΩD1−1 (5.185)

Finally, the constraint equations can be used to determine the canonical dependentmomenta:

pd = −(ΓpT

NNT F

N+1CN+1N+1T V

N Γp)−1ΓpT

N CN+1N+1T V

N (Ω′

N + ΓΩNΩD1−1) (5.186)

with

pd =

pdD1

pdD2

...pd

Dm

Γp =

(Γ

pD1

N . . . ΓpD2

N ΓpDm

N

)(5.187)

Di represents a dependent body, m is the number of dependent bodies. Once thedependent canonical momenta are known, the backward recursion can be restartedas if an open-loop system were considered.


SUMMARY

The algorithm comprises three main recursion steps, and three small additionalones to take care of the constraints. In a preliminary recursion phase, the con-straint matrices Cd, Cqd

and Cqdare calculated. The two first during a backward

process, the latter during a subsequent forward recursion. Then, the first mainbackward recursive step starts. The articulated inertia properties and the coeffi-cients D

pd

i are determined. Once all dependent bodies have been processed, the

recursion is halted. The vectors q′

i and Ω′

i and the coefficients γpd

i , Γpd

i , γΩi and ΓΩ

i

are determined. When the constraints are reached, the dependent canonical mo-menta pd

i are solved using the constraint equations. Then, the first main backwardrecursion is continued. Subsequently, the forward and second backward recursionsare performed as for open-loop systems.

CONCLUSION

It is possible to obtain the equations of motion recursively and with O(n)-efficiencyfor an arbitrary chain with one outer loop. The constraints are dealt with duringthe first part of the algorithm in a series of recursive phases over the dependentbodies. Less effort is needed if the dependent bodies are closer to the constraintjoint.

Branched structures with multiple constraint joints can also be treated with theproposed algorithm. When the dependent bodies associated to the different con-straint joints are decoupled, i.e. when the constraint joint do not share a dependentbody as ascendant, the constraint joints can be treated individually. If the depen-dent bodies are coupled, as the example in fig. 5.4 shows (dependent bodies areblack), the constraints need to be solved together. Obviously, a selection of decou-pled dependent bodies is preferable.

5.4 CONCLUSIONS

In this chapter, the recursive Hamiltonian algorithm has been extended to con-strained multibody systems. Two different methods were proposed. The firstone, based on the use of Lagrange multipliers, introduces the augmented mo-mentum vector P+, which embeds the Lagrange multipliers. The method hasO(n + nm + m2 + m3)-efficiency, m being the number of constraints, and is suitedfor lightly constrained systems. The second method is based on a coordinate reduc-tion, leading to a minimal formulation and introduces the constrained articulatedmomentum vector P∗

c

. It allows to treat each constraint separately and achieveO(n)-efficiency. A minimal formulation requires a partition in dependent and in-dependent coordinates, a bad choice can lead to singular behavior. The proposedalgorithm can formulate the equations of motion for any valid partition of coordi-

82 CHAPTER 5

Figure 5.4: Selection of coupled dependent bodies

nates and can thus avoid this singular behavior.

Chapter 6

IMPACTS, CONTACTS AND

TIME-VARYING TOPOLOGIES

“First, get the facts, then you can distort them at your leisure.”

-Mark Twain

Few simulations of realistic mechanical systems can be performed without bodiescolliding into each other or with the environment. Collision detection and impactresolution are therefore unavoidable tools for any good simulator. It may alsobe necessary to deal with contacts. A contact is mathematically modelled as aunilateral constraint. Making a contact is equivalent to changing the topologyof the multibody system, i.e. adding a (virtual) joint to the model. Similarly, atopology change can occur when a joint or link breaks, or when a robot arm graspsan articulated tool. In this chapter, it will be shown how the proposed Hamiltonianformalism deals with impacts and topology changes in a recursive way.

6.1 COLLISION DETECTION

Before the impact and contact equations are used, the simulator needs to knowwhen objects collide into each other. This can be a computationally intensiveprocess. There are three main problems in collision detection[Coutinho, 2001,Jimenez et al., 2001].

First, a scene may comprise many objects. To check for a collision, each pair ofobjects must be tested for intersections. If n objects are present, this theoreticallymeans n(n − 1)/2 checks are needed. This quadratical relationship is prohibitivefor large systems. A partitioning of the space can help overcome this problem. Ifthe space is split into cells, the checks need only to be performed on pairs of objectsthat are in the same cell (sometimes adjacent cells as well, when the objects are onthe border).

83

84 CHAPTER 6

A second problem is to localize the exact collision time. If no collision occurs,the state of the system is advanced in time by means of numerical integration. Ifa collision is detected, the objects are likely to have interpenetrated and the timeneeds to be rewound. Finding the correct collision time is an iterative process thatcan also be computationally intensive. The process can be improved by using goodprediction methods.

A third and last main problem is to detect collision between two complex ob-jects. The surface of an object is usually approximated by a collection of polygons.To check for intersections of two objects, all polygons of one object need to becompared with all polygons of the other object, which again can be very computa-tionally intensive. Hierarchical methods are popular to solve that problem. Eachobject gets several hierarchical levels of representation [James & Pai, 2004]. Thefirst level could be a bounding sphere containing the object1, the second level couldbe a refinement comprising several smaller spheres (see fig. 6.1). Each of thesespheres could further be subdivided into more even smaller spheres in the thirdlevel and so on. The deeper the level, the more detailed the representation of theobject. The collision detection process starts with the first level representation. Ifno intersection is detected, the real objects are certainly not intersecting either.The opposite is not true: if there is an intersection, there is no guarantee that theobjects intersect as well. In that case, the detection method needs to revert to ahigher representation level up till the primitive polygons.

(a) Level 1. (b) Level 2. (c) Level 3.

Figure 6.1: Different hierarchical levels of representation.

1The collision detection of two spheres is very easy. Consider two spheres A and B, withradii RA and RB and centers described by (xA, yA, zA) and (xB , yB , zB). The two spheres arecolliding if the distance between the two centers is smaller than the sum of the radii:

(xA − xB)2 + (yA − yB)2 + (zA − zB)2 < (RA + RB)2 ⇒ collision (6.1)

Hence, only one inequality needs to be tested. In the case of bounding boxes instead of boundingspheres, 15 inequalities need to be checked [Coutinho, 2001].

IMPACTS, CONTACTS AND TIME-VARYING TOPOLOGIES 85

6.2 IMPACT THEORY

An impact occurs when the distance between two bodies becomes zero. At thatmoment, the bodies deform: there is a compression phase followed by an expansionphase. If the normal contact force passes through zero, the impact is done. Someassumptions are made in classical impact theory: the impact has a very shortduration and can be treated as instantaneous, the pose and shape of the multibodysystem remains unchanged during impact, non-impulsive forces remain constant.Additionally, wave effects are not taken into account.

(a) Just before impact (b) Just after impact

Figure 6.2: Impact of two bodies A and B

A popular approach to the simulation of multibody systems subjected to impactsis the piecewise analysis: the system is simulated till a collision is detected, thenumerical integration is stopped, the velocities after impact are calculated by meansof a momentum balance and the numerical integration is restarted using the newstate coordinates. The momentum balance is obtained by taking the integral ofthe equations of motion over the duration of the impact. This momentum balanceis however dependent on the unknown impulses transferred through the contactpoint. Hence, additional conditions are needed to find the velocities at the endof the impact, these are called the impact laws. The two most popular ones areNewton’s and Poisson’s impact laws. Newton’s law is usually used for frictionlessimpacts and establishes a relationship between the relative velocities of the contactpoints projected on the normal of the contact surfaces after and before impact:

e = −nT (v(+)

cB− v(+)

cA)

nT (v(−)cB − v

(−)cA )

(6.2)

86 CHAPTER 6

n is the normal on the contact surfaces in the contact point c. vcAand vcB

standfor the linear velocity of the contact point of bodies A and B. The upper indices (−)

and (+) denote the moments just before and just after impact. e is called the impactfactor or coefficient of restitution. In the case e = 1, the impact is fully elastic, ife = 0, it is fully inelastic. The coefficient of restitution is an experimental value. Itis dependent on the material of the objects, but also on their geometry, the contactpoint and pose, the impact velocity (certainly if the speed is near the velocity ofsound in the given material) and so on. Newton’s impact law is certainly applicablefor ball-like shapes and moderate impact velocities, but one should be careful inmore complex situations [Stoianovici & Hurmuzlu, 1996]. Poisson’s law is based onimpulses and connects the normal impulses during the expansion and the compres-sion phases. A detailed survey on Poisson’s law and impact and contact theory ingeneral can be found in [Pfeiffer & Glocker, 1996, Wittenburg, 1977]. An impactcan also be considered as a continuous process, the contact forces between twoobjects can be described using Hertzian equations [Lankarani & Nikravesh, 1994].In the following section, a single frictionless impact on a multibody system will betreated using Newton’s impact law. The purpose is to show that the algorithm forthe forward dynamics does only need minor modifications to resolve impacts. Inother words, the recursive functions used to calculate the joint velocities q can alsobe used to calculate the change in coordinates ∆q = q(+) −q(−) during the impact.The sign ∆ is used to denote the change of the quantity during impact.

6.2.1 IMPACT EQUATIONS AND RECURSIVE ALGO-RITHM

The method will be demonstrated for a closed planar chain with N links intercon-nected by means of pin-joints (see fig. 5.2). One single frictionless impact occursbetween the environment and body K. The contact point is cK and the environ-ment is assumed to be fixed (zero velocity). The first step to obtain the impactequations is to construct the momentum balance. The Hamiltonian equations willbe used in the augmented form (5.15). For body K, where the impact occurs, thisgives

p+K = ET

K(T∗

K − ΩK × P+K) + ET

KKT F

cKTπ

cK(6.3)

Note the difference between the indices + and (+). The first one is used to representan augmented quantity, the second to denote the time just after impact. Tπ

cKis

a spatial force vector containing the impulsive forces written with respect to thecontact point cK . Remember that the impact is frictionless, this means that theimpulsive force is perpendicular on the contact surface. The impact happens in avery short time interval [t(−), t(+)]. Taking the integral of the equations of motion(6.3) over that time interval gives:


t(+)∫

t(−)

p+Kdt = ∆p+

K =

t(+)∫

t(−)

[ETK(T∗

K − ΩK × P+K) + ET

KKT F

cKTπ

cK]dt

= ETK

KT FcK

t(+)∫

t(−)

TπcK

dt = ETK

KT FcK

EπcK

π (6.4)

The integral of the non-impulsive forces vanishes, so only one contribution remains2.KT F

cKcan be taken outside the integral, because the configuration of the system

is assumed not to change during impact. In the last step, the impact matrixEπ

cKhas been introduced. It represents the subspace of the impulsive forces and

moments, which is orthogonal to the contact surface. π denotes the (normal)impulse. Equations (6.4) can also be written as:

∆p+K = ΨT

Kπ (6.6)

with

ΨK = EπT

cK

cKT VK EK (6.7)

For the descendants of body K, one gets:

∆p+i = 0 = ΨT

i π ∀i ∈ Des(K) (6.8)

Ψi = 0 (6.9)

For the ancestors, the momentum balance becomes:

∆p+i = ΨT

i π ∀i ∈ Anc(K) (6.10)

Ψi = EπT

cK

cKT Vi Ei (6.11)

2It may not be obvious that the term ETK(ΩK × P

+K

) is non-impulsive, because the aug-mented momentum vector is dependent on the Lagrange multipliers σ. Consider the Hamiltonianequations in the global formulation (2.22). Developing the integral of the last part yields

t(+)Zt(−)

ΦT

qσ dt = [ΦT

qσ]t

+

t(−) −t(+)Z

t(−)

ΦTq

σ dt = ΦTq

∆σ − ΦTq

∆σ = 0 (6.5)

An integration by parts has been performed. Clearly, the reaction impulses do not contribute tothe momentum balance during impact [Lankarani & Nikravesh, 1988].

88 CHAPTER 6

The vectors Ψi are basically a means of transforming the Cartesian impact sub-space into the joint impact subspace. Ψ = (Ψ1...ΨN ) is called the compositenormal vector [Lankarani & Nikravesh, 1988]. Let’s take a look at the algorithmbased on the augmented Lagrangian (section 5.2). The articulated mass matricesM∗

i are only dependent on the joint coordinates and do not change during impact.The remainder momentum vectors D were defined according to:

Di = iT Fi+1D

′

i+1 (6.12)

D′

i = M∗

i EiM∗−1

ji(p+

i − ETi Di) + Di (6.13)

These are linear combinations of the canonical momenta. Hence, it is possible towrite

∆Di = Dπi π (6.14)

Dπi = iT F

i+1Dπ′

i+1 (6.15)

D′πi = M∗

i EiM∗−1

ji(ΨT

i − ETi Dπ

i ) + Dπi (6.16)

by using ∆p+i = ΨT

i π. The expression for the change in joint and spatial velocitiescan now be found using equations (5.38)-(5.43):

∆qi = γπi π + γσ∆σ (6.17)

∆Ωi = Γπi π + Γσ

i ∆σ (6.18)

The unconstrained velocities are linearly dependent on the impulse π and arewritten as qu = γππ. γπ, γσ, Γπ and Γσ are just coefficients.

γπi = M∗

−1

ji(ΨT

i − ETi Dπ

i − ETi M∗

iiT V

i−1Γπi−1) (6.19)

Γπi = iT V

i−1Γπi−1 + Eiγ

πi (6.20)

There are two sets of unknowns: π and ∆σ. These can be found by combinationof Newton’s impact law and the constraint equations. The latter (5.51) can bewritten in the following form:

ETcN+1

N+1T VN Γσ

N∆σ + ETcN+1

N+1T VN Γπ

Nπ = 0 (6.21)


To find a suitable expression of Newton’s impact law, write the normal velocity ofthe contact point as

nT vcK= EπT

cKΩcK

= EπT

cK

cKT VK ΩK

(= Ψq

)(6.22)

Newton’s impact law can then be transformed into:

EπT

cK

cKT VK ∆ΩK = −(1 + e)EπT

cK

cKT VK Ω

(−)

K (6.23)

After substitution of (6.18), one finally obtains

EπT

cK

cKT VK (Γπ

Kπ + Γσ

K∆σ) = −(1 + e)EπT

cK

cKT VK Ω

(−)

K (6.24)

The Lagrange multipliers and the impulse can be found by combining (6.21) and(6.24). Hence, it is possible to handle impacts in a recursive way using the Hamil-tonian formalism. Moreover, the recursive impact equations are almost identicalto the recursive equations of motion. Less effort is required though, because theconfiguration of the system remains unchanged. The articulated mass matrices forexample do not need to be recalculated. Take a look at [Pereira & Nikravesh, 1996]for impacts involving friction.

6.3 CONTACTS

Contact theory is a research topic on its own [Pfeiffer & Glocker, 1996]. In a simpleapproach, a single frictionless contact can be modelled as an additional constraint.That constraint being unilateral, the reaction forces and moments must be trackedto check whether the contact is broken or not. In practice, one has to deal with fric-tion (Coulomb’s model) and multiple contacts, which complicate the problem sub-stantially. Contacts are typically formulated as a so-called linear-complementarityproblem (LCP), which is essentially a set of linear equations with additional in-equality conditions [Lotstedt, 1982, Baraff, 1994, Anitescu & Potra, 1997].

6.4 TIME-VARYING TOPOLOGY

The topology of a multibody system is the set of relationships between the com-ponents of the system. In other words, it represents the amount, the place and thekind of connections between the different bodies of the system. Some simulations ofmultibody systems can require dynamically changing topologies, which means thestructure of the system changes during the simulation process. This can happen

90 CHAPTER 6

when joints or links are breaking (fig. 6.3) or in systems where certain degrees offreedom are (artificially) blocked or unlocked. Think, for example, of a robot grasp-ing or releasing an articulated tool/object or of a cam and follower system. Theapparition or disappearance of contacts can also be viewed as topology changes.

(a) Before joint failure. (b) After joint failure.

Figure 6.3: Breaking of a joint.

If the topology of a multibody system changes during the simulation, the gover-ning equations of motion change as well. If the number of allowed topologies islimited, it is an option to derive one set of equations of motion for each possiblestructure. Symbolical methods [Samin & Fisette, 2003] can be used to optimizethese equations. If topology-triggering events occur during the simulation, one setof equations is replaced by the other. When the considered multibody system hasa large amount of degrees of freedom and the possible topologies are not restricted,deriving all respective sets of equations explicitly becomes impractical. To avoidthis, one needs a method where the calculations are not entirely altered when atopology change occurs. A descriptor form can be used: each body is describedas an unconstrained body on which additional constraints are imposed. Changingthe topology has repercussions on the added constraints and the derived Jacobian,but the major part of the equations stays unchanged [Baraff, 1996]. The symboli-cal approaches tend to loose their efficiency when topology changes are permitted.The optimization capabilities are decreased because the joint information cannotbe used.

Recursive (topological) algorithms on the other hand combine the O(n) efficiencywith an easy treatment of the topology changes. Each element of a multibodysystem is treated separately, so changing the topology has only direct repercussionson the calculations involved with the body where the change is located. As aresult, only a small part of the equations needs to be changed. Take a look atalgorithm 1 on page 48. The same kind of functions are called recursively for eachbody. Independently of the kind of joint, one has to compute the articulated mass


matrix M∗ and the remainder momentum vector D, the accumulated force vectorT, the articulated momentum vector P∗ and the spatial velocity Ω. These arethe quantities that are transmitted from one body to the other in a backward orforward way. This transmission is completely independent of the kind of joint thatconnects both bodies. The expressions of other quantities such as the reduced massmatrix M

′

, D′

and the joint velocity q are dependent on the kind of joint, but theyare not transmitted to the adjacent bodies. Instead, they are used as intermediatevariables. Only one implementation is needed for the joint-independent functions.The joint-dependent functions on the other hand, need one implementation foreach kind of joint. Once all this functions are implemented, the simulation processbecomes very easy. At each time step, the equations of motion are established.During the backward and forward recursion steps, the simulator runs through thelinks and evaluates a series of functions (see functions 2,3 and 4 on page 48). Ifthe function is joint-dependent, the simulator checks the kind of joint associatedto the link and makes a call to the respective implementation of that function. Ifthe type of a joint is changed during the simulation, all the simulator has to do ismake a call to a different implementation of the joint-dependent functions.

There is one important detail: even if the velocities are continuous, the canonicalmomenta may display a discontinuity. Take the example of a breaking joint: apart of the mass suddenly disappears resulting in a discontinuity in the articulatedmass matrix. As a consequence, the articulated momentum vector and the canoni-cal momenta display that same behavior. Moreover, coordinates can vanish or newcoordinates can be introduced. Hence, the numerical integration of a multibodysystem must be halted when a topology change occurs. Then the new canonical mo-menta are extracted from the joint velocities. After that, the numerical integrationis restarted with the new starting values.

6.5 CONCLUSIONS

In this chapter, it has been explained how impacts and topology changes can behandled with the proposed Hamiltonian algorithm. Impacts can be treated using amethod very similar to the forward dynamics formulation. Moreover, the recursivestructure of the algorithm allows to cope with changing topologies in a very efficientway. This is demonstrated by means of a numerical example in the next chapter.

92 CHAPTER 6

Chapter 7

NUMERICAL EXAMPLES

“Errors using inadequate data are much less than those using nodata at all.”

-Charles Babbage

The purpose of this chapter is manifold. First of all, it is shown that the algorithmworks and that it really has O(n)-efficiency. Different open-loop, branched andclosed-loop multibody systems are simulated to demonstrate that. In a secondstep, the numerical integration is brought into focus. The canonical momenta andacceleration based algorithms are compared to each other in order to investigatewhether the formulation method has an influence on the numerical integration.Remember that the efficiency of algorithm depends on both the formulation andthe numerical integration. It is an interesting sidenote that both formulations canbehave very differently when using an adaptive method straightforwardly. In theconsidered examples, the Hamiltonian equations performed much better. Tweakingthe tolerance levels of the error control can remove that difference though. Finally,an example of a system with changing topology is briefly discussed, handling thetopology appears to add only an insignificant amount of overhead.

7.1 HARDWARE DESCRIPTION

The speed of a simulation is dependent on the computer it is run on. The amountand kind of RAM and cache memory, the graphics acceleration card, the clock fre-quency, the type of processor, the operating system and many other factors play animportant role. The absolute simulation speeds can be used to get an impressionof the efficiency of the algorithm, but one should pay more attention to the relativeresults. All simulations are run on a 2.0GHz Pentium4 computer with 256Mb ofRAM and an ATI Mobility Radeon graphics card with 32Mb and 512Kb L2-cache.The program to test the running times of the simulations is written in C++. The

93

94 CHAPTER 7

program normally displays a 3D view of the simulated model on the screen, butthat option was turned off to avoid wasting processor time on the graphics. Theother examples focussed on comparing the numerical integration of the canonicalmomenta formulation with the acceleration based methods are written with Mat-lab. The running speeds are indeed not important for these examples, as onlythe accuracy and the efficiency of the numerical integrator are considered. Theoperating system is Windows XP Home edition.

7.2 SIMULATION SPEEDS

Figure 7.1: Chain with 100 links.

The motion during 1s of thirty-two different planar models was simulated. Eachmodel is built with one primitive link, all these links are interconnected by meansof pin-joints. A link is a rigid beam with a mass of 0.01kg and a moment of inertiaof 2.5

3 10−5kg m2 around the joint axis. The distance between two joints is 0.05m.The system is subjected to gravitational forces and a viscous friction coefficientof 0.001 N m

rad/s is associated to the pin-joints. An adaptive Runge-Kutta-Fehlberg

numerical integrator has been used with absolute and relative tolerances set to 10−6

for all state coordinates (see appendix C). There is no point in describing all 32used models, but it may be interesting to have a look at the three different types:open-loop, branched and closed-loop systems. Figure 7.1 is an open-loop systemwith 100 links. Each dash is one link. The motion is represented by superposingseveral frames on the figure. A lighter chain represents an older configuration,a darker chain represents a newer configuration. Hence, the chain swings fromright to left and back. The time between two consecutive configurations is 0.1s.

NUMERICAL EXAMPLES 95

Figure 7.2: Tree system with 150 links.

Figure 7.2 is a tree-like system with 150 links. The trunk of the system splitsin two branches at the 10th link, there is a new splitting 10 links further and alast splitting after 10 more links. This results in a total of 8 leaf bodies. Theconsecutive frames are 0.2s apart. The last type of model is depicted in figure 7.3.It represents a closed-loop system with 100 links. The loop is closed by means ofa pin-joint. One frame is given every 0.1s.

Figure 7.3: Closed loop with 100 links.

96 CHAPTER 7

0 50 100 150 200 250 300 350 4000

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5Computation time for 1 second simulation time

Number of links

Com

puta

tion

time

(s)

Open−loopTree systemClosed−loop

Figure 7.4: Simulation time

The results of the 32 simulations are given in figures 7.4 and 7.6. In the first figure,the time needed to compute one second of simulation is plotted as a function of thenumber of links. The three types of models are represented by a different symbol.For example, 0.46s was needed to compute one second of the motion for the open-loop model of figure 7.1 (100 links). This measurement can be found as a full doton the plot. Note that a tolerance of 10−6 was used, higher speeds can be achievedby decreasing it. Figure 7.5 represents the number of times that the equationsof motion were formulated (also referred to as number of function evaluations).A higher number of function evaluations means that the numerical integrator hasmore difficulty to achieve the given error tolerance. The more links are present,the more errors are propagated through the structure and the smaller the timesteps needed to compensate for these errors. This can be seen on the figure1. If thecomputation time is divided by the number of function evaluations, one obtains thetime needed to formulate the equations of motion once. This is illustrated in figure7.6. For example, the simulator needed to establish the equations of motion 5379times in order to simulate one second of motion for the open-loop system with 100links. This gives an average of 8.5 10−5s per function evaluation, which is againrepresented by a full dot on figure 7.6. The time for one function evaluation should

1The two last dots for 375 and 400 links may be explained by the fact that the number offunction evaluations is also dependent on the motion of the system. The 32 considered modelsdo obviously not follow the same trajectory.


0 50 100 150 200 250 300 350 4000

2000

4000

6000

8000

10000

12000Number of function evaluations for 1 second simulation time

Number of links

Num

ber

of fu

nctio

n ev

alua

tions


Figure 7.5: Number of function evaluations

be linearly dependent on the number of links for an O(n) algorithm. This appearsto be the case, but only till models comprising approximately 250 links. The lossof O(n) efficiency is not easy to explain, but it is probably related to the memoryof the computer2. Another conclusion that can be drawn is that the formulationtime for open-loop, branched and closed-loop systems is about the same, as onewould expect. A 3D-example was simulated for comparison. It is a chain with20 links interconnected by pin-joints. The rotation axes of two consecutive jointsare perpendicular to each other. The details of the motion are not important, butfig. 7.7 gives a good idea. The time for one function evaluation is 2.4 10−5s versus1.75 10−5s for the planar equivalent.

The total computation time of a simulation is the product of the time per functionevaluations and the number of function evaluations. This clearly indicates that theglobal efficiency is dependent on the efficiency of the formulation and the efficiencyof the solution of the equations of motion. As a consequence, the global efficiencyis not O(n), as indicated in fig.7.4).

2The computer used for the simulations has 512Kb of L2-cache and one link consumes slightlymore than 1240 bytes.

98 CHAPTER 7

0 50 100 150 200 250 300 350 4000

1

2

3

4

5

6x 10

−4 Time needed to establish the equations of motion

Number of links

time(

s) p

er fu

nctio

n ev

alua

tion


Figure 7.6: Formulation time

Figure 7.7: 3D-chain with 20 links


7.3 CANONICAL MOMENTA VERSUS ACCEL-ERATIONS

A numerical comparison between acceleration based and canonical momenta basedequations of motion is performed. The objective is to study the behavior of bothsets of equations during numerical integration. First, fixed step numerical integra-tors are used to compare the introduced errors for equal time steps. Subsequently,adaptive methods are used and the number of function calls needed to achieve agiven tolerance is recorded. The model used for the analysis is a relatively simplesystem with 5 links and two additional constraints.

7.3.1 MODEL DESCRIPTION

The constrained multibody system that will be discussed is based on a model ofa biped robot during the double support phase (fig.7.8), so both feet are fixed tothe ground during the simulation. The model is kept simple and only gravitationalforces are considered, while no friction is present and no contact detection is per-formed. The biped actually moves through the ground in the presented simulation.This does not matter, as the only purpose of this example is to compare the behav-ior of acceleration-based and canonical momenta-based equations during numericalintegration. The acceleration-based equations were obtained using the Lagrangianformalism.

Figure 7.8: Model of the biped.

100 CHAPTER 7

The system has five links and three degrees of freedom. Each link has a mass of1kg and a length of 0.25m, resulting in a moment of inertia of 0.0208kg.m2 aroundthe joint axis. The coordinates q1, q2 and q3 are considered as independent. Asingularity would occur if the links 4 and 5 would lay on the same line, but thisdoes not happen in the simulation, as it is limited to 0.85s. The initial velocitiesare all zero, while the initial coordinates are respectively q1 = 1.7,q2 = −0.7 andq3 = 0.3. The two ankles touch the ground at the same height, 0.25m apart.

7.3.2 NUMERICAL INTEGRATION WITH FIXED STEP

In a first comparison, a fixed step method is used. A Runge-Kutta-Fehlberg 4thorder method is used, as it provides an estimation of the local truncation error.The simulation is performed several times with different time steps ranging from0.0001s to 0.01s and with both a set of equations based on accelerations and aset of Hamiltonian equations. The evolutions of the coordinates, the coordinatevelocities and the canonical momenta are given in figures 7.9, 7.10 and 7.11.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9−10

−8

−6

−4

−2

0

2

4Generalized coordinates

Time (s)

Rel

ativ

e an

gle

(rad

)

q1

q2

q3

Figure 7.9: Generalized Coordinates

In table 7.1, one can see a synthesis of the results. The estimated truncation error,averaged over the total simulation time and over all generalized coordinates, is cal-culated as well as the mean end error on the coordinates (at t = 0.85s). This enderror is obtained by comparing the simulation results with a reference trajectory


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9−60

−50

−40

−30

−20

−10

0

10

20Coordinate velocities

Time (s)

Rad

.s−

1

qd1

qd2

qd3

Figure 7.10: Coordinates velocities

calculated with an adaptive Runge-Kutta-Fehlberg method. Absolute and relativetolerances of 1.0e− 18 and 2.2e− 14 (minimum allowed by Matlab’s ODE45) weretaken. Although the numbers are consistently smaller for the Hamiltonian equa-tions, the measure is to coarse and the differences are too small to draw any con-clusion. Similar results where obtained using a second order Runge-Kutta methodand the implicit trapezoidal method(ODE15S). Additionally, fig.7.12 demonstratesthe positive effect of the Hamiltonian equations on the evolution of the constraintviolation errors. The x and y values denote the position errors along the mainaxes of the inertial frame. The sudden change in direction of the curves happenduring a short violent swing of body 3 (impact-like motion at about 0.42s, seefig.7.10). As the numerical integration uses fixed steps, large errors are introducedat that moment. This means that the constraints errors are large as well. Usingthe acceleration-based equations, the large constraint errors at the accelerationlevel are integrated during the short swing period, resulting in a residual error atvelocity level when the swing phase is over. These errors at the velocity level arethen further integrated leading in an ever increasing constraint error at the posi-tion. When using the Hamiltonian equations, the large constraint errors duringthe swing phase are introduced at the velocity level. These are integrated duringa short time to give a residual error at the position level. After the swing phase,the constraint errors at the position level remain almost constant.

One can also take a look at the evolution of the total energy of the system. As

102 CHAPTER 7

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9−3.5

−3

−2.5

−2

−1.5

−1

−0.5

0Canonical momenta

Time (s)

Mom

entu

m (

kg.m

2 .s−

1 )

p1

p2

p3

Figure 7.11: Canonical momenta

Time step Average end error Mean local truncation errorAccelerations Hamilton Accelerations Hamilton

0.01s 2.8e-03 1.2e-03 1.6e-06 9.7e-070.005s 1.9e-04 5.4e-05 4.3e-08 3.9e-080.001s 4.5e-08 1.2e-08 1.4e-11 1.3e-11

0.0005s 1.1e-09 3.8e-10 4.4e-13 4.2e-130.0001s 3.8e-11 2.9e-11 1.4e-16 1.3e-16

Table 7.1: Fixed step integration with RKF45 method

no dissipative forces are present, the energy should remain constant. One can seein fig. 7.13 that the error is smaller for the Hamiltonian equations.

It is interesting to note that the local truncation error on the velocities is consis-tently larger than the error on the coordinates. For a fixed time step of 0.001s, theaveraged truncation error is 1.1e−09 for the velocities, while it is only 1.4e−11 forthe generalized coordinates. The canonical momenta have an even smaller averagetruncation error of 4.5e − 12.


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9−0.5

0

0.5

1

1.5

2

2.5

3x 10

−8 Constraint Violation Errors

Time (s)

Err

or (

m)

HamiltonXHamiltonYLagrangeXLagrangeY

Figure 7.12: Constraint violation errors for δt=0.001s

7.3.3 NUMERICAL INTEGRATION WITH ADAPTIVESTEPS

The same simulation is now performed with two adaptive methods (ODE45 andODE15S). Table 7.2 gives a list of simulations using different tolerance levels, boththe absolute and the relative tolerances are set to the same value. There is onecolumn for the average end error and two columns giving the number of functionevaluations (F.E.) needed for the simulation. The function which is referred to isthat which returns the accelerations q or the derivatives of the canonical momentaand the generalized velocities p and q). For example: the Runge-Kutta-Fehlbergmethod requires 6 F.E. for each time step3. The Hamiltonian equations clearlyneed to compute the equations of motion less often (30% to 40%).

Let’s now take a closer look at the case of 1e−06 tolerance. As told in the previoussection, the truncation errors for the velocities are much higher than the errors forthe coordinates. The numerical solver will therefore adapt its step size to the formererrors, resulting in even smaller truncation errors for the coordinates. On the otherhand, the canonical momenta do not exhibit such big errors and their errors aretreated at the same level as the coordinates. It is interesting to investigate whathappens when the tolerance on the velocities and canonical momenta is relaxed

3If the tolerance is not met, only 5 function evaluations are performed. This explains the oddnumbers in the table.

104 CHAPTER 7

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9−10

−8

−6

−4

−2

0

2

4

6

8x 10

−8 Total energy

Time (s)

Err

or (

Nm

)

HamiltonLagrange

Figure 7.13: Error on the total energy

Tolerance Method End error F.E. Lagrange F.E. Hamilton1e-04 ODE45 1e-02 313 2061e-05 ODE45 4e-04 457 3201e-06 ODE45 1e-05 661 4341e-09 ODE45 4e-09 2545 16461e-12 ODE45 2e-11 10135 65601e-13 ODE45 2e-11 16063 104001e-03 ODE15S 336 2351e-06 ODE15S 682 4601e-09 ODE15S 1542 9981e-12 ODE15S 7916 4815

Table 7.2: Adaptive step integration with two different solvers

or even removed (see table 7.3). With full error control, it is meant that all thestate coordinates are checked for errors, while only the generalized coordinates arechecked if partial control is applied. One can see that the errors on the velocitiesfully account for the extra computing cost using an adaptive method, while theoverall accuracy does not deteriorate significantly. Figures 7.14 and 7.15 emphasizethis: for each step of 0.01s, the number of iterations needed by the solver has beencalculated. The figures show the results for respectively full and partial error


Mean trunc. err. Average end err. Number of F.E.Tol=10−6 Acc. H Acc. H Acc. HFull err. control 4.2e-09 4.4e-08 1.4e-05 2.0e-05 1578 1177Partial err. control 4.7e-08 4.7e-08 4.8e-05 2.1e-05 1164 1159

Table 7.3: Adaptive step size with RKF45 method (tol.=10−6)

control. There is obviously more activity for the acceleration based method incombination with full error control.

0 10 20 30 40 50 60 70 80 900

1

2

3

4

5

6

7

8

9

10

11Number of iterations for each step

Step number (0.01s each)

Num

ber

of it

erat

ions Hamiltonian equations

Acceleration based equations

Figure 7.14: Number of iterations, full error control

This can be demonstrated in another way as well. For each step of 0.01s, thewinner is defined as the state coordinate that caused the solver to reduce the stepsize most frequently. As table 7.4 suggests, the error control is better balanced forthe Hamiltonian equations. Ignoring the velocities while performing the error con-trol is of course not recommended, but this example shows that an equal tolerancelevel for each state coordinate may not be the most judicious choice. On the otherhand, there is no evidence of a significant global accuracy between the Hamiltonianand acceleration based equations during numerical integration. In other words, theefficiency of the formulation of the equations of motion fully specifies the efficiencyof the whole simulation method. One should be careful with extrapolating thisresults to other models, more systems should be investigated to do so.

106 CHAPTER 7

0 10 20 30 40 50 60 70 80 900

1

2

3

4

5

6

7

8

9

10

11Number of iterations for each step

Step number (0.01s each)

Num

ber

of it

erat

ions

Hamiltonian equationsAcceleration based equations

Figure 7.15: Number of iterations, partial error control

Hamilton q1 q3 q3 p1 p2 p3

Number of wins 1 13 56 5 10 0

Acc. based q1 q3 q3 q1 q2 q3

Number of wins 0 0 0 10 31 44

Table 7.4: Distribution of truncation errors among the state coordinates

7.4 TOPOLOGY CHANGE

As a last numerical example, a system with varying topology is discussed. Thepurpose of this example is to give an idea of the distribution of processing timeover the different tasks of the simulation: simulation before the topology change,performing the topology change and simulation after the change. The consideredexample is a planar chain with 100 bodies interconnected by means of pin-joints.All elements are the same, they have a mass of 10 grams and an inertia of 0.83g.cm2.The distance between two consecutive joints is 5cm. A viscous friction coefficientof 0.001Ns

m is chosen for each joint and the chain moves under the influence ofgravity. A starting relative angle of 0.0005n is taken, n being the number of thejoint counting from the base body. The only motivation for this choice is to getan appealing simulation. The system is simulated during 3.0s, a fourth orderRunge-Kutta method with a fixed step size of 0.001s is taken for the numerical


integration. After 1.8s the joint of the 41st body is broken, splitting the system ina smaller chain of 40 bodies and another free falling one of 60 bodies. To performthe topology change, the simulation needs to be halted. After the change is done,the numerical integration is restarted. The accuracy of the simulation has not beeninvestigated, as this is not the focus of this example. The simulation is shown onfigure 7.16. One configuration is given for every 0.18s simulation time.

Figure 7.16: Simulation of a breaking chain with 100 links.

Table 7.5 contains the processing times for the different parts of the simulation.The processing time for 100000 function calls is also given, for better comparison.For the actual simulation parts, the function which is referred to is the functionreturning the time derivatives of the coordinates q and their conjugated momentap. This function is called 4 times for each step when using the above mentionedRunge-Kutta method. The slight processing difference for the two parts of thesimulation can be explained by the fact that less calculations have to be performedfor the unconnected body. As for the topology change, the function refers tothe calculation of the new starting values for the coordinates and the canonicalmomenta and some bookkeeping operations. It is called only once during thewhole simulation. One should keep in mind that the numbers given in the table arestrongly dependent on which computer is used. The relative computation times are

108 CHAPTER 7

Processing times Time for 100000 function callsTotal simulation 0.64s 5.40sSimulation till joint failure 0.39s 5.23sHandling topology change 0.00s 0.04s

Table 7.5: Processing times.

far more important. As one would expect, the computational load is approximatelythe same for the simulation before and after the joint failure. The topology changeitself is tackled in a negligible time lapse.

7.5 CONCLUSIONS

The algorithm has been illustrated by means of a few numerical examples. Open-loop, tree-like and closed-loop system have been considered and displayed the same0(n) behavior. The overall simulation didn’t show the same linear complexity-efficiency relationship, this was attributed to the difficulty of the numerical inte-grator to achieve a given tolerance for systems with an increasing amount of degreesof freedom. The objective of a second series of experiments was to compare the nu-merical integration of a set of Hamiltonian equations and a set of acceleration basedequations. It appeared that the overall accuracy using fixed step methods is at thesame level, which means that the Hamiltonian approach is more efficient due tothe faster formulation of the equations of motion. Moreover, the adaptive methodshad a harder time tackling the acceleration based equations: more iterations werenecessary while no significantly better accuracy was achieved. However, relaxing orremoving the absolute tolerance on the coordinate velocities brought the numberof iterations to the same level as in the case of Hamiltonian equations. One shouldthus consider using different tolerance levels for different state coordinates prior tosimulation.

Chapter 8

GENERAL CONCLUSIONS

“Philosophy of science is about as useful to scientists as ornithologyis to birds”

-Richard Feynman

The dynamic simulation of multibody systems has many applications and is gain-ing in popularity. This has triggered the development of a whole range of methodsfor formulating and solving the equations of motion. During the last decades, therecursive algorithms such as Featherstone’s articulated inertia method have beenmodified to extend their applicability to flexible bodies, parallel computing, mul-tiphysics and realtime simulations, optimization and so on. Clearly, improving theunderlying code benefits for all these applications.

This work describes a way of improving that underlying code by means of arecursive algorithm for formulating a Hamiltonian set of equations of motion. Thealgorithm has O(n)-efficiency, meaning that the number of arithmetical operationsneeded to establish the equations of motion is proportional to the number of degreesof freedom. Comparing the specific amount of required arithmetical operationswith other O(n)-methods in the literature showed that the proposed method ismore efficient. Moreover, it is a well-established fact that Hamiltonian equationsof constrained mechanisms are superior than acceleration based equations regardingthe evolution of the constraint errors during numerical integration.

The proposed algorithm is based on the introduction of the spatial momentumvector of a rigid body. This momentum vector allows to establish a link betweenthe linear and angular momenta and the canonical momenta. The Newton-Eulerequations were expressed in terms of the momentum vectors to produce a conciseand elegant 6 − dimensional notation for the equations of motion of a rigid body.

Using the momentum vectors and the philosophy of the articulated inertia method,it is possible to extend the concept of momentum vector to articulated bodies, re-sulting in the so-called articulated momentum vector. This articulated momentum

109

110 CHAPTER 8

vector can be linked to the canonical momenta of an open-loop multibody system.As a consequence, the equations of motion of an articulated body take the sameform as the equations for a single rigid body. It is shown how a recursive algorithmcan be obtained to establish the equations of motion of an open-loop system. Thealgorithm comprises three steps. First, a backward recursion is needed to calcu-late the inertia properties and the remainder momentum vector of the articulatedbodies. Then, a forward recursion is applied to propagate the velocities to the tips.In a last backward recursive process, the accumulated forces and the derivative ofthe canonical momenta are computed. Additionally, it is possible to calculate theaccelerations and the joint reaction forces and moments using two extra recursionsteps.

Most practical multibody systems contain closed-loops. It was thus a logical stepto extend the open-loop algorithm to constrained multibody systems. Two differentapproaches are proposed in the thesis. The first approach is based on the techniqueof Lagrange multipliers, the second approach is based on coordinate reductions.

In the method using Lagrange multipliers, the unknown reaction forces and mo-ments are embedded in the canonical momenta, resulting in the so-called aug-mented canonical momenta. The structure of the algorithm is not much differentfrom the algorithm for open-loop systems. The three basic recursions are stillpresent. During the first backward recursion step however, the Lagrange multipli-ers are transmitted to the base by means of the augmented canonical momenta.In the following forward recursion, the Lagrange multipliers are propagated to thetips, this time embedded in the velocities. When the tip is reached, the constraintequations can be used to solve the Lagrange multipliers. These are used to com-pute the constraint reaction forces and moments, allowing to continue with thethird recursion step and computing the accumulated spatial forces. The algorithmappeared to be a O(n + nm + m2 + m3)-method, it is therefore not suitable forheavily constrained systems.

The coordinate reduction approach leads to a minimal formulation. A recursivealgorithm is obtained by using the principle of virtual power. As the joint coordi-nates are interdependent, a partition must be made in dependent and independentcoordinates. To extract the equations of motion out of the principle of virtualpower, the virtual dependent spatial velocities need to be expressed in terms of thevirtual independent velocities. This is called a velocity reduction. This velocityreduction leads to the definition of the constrained momentum vector that allowsone to obtain the same formalism as for open-loop systems. Hence, approximatelythe same recursive procedure can be followed. During the first backward recursion,there is a need to project the constrained momentum vector on the joint subspace.While this results in the known associated canonical momenta for independentbodies, unknown dependent canonical momenta are obtained for dependent bod-ies. These unknown dependent canonical momenta are transmitted backwards tillthe last dependent body is encountered. At that moment, the recursion directionis reversed and the spatial velocities are propagated to the constraints. The spa-

GENERAL CONCLUSIONS 111

tial velocities cannot be calculated explicitly, but are functions of the dependentcanonical momenta and the spatial velocity of the independent body preceding thelast dependent body. The constraint equations can then be used to compute thedependent canonical momenta. From that point, the original backward recursioncan be continued just as for open-loop systems. The subsequent forward and back-ward recursive steps remain the same. The main advantage of this algorithm isthat multiple constraints can be dealt with independently, which results in O(n)-efficiency. A poor choice of dependent and independent coordinates can result ina singular behavior. In the case a singularity is about to occur, it is possible toswitch from one set of independent coordinates to another.

In most practical multibody systems, impacts between several bodies and/or theenvironment are likely to happen. Furthermore, a topology change can occur whena contact is made or broken, or when subsystems are artificially assembled ordisconnected. It is explained in the thesis that the proposed algorithm to obtainthe equations of motion can be easily adapted to produce the impact equations.Moreover, it is shown that topology changes can be incorporated with almost noeffort.

A chapter of numerical examples is provided to demonstrate the theoretical con-clusions. The O(n)-efficiency of the algorithm is illustrated by simulating multi-body systems with a varying number of bodies. A comparison is made betweenthe numerical integration of canonical momenta and acceleration based equationsof motion of a constrained system. This allows one to claim that, for open-loopsystems, the recursive Hamiltonian algorithm is more efficient: it has a better ef-ficiency for formulating the equations of motion and a comparable efficiency forsolving them. For the more general constrained multibody systems, the recursiveHamiltonian algorithm has a better efficiency for both the formulation and thesolution of the equations of motion.

“I don’t know what I may seem to the world, but, as to myself, Iseem to have been only like a boy playing on the sea shore, and divertingmyself in now and then finding a smoother pebble or a prettier shell thanordinary, whilst the great ocean of truth lay all undiscovered before me”

-Isaac Newton

112 APPENDIX

Appendix A

KINEMATICS

“I cannot believe that anything so ugly as multiplication of matricesis an essential part of the scheme of nature.”

-Sir Arthur Eddington

In this chapter, the mathematical background related to the kinematic descriptionof multibody systems is presented. Kinematic analysis is the study of the motionof a mechanical system, without regard to the forces and torques which cause it.It is an essential step for the dynamical analysis of a multibody system, but it isalso relevant as such for design and control purposes. Kinematic analysis is wellunderstood and the following text only introduces its basic concepts in a shapesuitable for the algorithms presented in this thesis.

A.1 POSITION

The term position is a relative concept, the position of a point is only defined withrespect to a reference point. Consider two points OO and OK (fig.A.1). Vectorr is the position vector of OK relative to OO. This representation of a positionmay be elegant, but it does not tell where the point really is. To quantify theposition, reference frames and coordinate vectors are needed. In 3-dimensionalspace, a reference frame basically consists of three non-coplanar axes (X,Y and Z)intersecting in one point. The intersection is called the origin (O) of the frame. Tosimplify calculations, the axes are chosen to be perpendicular. Three orthogonalunity vectors 1x,1y and 1z are defined along the three axes. In multibody dynamics,a distinction is made between global (or inertial) and local reference frames. Whilea global frame is fixed to the environment (it may also have a constant linearvelocity), a local frame is attached to a moving body. Vector r can be projected oneach axis of a chosen reference frame, resulting in the components of a coordinatevector :

113

114 APPENDIX A

Or =

rT 1xO

rT 1yO

rT 1zO

Kr =

rT 1xK

rT 1yK

rT 1zK

(A.1)

Of course, the coordinate vector depends on which reference frame is used. This isdenoted by the index on the upper left. r (without index) is called a physical vector,because its representation is independent of reference frames. Physical vectors area more general and abstract representation of physical quantities and they allowequations to be more elegant and concise. As soon as tangible numbers are requiredhowever, like for implementation on a computer, one needs to revert to coordinatevectors.

Figure A.1: Global and local reference frames

A.2 ORIENTATION

Like a position, an orientation is only defined relative to a reference orientation.The orientation of a rigid body can be represented by the unity vectors 1x,1y

KINEMATICS 115

and 1z along the axes of its reference frame. Although these three vectors areeach of dimension 3 (resulting in 9 components), only three coordinates are neededto specify the orientation. The vectors are indeed constrained by 3 unity and 3orthogonality equations:

1Tx 1x = 1 1T

y 1y = 1 1Tz 1z = 1 (A.2)

1Tx 1y = 0 1T

y 1z = 0 1Tz 1x = 0 (A.3)

The following subsections give an overview of different ways of representing ori-entations. The rotation matrix is an all-purpose tool, but requires 9 dependentcoordinates. Euler angles use the minimum amount of coordinates but are proneto singularities. Euler parameters finally, require 4 coordinates and are singularity-proof. Their physical interpretation is less obvious though.

A.2.1 ROTATION MATRIX

Consider two reference frames X1Y1Z1 and X2Y2Z2, sharing the same origin. Theposition vector r can be described in both frames resulting in respective coordinatevectors 1r and 2r.

r = rx21x2

+ ry21y2

+ rz21z2

(A.4)

1r =

rx1

ry1

rz1

=

rT 1x1

rT 1y1

rT 1z1

=

(1Tx2

1x1)rx2

+ (1Ty2

1x1)ry2

+ (1Tz2

1x1)rz2

(1Tx2

1y1)rx2

+ (1Ty2

1y1)ry2

+ (1Tz2

1y1)rz2

(1Tx2

1z1)rx2

+ (1Ty2

1z1)ry2

+ (1Tz2

1z1)rz2

(A.5)

=

1Tx1

1x21T

x11y2

1Tx1

1z2

1Ty1

1x21T

y11y2

1Ty1

1z2

1Tz1

1x21T

z11y2

1Tz1

1z2

rx2

ry2

rz2

(A.6)

= 1r = 1R22r (A.7)

The relation between both coordinate vectors is thus given by the rotation matrix

1R2 =

1Tx1

1x21T

x11y2

1Tx1

1z2

1Ty1

1x21T

y11y2

1Ty1

1z2

1Tz1

1x21T

z11y2

1Tz1

1z2

(A.8)

The elements of the rotation matrix are dot products of unit vectors, it is oftencalled a direction cosine matrix. The inverse transformation matrix is:

2R1 = 1RT2 (A.9)

116 APPENDIX A

A rotation matrix can be interpreted in different ways. In equation (A.7), Rrepresents a coordinate transformation. This is called a passive interpretation. Onthe other hand, the same matrix can also represent a real rotation of a vector r.From this perspective, 2r is the initial vector and 1r the rotated vector. This iscalled an active interpretation.

A.2.2 EULER ANGLES

Figure A.2: Leonhard Euler (1707-1783)

The rotation matrix is a robust tool, but it can be cumbersome to work with 9coordinates, while 3 would be sufficient. The idea behind Euler1 angles is simple:

1Leonhard Euler (fig.A.2) was born in Basel(Switzerland) on the 15th of April 1707. He re-ceived his primary mathematical education from his father. Euler entered the University of Baselaged 14 and was tutored by Johann Bernoulli who rapidly discovered his enormous potentialfor mathematics. Euler was helped by an extraordinary memory, he was able to perform men-tal calculations with numbers up to 50 decimals. Isaac Asimov once said: “The most excitingphrase to hear in science, the one that heralds new discoveries, is not ‘Eureka’ (I found it) but‘That’s funny...’”. Euler must have had many funny moments in his life. He made significantcontributions to many fields of mathematics and physics, such as geometry, calculus and numbertheory, the dynamics of ideal compressible gasses and rigid bodies. He proved for example, thatany motion of a rigid body with one fixed point can be described as a rotation about a givenaxis. (Theory of the Motions of Rigid Bodies (1765).) Euler also studied elasticity, acoustics,music and the wave theory of light. He worked on cartography, fire engines, magnetism and shipbuilding. He heavily contributed to the systematization of mathematics by introducing a greatpart of modern scientific notation like the imaginary number i =

√−1, f(x) to denote a function,

KINEMATICS 117

an arbitrary rotation is decomposed in three elementary rotations which can eachbe quantified with one angle θ. If the original orientation is represented by referenceframe 0 and the orientation after rotation by reference frame 3, one can introducetwo judiciously chosen intermediate frames 1 and 2 and write

0R3(θ1, θ2, θ3) = 0R1(θ1)1R2(θ2)

2R3(θ3) (A.10)

The rotation axes of the partial transformations need to be fixed either in the globalreference frame or in a relative reference frame. Figure A.3 illustrates an example:0R1 is a rotation about the Z0 axis, 1R2 about the X1-axis and 2R3 a rotation aboutthe new Z2-axis. The associated set of angles θ1,θ2 and θ3 are called (symmetric)Euler angles in this specific case. If the global rotation matrix described by (A.10)is worked out explicitly, one obtains:

0R3 =

0 cos θ1 cos θ3 − sin θ1 cos θ2 sin θ3 sin θ1 cos θ3 + cos θ1 cos θ2 sin θ3 sin θ2 sin θ3

− cos θ1 sin θ3 − sin θ1 cos θ2 cos θ3 − sin θ1 sin θ3 + cos θ1 cos θ2 cos θ3 sin θ2 cos θ3

sin θ1 sin θ2 − cos θ1 sin θ2 cos θ2

1AAs one can see, a problem arises when θ2 = 0. In this circumstance, the firstand third rotations occur about the same axis and the global transformation is arotation about the Z0-axis with an angle θ1 + θ3. The consequence is that there isno unique set of angles to describe that specific range of orientations. Moreover,the rotation is restricted to two dimensions, a phenomenon known as gimbal lock.This leads to singularity problems when trying to find the Euler angles or theirderivatives given the rotation matrix. Unfortunately, the singularity problem isfundamental and no combination of elementary rotations can be found that isnot prone to it. There exist 12 sets of Euler angles, 6 symmetric ones (whichmeans the first and last rotations are about the same axis) and 6 asymmetricones. Asymmetric Euler angles are also known as Tait-Bryan or Cardan angles,for example: the first rotation about Z0, the second about Y1 and the third about

e for the base of natural logarithms, π for pi, ∆y,∆2y and the summation signP

.Euler followed an impressive career. He started at the mathematical division of the Academy

of St-Petersburg in 1726 and served as a medical lieutenant in the Russian navy at the sametime. In 1730 he became a full member of the Academy as Professor of physics, he was appointedthe senior chair in mathematics in 1733. In 1741 he left for the Academy of Sciences in Berlin,that was presided by Maupertuis at the time. He served as a government advisor with regard toinsurances, state lottery, pensions and artillery. After Maupertuis’s death in 1759, Euler assumedleadership of the Academy. After some disputes with the king, he returned to St.Petersburg in1766.

His cartographic work at the geography section of the Academy of St Petersburg probablycost him his right eye in 1738 (“Now I will have less distraction”). Cataract would make himtotally blind in 1766. He could however still continue his scientific work thanks to his phenomenalmemory. As a matter of fact, half of his total output was produced during the 17 years ofblindness. Euler was certainly the most prolific mathematician of all times, he wrote no less than800 papers. He also had 13 children... Euler said his greatest mathematical discoveries wheremade holding a baby in his arms and having a swarm of children playing around him. Two of hissons, Johann Albrecht and Christof Euler assisted him in his research. St Petersburg Academywould publish Euler’s unrevealed work for 50 years after his death in 1783.

118 APPENDIX A

Figure A.3: Decomposition of an arbitrary rotation into three primitive rotationsusing Euler angles

X2. Another widely used set of angles that use fixed rotation axes are the so-calledRoll-Pitch-Yaw angles. They are mainly used for describing the attitude of ships,cars or airplanes. Finally, there also exists a set of Rodrigues-Hamilton parameters,which is closely related to the Euler parameters described in the next section. Theyare defined as q = (kx tan θ

2 ky tan θ2 kz tan θ

2 )T , k being a unit vector along therotation axis. Euler, Tait-Bryan, Roll-Pitch-Yaw angles and Rodrigues-Hamiltonparameters: all are triplets and thus prone to singularities. The problem can becircumvented by switching from one set of rotations to another set as soon as asingularity occurs (or is about to occur). This kind of artificial solution is notrecommended and bothersome to implement.

A.2.3 QUATERNIONS

Figure A.4: Brougham bridge

As mentioned before, Euler angles and other representations of rotations using 3

KINEMATICS 119

coordinates (triplets) always contain singularities. Avoiding these singularities re-quires a bunch of checks and coordinate changes, which is not desirable. To addressthis problem, one can revert to a special set of quaternions2. These quaternions ǫ

are defined as [Shoemake, 1985, Coutinho, 2001]:

ǫ0 = cosθ

2(A.12a)

ǫ1 = kx sinθ

2(A.12b)

ǫ2 = ky sinθ

2(A.12c)

ǫ3 = kz sinθ

2(A.12d)

2A point on a line can be described by a real number (∈ R). Similarly, a point on a plane canbe represented by a complex number (∈ C). Moreover, the multiplication of complex numberscan be interpreted as a rotation in a plane. The discovery of quaternions (∈ H) was initiated bythe search for hypercomplex numbers which would describe a point in 3-dimensional space. Itimplied the definition of a hypercomplex multiplication which would be equivalent to a rotation in3D-space. While rotations can easily be combined by multiplying rotation matrices, it seems (andis) impossible to multiply triplets. As a matter of fact, such hypercomplex numbers only existin 1, 2 and 4 dimensions (as Frobenius proved). This is a problem that haunted William RowanHamilton and he devoted much time to tackling the problem [Hamilton, 1850]. “Hamilton’s son:Well papa, can you multiply triplets? Father(shaking his head in despair): no, I can only addand subtract them”. Big frustration. It took Hamilton 10 years to realize that a fourth dimensionwas necessary. It was on the 16th of October 1843, a Monday for whom it may interest, whileHamilton and his wife Helen were walking along the Royal Canal in Dublin, that the idea ofquaternions were formed in Hamilton’s mind. He could not resist the urge of carving the basicformulae (A.11) on a stone of Brougham Bridge (pronounced ’broom’). A plaque memorizing thismoment can still be seen at the bridge (figure A.4).

“Here as he walked byon the 16th of October 1843Sir William Rowan Hamiltonin a flash of genius discoveredthe fundamental formula forquaternion multiplication

i2 = j2 = k2 = ijk = −1 (A.11)

& cut it on a stone of this bridge”

Unfortunately, the initial carving vanished with time... Hamilton did discover the quaternions,but he did not fully grasp its subtleties. The world would have to wait for William KingdomClifford (1845-1879) to unveil the secrets of higher dimensional number systems [Clifford, 1878].It is interesting to note that vector calculus has its origins in the theory of quaternions. Theuse of vectors is broadly spread nowadays and quaternions are looked at with some reluctance,but quaternions really existed first. Moreover, it is probably Hamilton who introduced the wordvector in its actual context. Although the discovery of quaternions is credited to Hamilton, thereis some evidence that Gauss actually came up with the idea first, his work went unnoticed though.

120 APPENDIX A

and are based on the following expression, known as Rodrigues’3 formula:

r′

= Rr = k(kT r)(1 − cos θ) + cos θ r + sin θ(k × r) (A.13)

Above expression describes the rotation of a vector r about an axis represented byunit vector k and over an angle θ. r

′

is the rotated vector. Remark that the activerepresentation is used, but the rotation matrix is the same as for a coordinatetransformation from a local to an inertial reference frame.

There exist many more definitions of quaternions, equations (A.12a) are howevermost commonly used. They are called Euler (symmetric) parameters. The 4 Eulerparameters are interdependent, they are related through the unity constraint

ǫT ǫ = ǫ20 + ǫ21 + ǫ22 + ǫ23 = 1 (A.14)

In the specialized literature, Euler parameters are usually formally written as theconcatenation of a scalar ǫ0 and a vector ǫv = (ǫ1 ǫ2 ǫ3)

T :

ǫ =

(ǫ0ǫv

)(A.15)

In terms of Euler parameters, equation (A.13) for the rotation of a unity vector 1r

becomes:

R1r = 2ǫv(ǫTv 1r) + (2ǫ20 − 1)1r + 2ǫ0(ǫv × 1r) (A.16)

which provides an expression for the rotation matrix:

R = (2ǫ20 − 1)I + 2(ǫvǫTv + ǫ0ǫv) (A.17)

ǫv is a skew-symmetric operator representing the cross product ǫv×. It can bewritten in matrix notation as well

ǫv = ǫv× =

0 −ǫ3 ǫ2ǫ3 0 −ǫ1−ǫ2 ǫ1 0

(A.18)

The explicit expression for the rotation matrix in terms of quaternions takes fol-lowing form:

R = 2

(ǫ20 + ǫ21) −12 (ǫ1ǫ2 − ǫ3ǫ0) (ǫ1ǫ3 + ǫ2ǫ0)

(ǫ1ǫ2 + ǫ3ǫ0) (ǫ20 + ǫ22) −12 (ǫ2ǫ3 − ǫ0ǫ1)

(ǫ1ǫ3 − ǫ0ǫ2) (ǫ2ǫ3 + ǫ0ǫ1) (ǫ20 + ǫ23) −12

(A.19)

3Benjamin Olinde Rodrigues (1794-1851) was poorly appreciated by his contemporaries, bothas a mathematician and for his progressive social ideas “... women will one day conquer equalitywithout any restriction”. There are many incorrect references to him in literature. Several authorsthought Olinde and Rodrigues were actually two different people...

KINEMATICS 121

As one can observe, no trigonometric functions are present in the matrix. Eulerparameters more importantly cannot lead to singularities, which makes them usefulfor implementation on a computer. The drawback is that 4 dependent coordinatesare needed. As discussed in chapter B about dynamics, this results in constrainedequations of motion, which are more difficult to solve. The problem however is notas stringent as, for example, constraints introduced by loop-closure, since Eulerparameters introduce a local constraint, which relates only these 4 parametersand no other coordinates. Let’s now take a closer look at quaternion algebra. Aquaternion can be viewed as a hypercomplex number of dimension 4. Complexnumbers are well understood, they are written as the sum of a real number a andan imaginary number bi:

c = a + bi (A.20)

with i2 = −1. Similarly, a quaternion can be written as the sum of a real part andthree imaginary parts:

ǫ = ǫ0 + iǫ1 + jǫ2 + kǫ3 (A.21)

The hypercomplex numbers i, j and k are defined by following expressions:

i ∗ i = j ∗ j = k ∗ k = i ∗ j ∗ k = −1 (A.22)

The sign ∗ is used to differentiate quaternion multiplication from vector multipli-cation. Above expression imply the following 4 additional relationships:

i ∗ j = −j ∗ i = k (A.23)

j ∗ k = −k ∗ j = i (A.24)

k ∗ i = −i ∗ k = j (A.25)

Using the partition in scalar and vectorial parts, the multiplication of two quater-nions ǫa and ǫb can be written as

ǫa ∗ ǫb =

(ǫa0

ǫb0 − ǫTav

ǫbv

ǫa0ǫbv

+ ǫb0ǫav+ ǫav

× ǫbv

)(A.26)

A 3-dimensional vector r can be represented by a so-called pure quaternion ρ,which is a quaternion having its scalar set to zero: ρ = (0 r)T . The rotation ofvector r into vector r

′

can be interpreted as following quaternion multiplication ofquaternion ρ:

122 APPENDIX A

ρ′

= ǫ ∗ ρ ∗ ǫ (A.27)

ǫ = (ǫ0 − ǫv)T is the adjoint quaternion of ǫ. Developing (A.27) indeed results

in the Rodrigues formula (A.13). Note that the combination of two rotations ǫ1

and ǫ2 is represented by the quaternion multiplication ǫ2 ∗ ǫ1, which requires lessarithmetical operations than the multiplication of two arbitrary matrices. It isconvenient to introduce the auxiliary quaternion matrix Q and its conjugate Q asfollows

Q(ǫ) =

(ǫ0 −ǫT

v

ǫv ǫ0I + ǫv

)(A.28)

Q(ǫ) =

(ǫ0 −ǫT

v

ǫv ǫ0I − ǫv

)(A.29)

Note that these matrices are orthonormal, they represent rotations in the Euclidian4D space. The multiplication now reduces to an even more concise formulation:

ǫa ∗ ǫb = Q(ǫa)ǫb (A.30)

ǫb ∗ ǫa = Q(ǫa)ǫb (A.31)

These Q and Q matrices will reappear later in the discussion of spatial velocities.For the interested, there exists a dual quaternion to describe spatial motion aswell [Stejskal & Valasek, 1996], but they will not be used in this thesis. Anotherpossible set of quaternions is the so called angle-axis set ǫ = (θ kx ky kz)

T .

A.2.4 CONCLUSION

Which coordinates to choose for expressing an orientation depends on several as-pects: computational speed, simplicity, singularities, formalism, studied model...Euler parameters (quaternions) combine a singularity-free handling of rotationsand efficient multiplications. On the other hand, more coordinates are used thanneeded. When applied in the forward dynamics problem, this means a non-minimalset of coordinates is used, resulting in a set of DAE (Differential Algebraic Equa-tions). In many applications, the joints restrict the range of possible orientations,leading to rotations that can be expressed singularity-free using Euler-type angles.Gimbal locks for example are a rare occurrence for biological joints. In formalismsusing absolute coordinates however, a full description of the orientation is alwaysneeded, and the choice of coordinates becomes more important. For mere kine-matical computations, one may use rotation matrices instead. For use in dynamicsthey become impractical, as the number of constraints tends to be prohibitive.

KINEMATICS 123

A.3 HOMOGENEOUS TRANSFORMATION

The homogeneous transformation matrix is a practical tool to describe the relativeorientation and position between two reference frames. It is in fact the aggregationof a rotation and a translation into a single 4×4 matrix. Consider a position vectorr. Its components with respect to reference frame 2 are denoted as coordinate vector2r, its components relative to frame 1 are found in 1r. The transformation fromframe 2 to frame 1 can be written as follows:

1rh =

(1r

1

)=

(1R2

2r + O1O2

1

)= 1H2

(2r

1

)(A.32)

with

1H2 =

(1R2 O1O2

0 1

)(A.33)

The 4-dimensional vector rh is called a homogeneous vector. When transformingposition vectors, the last element is a one. When transforming vectors such asforce or velocity vectors v, the translational part needs to be dropped, and thehomogeneous vector takes a zero as last element:

1vh =

(1v

0

)= 1H2

(2v

0

)=

(1R2

2v

0

)(A.34)

As one can see, position vectors transform into position vectors and free vectorsinto free vectors. The main advantage of homogeneous transformations will becomeapparent in the description of the forward kinematics (section A.5), as the combi-nation of frame transformations is equivalent to the multiplication of homogeneoustransformation matrices.

A.4 CONFIGURATION SPACE

The configuration of a rigid body can be represented by a local reference framefixed to the body. The relation between this frame and a chosen global (inertial)frame is dependent on 6 coordinates. Similarly, the configuration of a set of nrigid bodies can be described by 6n coordinates. In general multibody systemshowever, the bodies are interconnected and kinematical constraints are introducedsuch that the 6n coordinates are not all independent. Instead of dealing with6n coordinates and a set of p constraint equations, one can also use generalized(or Lagrange) coordinates which encapsulate the constraints. If p independentconstraints are present, the system has 6n − p degrees of freedom (DOF) and thesame number of generalized coordinates are minimally required to describe the

124 APPENDIX A

system’s configuration. The values of the generalized coordinates correspond to apoint in a (6n − p)-dimensional hyperspace, known as configuration space. Mostoften, a trade-off is made between the convenience of working without constraintsand the difficulty of describing certain values with a minimal amount of coordinates.Many different sets of coordinates can be chosen, they must however always beunequivocal.

A.4.1 CARTESIAN COORDINATES

Cartesian or reference point coordinates use the position of a reference point (suchas the center of mass or a point on the joint axis) and global coordinates such asEuler angles or Euler parameters to specify the orientation of the body. Thesecoordinates are said to represent a point in Cartesian space. For example, theplanar double pendulum in figure A.5(a) has two degrees of freedom. One canhowever ignore the constraints and use 3 redundant coordinates to define the poseof each body. In this case, the position of the center of mass is described by globalcoordinates (x1,2, y1,2) and the orientation is given by global angles θ1,2. The Carte-sian space is thus described by the set (x1, y1, θ1, x2, y2, θ2). Note the differencebetween configuration space and Cartesian space: due to the constraints, somepoints in Cartesian space represent an impossible configuration. The configurationspace is a subspace of the Cartesian space.

(a) Reference point coordinates (b) Joint coordinates

Figure A.5: Planar double pendulum

A.4.2 JOINT OR RELATIVE COORDINATES

Relative coordinates define the position and orientation of two adjacent bodiesby using coordinates corresponding to the degrees of freedom of the connectingjoint. They are therefore also called joint coordinates and they are said to form

KINEMATICS 125

the joint space. Joint coordinates have the advantage of requiring a low numberof coordinates. For open-loop multibody systems, they even require the minimumamount of coordinates. Take a look at figure A.5(b). θ1 and θ2 are now relativeangles and the joint space is described by (θ1, θ2). There are as many coordinatesas degrees of freedom. In this case, the joint space is the same as the configurationspace. In closed-loop systems, the joint coordinates are related to each other bymeans of some constraint equations. In that case, the configuration space is asubspace of the joint space.

A.4.3 KINEMATIC CONSTRAINTS

HOLONOMIC CONSTRAINTS

Geometric constraints may be imposed on a multibody system, restricting the spaceof motion. These constraints are commonly mathematically represented by a setof (generally nonlinear) algebraic equations in terms of the generalized coordinatesq and the time t:

Φ(q, t) = 0 (A.35)

Geometric constraints are a subset of the larger set of kinematic constraints, whichinvolve not only the generalized coordinates, but also the coordinate velocities q:

B(q, q, t) = 0 (A.36)

In most applications, the constraints are linear in the velocities. This is e.g. thecase when the geometric constraints are given in their differential shape.

B(q)q = C(t) (A.37)

This is called a Pfaffian form. If equations (A.37) are integrable, i.e. if they areequivalent to a set of geometric constraints (A.35), they are said to be holonomic.

NONHOLONOMIC CONSTRAINTS

Non-holonomic constraints are constraints that are not integrable and can thereforenot be transformed into a set of geometric constraints. As a consequence of thenon-integrability, there exists a discrepancy between the local and global degreesof freedom. The local degrees of freedom are basically defined by the dimension ofthe subspace of allowed velocities, while the global degrees of freedom are definedby the dimension of the subspace of the configurations that the system may have.A typical example of a nonholonomic system is a car. A car can move forwardsand backwards and can turn its front wheels left and right. Locally, it thus has twodegrees of freedom. On the other hand, a good driver should be able to get thecar in any position and orientation (in 2D of course), but he may need to performsome manoeuvres to do so. The car has 3 global DOF.

126 APPENDIX A

OPEN-LOOP, CLOSED-LOOP AND CONSTRAINED MULTIBODY

SYSTEMS

In the specialized literature about multibody dynamics, one often finds the termsopen-loop, closed-loop and constrained systems. A closed-loop system is a systemcomprising at least one kinematical loop. Conversely, open-loop systems do not haveany kinematic loop. This implies that they can be described with joint coordinateswithout the need of adding constraint equations. The term constrained system isnot always clear. A multibody system can be described with independent coor-dinates or with dependent coordinates and associated constraint equations. Fromthat point of view, the adjective “constrained” applies to the mathematical rep-resentation of the system, rather than to the system itself. One may argue that“open-loop” contains a contradiction, because a loop does imply a closed structure.Some authors indeed use “open chain” instead, but the original terminology is sowidely spread that this thesis will not make an exception.

A.5 FORWARD KINEMATICS

In robotics, a forward kinematics problem is often described as a problem of findingthe position and orientation of a robot’s end-effector, also called tool center point(TCP)[Craig, 1989]. Or to put it another way, given the joint coordinates (anglesand translations), the position and orientation of the reference frame fixed to theend-effector need to be found. It is a purely geometrical problem and is generallyseen as a mapping from joint space to Cartesian space (see section A.4). In multi-body dynamics, one is not only interested in the motion of the end-effector, butin the motion of each body individually. The definition of a forward kinematicsproblem thus needs to comprise the determination of the position and orientationof all local reference frames with respect to an inertial frame.

A forward kinematic problem is fairly simple and can always be solved using aforward recursive method. Consider figure A.6 for instance. It represents a serialrobot with 6 DOF. The global configuration of the first body can be represented bya homogeneous transformation matrix 0H1. This transformation matrix is generallydependent on the joint coordinates, in this particular case coordinate q1 describinga vertical translation. The relative configuration of body 2 referenced to body 1 issimilarly given by matrix 1H2. The global configuration of body 2 can now easilybe obtained by multiplication of both matrices:

0H2 = 0H11H2 (A.38)

The process can be continued all the way to the tool center point, using followingrecursive formula:

0Hi = 0Hi−1i−1Hi (A.39)

KINEMATICS 127

Figure A.6: Serial robot

As an example, the first two homogeneous transformation matrices are given ex-plicitly:

0H1 =

1 0 0 00 1 0 00 0 1 q1

0 0 0 1

1H2 =

cos q2 − sin q2 0 0sin q2 cos q2 0 0

0 0 1 l120 0 0 1

(A.40)

The construction of homogeneous matrices can become quite involved, a popularway to deal with them is reverting to the so-called Denavit-Hartenberg parameters[Denavit & Hartenberg, 1955].

A.6 INVERSE KINEMATICS

An inverse kinematics problem is more difficult: find the set of all possible configu-rations that the system may adopt to attain a given position and orientation of theend-effector. As the problem’s definition reveals, the answer is not always unique.

128 APPENDIX A

There may be multiple and even an infinite amount of solutions, there may alsobe no solution at all. Assume that the desired pose of the end-effector is given byhomogeneous matrix HD and that the configuration of the system is representedby n generalized coordinates q. The homogeneous transformation matrix of theend-effector is then a function of q.

0HTCP (q) = HD (A.41)

This is a set of 6 nonlinear equations in n unknowns q. In a few simple cases, theseequations can be solved analytically [Craig, 1989], but in most cases, one has torevert to numerical algorithms such as the Newton-Raphson method.

A.7 VELOCITIES

The motion of a multibody system is not only characterized by its configuration,but also by the change in configuration. This change can be described by thetime derivative of the generalized coordinates, but also by the linear and angularvelocities of each individual body. In this section, the concept of spatial velocityis introduced and the propagation of velocities throughout a multibody system isbriefly analyzed.

A.7.1 VELOCITY FIELD IN A RIGID BODY

A rigid body is a system of particles whose relative distances remain constant. Toknow the body’s configuration, one needs the position of one single point of thebody and the orientation of a reference frame fixed to that body. Similarly, to knowthe instantaneous motion of the body, one needs the linear velocity of one singlepoint O, and the angular velocity ω of the body-fixed frame. This informationallows the calculation of the velocity of any point A of the rigid body (see fig.A.7):

vA = vO + ω × OA = vO + ω.OA (A.42)

Remember that the angular velocity is the same for all points of a rigid body, thisis why the index is dropped. The spatial velocity Ω is defined as a concatenationof the linear and the angular velocities of the body, taken in the origin of the bodyfixed reference frame4.

ΩO =

(vO

ω

)(A.43)

4A slightly different definition of spatial vectors is used in [Featherstone, 2001].

KINEMATICS 129

Figure A.7: Velocity distribution of a rigid body

The index is important, as it denotes the point of which the velocity is taken. Onecan immediately derive the transformation operator AT V

O for the spatial velocitiesusing (A.42):

ΩA =

(vA

ω

)=

(I AO

0 I

)(vO

ω

)= AT V

O ΩO (A.44)

Operator AT VO is also called spatial shift dyadic [Critchley & Anderson, 2003].

A.7.2 JOINT MATRIX

The superposition principle holds for the propagation of velocities through a multi-body system. Take a look at fig. A.5(b). One can write the spatial velocity of body2 as the combination of the spatial velocity of body 1 and the relative velocity Ωr2

induced by the joint (the index r stands for relative).

Ω2 = 2T V1 Ω1 + Ωr2

(A.45)

The relative spatial velocity Ωr2depends on the joint (also called coordinate)

velocity θ2. It can be expressed in the form

Ωr2= E2θ2 + Ωt2 (A.46)

Ωt2 arises from a driving constraint that is explicitly dependent on time: so-calledrheonomous constraints. This does not mean that Ωt2 itself is dependent on time,

130 APPENDIX A

it may be a constant as well. As a matter of fact, it is zero in many practicalapplications. E2 is called the joint matrix, it represents the space of virtual relativemotion (i.e. when Ωt2 is removed). Its components are the partial derivatives ofthe relative spatial velocity to the joint velocities. More generally, one can writefor an arbitrary body K:

ΩK = KT V

K−1ΩK−1 + EK qK + ΩtK=

(I ˜OK−1OK

0 I

)(vK−1

ωK−1

)+ EK qK + ΩtK

(A.47)

The dimensions of the joint matrix EK (6 × nK) and the coordinate velocitiesvector qK (nK × 1) for a certain joint K are dependent on the number of degreesof freedom nK of that joint.

EXAMPLE 1: PIN-JOINT

Consider two rigid bodies K−1 and K interconnected by a pin-joint. The rotationaxis is directed along the ZK-axis. In that case, the joint matrix of body K canbe written as:

KEK =(0 0 0 0 0 1

)T(A.48)

The upper left index denotes that the components of the matrix are expressed withrespect to reference frame K. Note that the joint matrix is constant in the localreference frame.

EXAMPLE 2: SPHERICAL JOINT

An arbitrary rotation can be described in different ways, but quaternions are usedin this example, for the advantages described in section A.2.3. As was seen in thatsection, the rotation of a vector rv can be represented by a quaternion multiplica-tion of a pure quaternion ρ = (0 rv)T .

ρ′

= ǫ ∗ ρ ∗ ǫ (A.49)

Taking the time derivative of above expression results in

ρ′

= ǫ ∗ ρ ∗ ǫ + ǫ ∗ ρ ∗ ˙ǫ (A.50)

This holds as ρ is fixed to the inertial reference frame. Note that, as ǫ is a unitquaternion, the following properties hold:

ǫT ǫ = 1 ⇒ ǫT ǫ = ǫT ǫ = 0 (A.51)

KINEMATICS 131

ǫ ∗ ǫ = ǫ ∗ ǫ =

(10

)(A.52)

The inverse relation of (A.49) can now be written as

ρ = ǫ ∗ ρ′

∗ ǫ (A.53)

Substitution of (A.53) in (A.50) yields

ρ′

= ǫ ∗ ǫ ∗ ρ′

+ ρ′

∗ ǫ ∗ ˙ǫ (A.54)

Let’s now establish the link between the active and the passive interpretations ofthe rotation. Quaternion ρ can be associated with 1r, which is the projection ofphysical vector r in the body-fixed frame. ρ

′

can be associated with 0r, being theprojection in the inertial frame. Considering equations (A.26) and (A.51), one seesthat the scalar parts of ǫ ∗ ǫ and ǫ ∗ ˙ǫ are zero and their vector parts are opposite.So, using 0r = 0ω × 0r, one may identify

(0

0ω

)= 2ǫ ∗ ǫ = 2QT ǫ (A.55)

The inverse relation is then

ǫ =1

2

(0

0ω

)∗ ǫ =

1

2Q

(0

0ω

)(A.56)

If the spatial velocity, and hence the angular velocity, is known in the body-fixedframe, one may prefer to use the associated expressions:

(0

1ω

)= 2ǫ ∗ ǫ = 2QT ǫ (A.57)

ǫ =1

2ǫ ∗

(0

1ω

)=

1

2Q

(0

1ω

)(A.58)

The second last relation (A.57) can also be written as

1Ω = 1Eǫ (A.59)

which finally enables one to write the desired joint matrix in the case Euler param-eters are used:

132 APPENDIX A

1E = 2

0 0 0 00 0 0 00 0 0 0ǫ1 ǫ0 ǫ3 −ǫ2ǫ2 −ǫ3 ǫ0 ǫ1ǫ3 ǫ2 −ǫ1 ǫ0

(A.60)

It is important to note that the time derivative E is generally not equal to zero,unlike in the pin-joint example.

A.8 ACCELERATIONS

The linear velocity is defined as the time derivative of a position vector with respectto the fixed inertial frame. To obtain accelerations, the velocities need also to bedifferentiated with respect to the same fixed frame.

d0Ω

dt=

(a

d0ω

dt

)(A.61)

Later, it will become clear that the use of the time-derivative of the velocities withrespect to the moving body-fixed frame is more convenient:

dKΩ

dt= Ω =

(v

ω

)(A.62)

Remember that the dot-symbol denotes the derivative with respect to the localaxes. There is a clear difference between expressions (A.61) and (A.62). Considerthe motion of the center of mass of the system in fig.A.1 for example, for whichthe body rotates at constant speed. In that case one gets:

d0ΩG

dt=

(aG

0

)ΩG =

(0

0

)(A.63)

Finally, it is possible to obtain a forward recursive expression for the calculation ofspatial accelerations by differentiation of (A.47)(see also appendix D.4.2):

ΩK = KT VK−1ΩK−1 + (EK qK × I)T KT V

K−1ΩK−1 + EK qK + EK qK (A.64)

A.9 CONCLUSIONS

In this chapter, an overview of the different kinematic aspects needed to performmultibody simulations has been given. The concepts of generalized coordinates and

KINEMATICS 133

configuration space have been introduced. The description of the pose of a rigidbody has been discussed and the forward and inverse kinematic problems have beenexplained. The most important thing the reader should remember is the conceptof spatial velocities and how these are propagated through a multibody system.

134 APPENDIX A

Appendix B

DYNAMICS

“Mechanics is the paradise of the mathematical sciences, because bymeans of it one comes to the fruits of mathematics.”

-Leonardo da Vinci

Dynamic analysis is the study of the relationship between force and motion. Asin kinematic analysis, two main problems can be distinguished: the forward andthe inverse dynamics. The inverse dynamics aims at calculating the driving forcesand torques that impose a specific motion and the reactions that occur in thesystem’s joints. All external forces, the system’s configuration, the velocities andaccelerations of the mechanism are needed to solve this problem. The inversedynamics is mainly used for control purposes, where estimations of the motortorques are needed to obtain a desired trajectory. A well-known method that usesthis technique is the computed torque control [Craig, 1989]. The forces to which asystem is subjected can also be needed for design, where dimensioning of criticalelements is required. In many cases however, the external forces are known and oneis interested in the motion of the mechanism. This is the opposite problem: theforward dynamics. Given the initial conditions and all external and driving forcesexerted on the system, the motion has to be found over a certain time interval.The forward dynamics enables to simulate a mechanism and to predict its behaviorunder given circumstances. It is used for virtual prototyping, animation, advancedcontrol techniques and many other applications (see section 1).

B.1 STATE SPACE

In the previous chapter, it was seen that the configuration of the system can bedescribed by a set of generalized coordinates. The position and orientation of eachbody is however not sufficient to predict the motion of a multibody system. Indeed,dynamic equations of unconstrained systems can be brought to a set of 2n first or-

135

136 APPENDIX B

der ordinary differential equations (n being the number of coordinates). TheseODEs require 2n initial values to give a unique solution. Thus, 2n coordinates areneeded to describe the state of the system unequivocally at any given time. Thesecoordinates are called state coordinates. Beside the n generalized coordinates q,one can opt for the time derivative of the generalized coordinates q, also called gen-eralized velocities, to complete the set of state coordinates. A linear combinationof these generalized coordinates is also a viable choice. These linear combinationsare called generalized speeds [Kane & Levinson, 1985]. Yet another choice could bethe so-called canonical momenta p, the backbone of this work. The space spannedby the state coordinates is called state space. One can visualize the motion of asystem as the motion of a point in that space.

B.2 DIFFERENT FORMALISMS

Figure B.1: Sir Isaac Newton (1643-1727)

Mechanical systems move according to the laws of physics. These laws can beformulated mathematically and then solved to obtain the motion of the system.Although the laws of physics are unique, the mathematical formulation is not:several representations are possible to describe the same phenomenon. These rep-resentations are of course equivalent, but their numerical implementations generallydiffer. Even if the simulated behavior is the same in the long run, large discrep-ancies in efficiency may be observed. In the following sections, several formalisms

DYNAMICS 137

are described: the Newton-Euler equations, the principle of virtual power, the La-grangian and the Hamiltonian equations and the Gibbs-Appell equations. Thisis of course not an exhaustive list, many other formalisms exist or are still to beinvented.

B.2.1 NEWTON-EULER EQUATIONS

PARTICLE DYNAMICS

The modern mathematical representation of Newton’s1second law for a particle is

d

dt(mv) = f (B.1)

1Isaac Newton was born in 1642, the exact same day Galileo Galilei died. In 1664, a bubonicplague outbreak swept through Europe, an event that would ironically cause a very fortunateyear for science. The plague forced the Trinity College in Cambridge to close its doors for 2 years,making twenty-two year old Isaac Newton retreat to his home in Woolthorpe. The two years hestayed there turned out to be the most fructuous of his life. It is there that, according to themyth, Newton was contemplating the day-time moon, seated under a tree, when an apple fellfrom its branch and hit the ground. Supposedly, Isaac started wondering why that small juicyorb was attracted by the earth, while this huge piece of rock would just stay there high in the sky.It didn’t last long before his genius came up with the idea that the moon was really falling tothe earth, but constantly failed to hit it, resulting in a permanent orbiting motion. This impliedthat both the apple and the moon where under influence of a same force and Newton created hisgravity theory, establishing the link between celestial (Kepler) and terrestrial (Galilei) mechanics.It is not the only spark of genius that Newton experienced in his lifetime, he also made significantbreakthroughs in optics and fluxion, nowadays known as calculus. Some arguably state thatNewton is the greatest scientist up till now and will probably remain the greatest scientist dueto the unique historical climate he lived in. After the plague, he came back to Trinity College asa professor and lectured optics and calculus. In 1695, he was appointed as warden at the BritishMint. Newton’s achievements in the 1690’s were overshadowed by a dispute with Leibniz aboutwho invented the differential calculus. This dispute would have important repercussions on therelations between Continental and British researchers. British mathematicians were reluctant ofsharing their research for over a century.

It is not commonly known that Newton had many other passions, his heart was really in moreoccult fields as alchemy and astrology. He was also very interested in theology and spent a lot oftime studying the Bible. Newton was not a Trinitarian, he once wrote an essay in an attempt tofind the corruptions that had been introduced into Christianity (supposedly in the 4th centuryby Athanasius at the Council of Nicea). This work was not published during his life time, as thiswould probably have him expulsed from his position in Cambridge (no wonder, given the college’sname). Anyway, there is agreement that the Philosophiae Naturalis Principia Mathematica,published in 1687, was Newton’s biggest achievement and the first and greatest work ever writtenon theoretical physics. In the first part of the Principia, he discusses his laws of motion and thelaw of gravitation that laid the foundations of today’s Newtonian mechanics. Here is a modernphrasing of the three laws: An object at rest tends to stay at rest and an object in motion tends to stay in motion

with the same speed and in the same direction unless acted upon by an unbalanced force. The acceleration of an object as produced by a net force is directly proportional to themagnitude of the net force, in the same direction as the net force, and inversely proportionalto the mass of the object.

138 APPENDIX B

m being the mass of the particle, v its velocity and f the total force impressedupon it. It is also often written in terms of the acceleration a:

f = ma (B.2)

This form was first introduced by Euler.

SYSTEM OF PARTICLES

For a system of N particles, one can write equation (B.1) for each element individ-ually and take the summation:

N∑

i=1

d

dt(mivi) =

d

dt

N∑

i=1

(mivi) =

N∑

i=1

fi = f (B.3)

considering constant masses and f being the resultant of all the forces fi acting onthe individual particles. It is obviously not possible to predict the motion of eachparticle individually with above equation, but it is possible to obtain a notion ofthe global motion of the entire system by virtue of the center of mass. The centerof mass is defined as

xG =

∑Ni=1 mixi∑N

i=1 mi

(B.4)

Equations (B.3) can then be reformulated as For every action there is an equal and opposite reaction.

The first law can essentially be recognized as Galileo’s concept of inertia, which represents thetendency of the body to resist any change in motion. Sir Arthur Eddington rephrased it this way:“Every body continues in its state of rest or of uniform motion in a straight line; except insofaras it doesn’t”. [Eddington, 1967]. He meant by this that the first law may not be a fundamentaltruth, it is just a law that appears to be able to describe certain aspects of the physical world verywell. No one can however prove it, no one can guarantee that no unknown influence acts on thebody. A physical law is a matter of belief, if some observations do not agree with predictions, ascientist will rather search for the cause of the disturbance than question the law itself. Einsteinwould indeed show that the Newtonian equations are not correct anymore for objects moving nearthe speed of light (“Newton, forgive me.”, Einstein after introducing his relativity theories). Formost mechanical engineering applications however, the speeds are more modest, and the equationshold.

Newton’s second law is in contradiction with Aristotle’s view on motion. Failing to appreciatethe effect of frictional force, Aristotle believed that a body could only be in motion if a force wasexerted on it.

Isaac Newton was knighted by Queen Anne in 1705 and died in 1727, at the honorable age of85. He probably was the single most important contributor to the development of modern scienceand certainly deserves the epigraph on his tomb: “Mortals! Rejoice at so great an ornament tothe human race!”.

DYNAMICS 139

d

dt(mvG) = f (B.5)

with

m =

N∑

i=1

mi (B.6)

vG =

∑Ni=1(mivi)

m(B.7)

being respectively the total mass of the system of particles and the velocity of thecenter of mass. So far, the linear motion of the center of mass of the system canbe calculated. By taking the moment of equations (B.1) about a point O and bysumming over all particles, one can get an idea of the global rotation of the systemabout that point.

N∑

i=1

ri ×d

dt(mivi) =

N∑

i=1

ri × fi = tO (B.8)

⇒d

dt

n∑

i=1

(ri × mivi) = tO − vO × mvG (B.9)

ri representing the position vector from point O to particle i. tO is the resultantof the force moments acting on the system about point O and vO is the linearvelocity of reference point O. Note that dri

dt = vi − vO, as the time derivative istaken with respect to the inertial reference frame.

RIGID BODY

A rigid body is basically a system of particles that do not move relative to eachother. The distance between each pair of particles thus remains constant at alltimes. As a consequence, it is sufficient to know the linear and angular velocitiesin one point to know the velocity distribution in the whole system. Rememberequation (A.42) from the kinematics section (appendix A):

vA = vO + ω × OA (B.10)

The linear momentum of a rigid body is defined as:

pl = mvG (B.11)

140 APPENDIX B

It can be interpreted as a measure for the amount of translational motion. Substi-tuting above equation in (B.5) gives following concise form:

dpl

dt= f (B.12)

Similarly, the angular momentum of a body with respect to a certain point O isdefined as

paO=

N∑

i=0

OXi × mivi (B.13)

The position vector ri from reference point O to particle i is written as OXi.Equation (B.13) allows to reshape (B.9) into

dpaO

dt= tO − vO × pl (B.14)

Definition (B.13) is not easy to work with, but in can be turned into a more suitableexpression as follows (using (D.7)):

paO=

N∑

i=1

OXi × mi(vO + ω × OXi) (B.15)

= OG × mvO +

n∑

i=1

mi(OXTi OXiI − OXiOXT

i )ω (B.16)

= OG × mvO + JOω (B.17)

with JO the inertia tensor about point O.

JO =

n∑

i=1

mi(OXTi OXiI − OXiOXT

i ) (B.18)

Taking the time derivative of (B.15)

dpaO

dt=

d

dt(JOω) + vG × mvO + OG × maO = tO − vO × mvG (B.19)

finally yields the Euler equations:

d

dt(JOω) = tO − OG × maO (B.20)

DYNAMICS 141

Note that the summation symbols can be replaced by an integral symbol in thelimit. A rigid body can indeed be interpreted as a collection of particles or as acontinuum. The global results remain the same.

B.2.2 D’ALEMBERT’S PRINCIPLE

Figure B.2: Jean Le Rond D’Alembert (1717-1783)

The principle of virtual power originates from statics and roughly states that thereaction forces and torques do not deliver any net power under a virtual motion.The principle of virtual work is equivalent but is described in terms of infinites-imal displacements instead of velocities. The latter are more intuitive though.D’Alembert’s principle is basically an extension of the principle of virtual work tomoving systems, the only difference is the added inertial term [Goldstein, 2002].

n∑

i=1

vvT

i fci=

n∑

i=1

vvT

i (fi − miri) = 0 (B.21)

fi represents the external forces applied on particle i, fcithe internal forces, also

called constraint or reaction forces. A virtual velocity vvi is a velocity that is allowed

by the system provided all explicitly time dependent (or rheonomous) constraintsare kept fixed. If only explicitly time independent (scleronomous) constraints are

142 APPENDIX B

present, a virtual velocity is a possible velocity. Take a look at the recursive equa-tion for a spatial velocity (A.47):

ΩK = KT V

K−1ΩK−1 + EK qK + ΩtK(B.22)

The virtual spatial velocity ΩvK is obtained by dropping the last term:

ΩvK = KT V

K−1ΩvK−1 + EK qK (B.23)

Similarly to the joint matrix E, one can define a joint constraint matrix Ec thatforms a basis for the spatial reaction forces in the joint. It is clear that ET Ec =ET

c E = 0. In other words: the subspace of the motion allowed by the joint isorthogonal to the subspace of the constraints induced by the joint. D’Alembert’sprinciple is nothing more than an expression of this orthogonality. Observe thatthe union of both subspaces includes the whole 6D-space.

While it was Jean Le Rond D’Alembert who developed the method, James Bernoullideserves some credit, as it was he who really came up first with the concept.D’Alembert’s principle is often referred to as Kane’s method [Kane & Levinson, 1985],especially in the United States. Both are basically the same, but Kane emphasizesthe use of generalized speeds or quasi-velocities. Generalized speeds are defined aslinear combinations of time derivatives of generalized coordinates. Their use maylead to more efficient equations of motion [Mitiguy & Kane, 1996].

B.2.3 LAGRANGIAN EQUATIONS

The equations of motion of a mechanical system can be derived from a completelydifferent starting point: the principle of least action. Maupertuis introduced theprinciple and it was further developed by Euler, Lagrange2 and Hamilton. Theprinciple of least action originates from the belief that the perfection of the universe

2Joseph-Louis Lagrange was an Italian/French physicist and one of the greatest mathemati-cians of the 18th century. He was born in Turin in 1736 with the name Giuseppe LodovicoLagrangia but soon started to sign ’Lagrange’ as he leant more towards his French roots fromhis father’s side. Surprisingly, Lagrange had initially not been interested in mathematics. Hewas however mostly selftaught and published his first mathematical paper aged 18. In 1766,he left the Royal Academy of Sciences in Turin to succeed to Euler as director of mathemat-ics in Berlin. Lagrange stayed there for about 20 years, working on astronomy, dynamics, fluidmechanics and probability. He also contributed to the foundation of group theory and the cal-culus of variations. Lagrange’s most important achievement is undoubtedly his Mecanique An-alytique[Lagrange, 1853], a comprehensive work which explains the whole of solids and fluidsmechanics starting from the principle of virtual work. Not one single diagram can be found inhis treatise or, as Lagrange himself stated: One will not find figures in this work. The meth-ods that I expound require neither constructions, nor geometrical or mechanical arguments, butonly algebraic operations, subject to a regular and uniform course.” Hamilton would describe theMecanique Analytique as a scientific poem. Lagrange left Berlin for the Academie des Sciencesin Paris in 1787. He survived the French Revolution, but the Reign of Terror in 1793 wouldsuppress the Academie des Sciences and he would have been arrested for being a foreigner were itnot for the intervention of his friend Lavoisier. Lagrange was granted an exception, but Lavoisier

DYNAMICS 143

Figure B.3: Joseph-Louis Lagrange (1736-1813)

requires an economical use of energy. Natural motions must be such as to make theaction minimum. When a change occurs in nature, the quantity of action necessaryfor the change is the least possible.” [de Maupertuis, 1756]. In classical mechanics,the action is the Lagrangian L, which is the difference between kinetic (T ) andpotential (V ) energy:

L = T − V (B.24)

The potential energy arises from conservative forces imposed on the system. Non-conservative forces will be added afterwards. A variant of the principle of leastaction, referred to as Hamilton’s principle will be used here. Hamilton’s principlestates that the integral of the action is a minimum for the real motion:

δ

∫L(q, q, t)dt = 0 (B.25)

Minimizing this functional is a typical problem in the calculus of variations andthe solution is given by the so-called Euler-Lagrange variational equations (seeappendix D.1):

d

dt(∂L

∂q) −

∂L

∂q= 0 (B.26)

would loose his head to the guillotine one year later. One month earlier, Lagrange was appointedprofessor of analysis at the newly founded Ecole Polytechnique. Lagrange died in 1808, one weekafter receiving the Grande Croix de l’Ordre Imperial de la Reunion

144 APPENDIX B

These equations are known as Lagrange’s equations of the second kind. They forma set of ordinary differential equations (ODE) of second order. The Lagrangeequations can easily be derived from D’Alembert’s principle as well. The principleof least action is independent of the chosen coordinates. Hence, the Lagrangian canbe expressed with any valid choice of generalized coordinates. One must however becareful when using a set of dependent coordinates. In that case, one is confrontedwith a constrained optimization problem. This is again a well-known problem invariational calculus. Therefore, a new function L+ incorporating the constraints isintroduced:

L+ = L + λT Φ (B.27)

It is called the augmented Lagrangian. λ is a vector of Lagrange multipliers andΦ(q, t) = 0 is a set of m nonholonomic constraint equations. The solution to theproblem is given by3:

d

dt(∂L

∂q)T − (

∂L

∂q)T − ΦT

qλ = 0 (B.30)

with Φq = ∂Φ∂q

the Jacobian of the constraint equations. These second orderdifferential equations need to be solved together with the constraint equations.They form a mixed set of differential algebraic equations (DAE) and are referredto as Lagrange’s equations of the first kind. The kinetic energy T is commonlywritten as

T =1

2qT M(q)q (B.31)

with M the generalized mass matrix, which is symmetric. The potential energy isusually independent of the velocities. Hence, one may write

(∂L

∂q)T = M(q)q (B.32)

3The (partial) derivative of a vector p = (p1 p2...pn)T with respect to a scalar t and the partialderivative of a scalar L with respect to a vector q = (q1 q2...qn)T are respectively defined as:

dp

dt=

dp1dt

dp2dt

...dpn

dt

T ∂L

∂q=

∂L∂q1

∂L∂q2

... ∂L∂qn

(B.28)

Analogously, the partial derivative of a vector q with respect to a vector q is given by

∂q

∂q=

0BBB ∂q1∂q1

∂q1∂q2

...∂q1∂qn

∂q2∂q1

∂q2∂q2

...∂q2∂qn

... ... ... ...∂qn

∂q1

∂qn

∂q2...

∂qn

∂qn

1CCCA (B.29)

DYNAMICS 145

d

dt(∂L

∂q)T = Mq + Mq (B.33)

which finally leads to the following set of n second order differential equations andm algebraic equations[de Jalon & Bayo, 1994]:

Mq = Lq − Mq − ΦTqλ (B.34)

Φ = 0 (B.35)

Note that the components of the generalized mass matrix M acting on q can befound as

Mij ,∂2L

∂qi∂qj(B.36)

The mass matrix is also commonly denoted by the symbol H, because it is a Hessianmatrix.

It is convenient to turn the set of DAE (B.30) into a set of ODE, so that conven-tional ODE-solvers can be used (see appendix C). This can be done by differenti-ating the constraint equations twice with respect to time:

Φ(q, t) = 0 (B.37)

Φqq = −Φt (B.38)

Φqq + Φqq + Φt = 0 (B.39)

Φt is the partial derivative of the constraints with respect to time. Combinationof (B.34) and (B.39) gives following expression:

(M ΦT

q

Φq 0

)(q

λ

)=

(Lq − Mq

−Φqq − Φt

)(B.40)

which can be solved for q and λ. The use of the constraint equations at the ac-celeration level (B.39) does not have any theoretical repercussions, it does howeverinduce problems during numerical integration. As small numerical errors are in-troduced on the acceleration level, these will be integrated twice and will resultin uncontrolled errors at the velocity and position levels. Therefore stabilizationmethods for the constraint violation errors are needed. Well known procedures arethe Baumgarte [Baumgarte, 1983] or the more general penalty stabilization methodand the coordinate partitioning method [Haug, 1989]. Promising alternatives using

146 APPENDIX B

a manifold approach where proposed in [Blajer, 2002] and [Terze et al., 2001]. Thissubject is discussed in more details in appendix C. Note that the term ΦT

qλ hasa simple physical interpretation: it represents the generalized reaction forces. In-deed, the Jacobian of the constraint equations is orthogonal to the virtual velocities(Φqq

∗ = 0) and must hence span the same subspace as the constraints. The Jaco-bian forms a basis for the constraints, the Lagrange multipliers are a measure forthe magnitude of the reaction forces.

When non-conservative forces Qnc are added to the system, the action becomesthe sum of the Lagrangian and the work Wnc done by these non-conservative forces.Hamilton’s principle is then reformulated as:

δ

∫(L + Wnc)dt = 0 (B.41)

Considering the fact that δWnc = δqT Qnc, the Lagrangian equations of motionbecome:

Mq = Lq − Mq − ΦTqλ + Qnc (B.42)

Lagrange’s equations can be cumbersome to establish. While it becomes practi-cally impossible to derive them manually for even a few degrees of freedom, com-puterized methods may provide relief. The partial derivatives tend to produce tonsof terms, which can however be greatly simplified. A symbolical optimization istherefore not a superfluous luxury. It is interesting to note that the equationsshould not be used as a recipe, the underlying assumptions should always be keptin mind. For example, equations (B.40) are only valid for systems with constantmasses [Pesce, 2003]. Systems with varying mass, as the deployment of a heavycable from a reel, introduce extra terms in the equations of motion.

B.2.4 HAMILTONIAN EQUATIONS

A set of Hamiltonian equations of motion is expressed in terms of the generalizedcoordinates q and the canonical momenta p defined according to

p = (∂L(q, q, t)

∂q)T (B.43)

They can be obtained by applying a Legendre transformation on Lagrange’s equa-tions, resulting in

q = (∂H

∂p)T (B.44a)

DYNAMICS 147

p = −(∂H

∂q)T + Qnc − ΦT

qλ (B.44b)

Φ = 0 (B.44c)

This is a set of 2n + m first order differential-algebraic equations. A detailedderivation and discussion are given in chapter 2.

B.2.5 GIBBS-APPELL

A disadvantage of the Lagrangian approach is that nonholonomic constraints can-not be embedded in the equations of motion without reverting to Lagrange multi-pliers. Gibbs and Paul Appell independently provided a solution to that problem,by opting for quasi-coordinates and by introducing a function S in analogy withthe kinetic energy:

S =1

2

∑

i

mixTi xi (B.45)

Saint-Germain later called this the energy of accelerations. In terms of S, theequations of motion are written as

∂S

∂q= QT (B.46)

Q representing the generalized forces. For those who will not settle for less thana full derivation of these equations, [Appell, 1899] may provide satisfaction. Ac-cording to [Pars, 1965], the Gibbs-Appell equations are “Probably the simplest andmost comprehensive equations of motion so far discovered.”

B.3 CHOICE OF STATE COORDINATES

A large variety of dynamics formulations is available, many of which differ in onlya few details. Probably the most important choice to face is how to model theconstraints. Either one finds minimal coordinates by eliminating the constraintsor one keeps all coordinates and must then construct the constraint forces. Withinthe Lagrangian formalism this leads to the equations of second and first kind re-spectively [Goldstein, 2002]. In the engineering literature one often speaks of astate space and a descriptor form of the equations of motion.

The descriptor form uses a high number of coordinates (6 or 7 in 3D, dependingon whether Euler parameters are used or not) and a high amount of constraints,which lead to a DAE formulation. The equations of motion however are typicallyvery simple. The Jacobian matrix of the constraint equations usually has a sparse

148 APPENDIX B

structure (block diagonal). Joint constraints indeed only affect adjacent bodiesand their corresponding coordinates. This has been exploited by [Baraff, 1996] toobtain an O(n) method using Cartesian coordinates.

In the state space approach, also called minimum form, all constraints are embed-ded in the coordinates. The number of equations is reduced to a minimum and oneis confronted with a set of ODEs. It can however be very complicated to establishthe equations and they are typically strongly nonlinear [de Jalon & Bayo, 1994].

Joint or relative coordinates are somewhere between the descriptor form and thestate-space form. A low number of coordinates is used, but a few constraints arestill required. The advantage is that the topological structure of the system can befully exploited, resulting in efficient recursive algorithms.

B.4 CONCLUSIONS

There is obviously a large amount of different mathematical representations to de-scribe the dynamic behavior of a mechanical system. One can revert to vectorialformalisms such as the Newton-Euler equations and d’Alembert’s principle. On theother side are the scalar representations such as Lagrange and Hamilton’s equa-tions. An important choice is whether the constraints are eliminated by reducingthe number of coordinates or whether they are incorporated into the equations ofmotion by introducing additional reaction forces.

Appendix C

NUMERICAL INTEGRATION OF THE

EQUATIONS OF MOTION

“As long as algebra and geometry have been separated, their progresshave been slow and their uses limited; but when these two sciences havebeen united, they have lent each mutual forces, and have marched to-gether towards perfection.”

-Joseph-Louis Lagrange

The simulation of a multibody system consists of two steps: the formulation andthe solution of the equations of motion. Since no analytical solution exists formost practical MBS (certainly in case of a priori unknown external forces due touser interactions), a numerical approximation of the solution is needed. Findingthis approximative solution is the job of the numerical integrator. More specifi-cally, the numerical integrator has to calculate the system’s state after an arbitrarytime, given the actual state. This is called an initial value problem (IVP). Thereexist many different solvers, and it is not a simple task to pick the most suitableone. As a matter of fact, the choice of numerical integration scheme does not onlydepend on the system being studied, but also on its motion, on the required ac-curacy, stability issues, restrictions on processing times, the chosen formalism andgeneralized coordinates, boundary conditions, tolerated numerical dissipation...

Most proposed numerical integrators are designed to solve canonical sets of firstorder ordinary differential equations (ODE), although a few methods exist for solv-ing mixed sets of differential-algebraic equations (DAE). For the latter however, onemay prefer to transform the differential-algebraic equations into ordinary differen-tial equations to get access to a wider variety of methods. The drawback of thatchoice is the introduction of so-called constraint violation errors, which will be dis-cussed at the end of this section. DAE’s are characterized by a so-called differentialindex. The acceleration based formulations have an index of 3, the Hamiltonian for-mulation has index 2 [de Jalon & Bayo, 1994]. As shown in [Brenan et al., 1989],

149

150 APPENDIX C

index 2 DAE’s have a better behavior during numerical integration. Hence, theuse of canonical momenta may be numerically advantageous compared to the useof accelerations.

C.1 EXPLICIT METHODS

Explicit methods calculate the state at the next time step using information aboutthe state at the actual and past time steps. The advantage is that everything can becomputed explicitly, so no iterative procedures are required. Unfortunately, mostexplicit methods have a rather restricted stability range [de Jalon & Bayo, 1994].

C.1.1 RUNGE-KUTTA METHODS

(a) Runge(1856-1927) (b) Kutta(1867-1944)

Consider following canonical set of first order ordinary differential equations:

y = f(y, t) (C.1)

The most simple numerical method for solving an initial value problem can beobtained by cutting a Taylor series to the 1st order:

y(tn + ∆t) = yn+1 = yn + yn∆t + O(∆t2) = yn + f(yn, tn)∆t + O(∆t2) (C.2)

NUMERICAL INTEGRATION OF THE EQUATIONS OF MOTION 151

This is called the Euler method. Although it is a tempting option for its simplicity,the method is not very accurate, and one will often end up with very small timesteps to achieve the required accuracy. Indeed, the error term is of O(∆t2), soif the time step is cut in n equal parts, the error is divided by n2, but one hasto take n times as many steps. This means that the accumulated error dependslinearly upon ∆t. More sophisticated methods requiring more function evaluationsare likely to outperform Eulers method because their higher cost per step is largelycompensated by the larger stepsizes they allow. Moreover, Euler’s method is notvery stable.

One can now consider using the Euler method (C.2) to take an intermediate stepto the midpoint of the time interval and use the derivatives in that point to advanceto the end of the interval:

k1 = f(yn, t) (C.3)

k2 = f(yn +1

2k1∆t, tn +

1

2∆t) (C.4)

yn+1 = yn + k2∆t + O(∆t3) (C.5)

This is called the midpoint method or second order Runge-Kutta method. It is alsoknown as the Euler-Heun method. The error term is now O(∆t3), but 2 functionevaluations are required instead of one for the Euler method.

One can go further and design other methods by using different approximationsof function f(y, t) (different high-order coefficients) and eliminate error terms by ajudicious combination of these approximations. The whole family of Runge-Kutta1

methods is based on this principle. One of the most widely used offspring of thefamily is the 4th-order Runge-Kutta method:

k1 = f(yn, tn) (C.6)

k2 = f(yn +k1

2∆t, tn +

1

2∆t) (C.7)

k3 = f(yn +k2

2∆t, tn +

1

2∆t) (C.8)

1At the age of 19, Carl Runge enrolled at the University of Munich to study literature. Afteronly 6 weeks of lectures, he changed to mathematics and physics and later specialized in differentialgeometry and numerical solutions of algebraic equations. He made important contribution to thestudy of the wavelengths of spectral lines of elements. Runge still used to perform handstandsto entertain his grandchildren when he turned 70, but he died a few months later from a heartattack. Martin Wilhelm Kutta was born in Pitschen (today’s Poland) but attended his academicstudies in Germany. The Runge-Kutta methods are based on his work, but he also became famousfor his work in aerodynamics.

152 APPENDIX C

k4 = f(yn + k3∆t, tn + ∆t) (C.9)

y(tn + ∆t) = yn + (1

6k1 +

1

3k2 +

1

3k3 +

1

6k4)∆t + O(∆t5) (C.10)

It is of order 4 and requires 4 function evaluations. An advantage of this specificsolver is that a high order accuracy can be achieved with a fairly easy implemen-tation. Note that a higher order doesn’t necessarily mean a superior method. It isonly the case if the price of computing function f(y, t) more often is fully compen-sated by the larger timesteps allowed to obtain a same accuracy.

Other popular methods are the Runge-Kutta-Fehlberg (Matlab’s ODE45) and theCash-Karp methods which are enhanced to enable fast time step adjustments (seesection C.5). It can be proved that up to two additional function evaluations areneeded to augment the order of a RK-method (12 function evaluations for a 8thorder method [Prince & Dormand, 1981]).

C.2 STIFF SYSTEMS

To define certain properties of differential equations or to characterize the stabilityof ODE-solvers, following set of linear differential equations is often used:

y = Λy (C.11)

For example, these equations are used to check the range of Λ and ∆t for which acertain numerical integration method is stable. This allows one to compare differentmethods and pick the most suitable one. When nonlinear equations are considered,it is much more difficult to quantify the stability regions. Therefore, one will relyon the results obtained from linear systems to choose the best ODE-solver.

The same story holds for the definition of the stiffness of a system. The linearsystem (C.11) is called stiff if a large dispersion is present in the values of Λ

[de Jalon & Bayo, 1994]. This can happen when the different components of amultibody system have masses and stiffness and damping coefficients with muchdifferent scales. It can also occur for large amounts of bodies, for constrainedmultibody systems or due to the specific formulation of the equations of motion.The latter is also referred to as formulation stiffness [Ascher et al., 1997].

Let’s now take a look at a simple example. Consider a particle moving in a plane,its position is described by coordinates (x, y). It is now assumed that the motionis constrained to a line (x-axis). This can be mathematically formulated as:

q =

(xy

)=

(f(x)−ky

)(C.12)


A large positive value is chosen for k to simulate the constraint and it is assumedthat the initial y-value is close to zero. The motion along the x-axis is not veryimportant. For the sake of the example however, it should be much slower thanthe motion along the y-axis. It is therefore chosen as x = −x. The particle willslowly (and exponentially) decay to the origin in the x-direction and will almostinstantly arrive at its destination in the y-direction. One can now try to calculatethe solution using an explicit method. Take Euler’s method (C.2) for the sake ofsimplicity. This results in:

(xn+1

yn+1

)=

((1 − ∆t)xn

(1 − k∆t)yn

)(C.13)

As one can readily see, the value of yn+1 will be higher than yn if |1 − k∆t| > 1.In other words: if k∆t > 2 or k∆t < 0 (which is physically impossible), the valueof y will keep on growing till infinity. Euler’s method is thus unstable for k∆t > 2.This means that the largest time step one can take is less than 2

k . The stiffer thesystem, the smaller the allowed time step and the slower the numerical integration.Although the particle will not move a lot in the y-direction, the simulation alongthe x-direction will be extremely difficult. A stiff system requires tiny time stepsfor stability reasons, while accuracy considerations may allow much larger timesteps. Other explicit methods, like the Runge-Kutta method of 4th order, canhandle bigger time steps, but nevertheless exhibit the same unstable behavior.

C.3 IMPLICIT METHODS

Explicit methods are not adequate for solving stiff systems, as very small time stepsmay be needed for retaining stability. The required time steps may even be so small,that round-off errors induce instability and the system cannot be simulated at all.Instead, one can use implicit methods. These calculate the new state coordinatesby means of an implicit function, which implies the use of iteration procedures.

C.3.1 BACKWARD EULER METHOD

The backward Euler method will be applied on the particle example used in sectionC.2 to show the principles and (dis)advantages of implicit methods. The explicitEuler scheme was given by

yn+1 = yn + f(yn, tn)∆t (C.14)

The implicit or backward Euler scheme is obtained by evaluating function f in thenew state:

yn+1 = yn + f(yn+1, tn+1)∆t (C.15)

154 APPENDIX C

For nonlinear systems, one generally needs an iteration process to find yn+1, theNewton-Raphson method usually works fine. Linear systems are nice, because thenew state can actually be computed explicitly. Let’s apply (C.15) on the particleexample (C.12):

(xn+1

yn+1

)=

(xn

yn

)+

(−xn+1

−kyn+1

)∆t (C.16)

⇒

(xn+1

yn+1

)=

( xn

1+∆tyn

1+k∆t

)(C.17)

The method is absolutely stable, even if the time step ∆t tends to infinity (iteven yields the correct answer). There are no stability restrictions on the timestep anymore for linear systems, but this property cannot be extended to genericnonlinear systems. In practice however, implicit methods allow for much largertime steps than explicit methods. The price one has to pay is to compute thesolution of a nonlinear set of algebraic equations. Note that taking larger timesteps still decreases the accuracy.

C.3.2 OTHER IMPLICIT METHODS

Details of other and more sophisticated implicit methods can be found in [Gear, 1971,Abramowitz & Stegun, 1972, Atkinson, 1978, Press et al., 1992]. Popular methodsare the implicit Runge-Kutta methods, the Rosenbrock’s methods and the back-ward differentiation formulas also known as Gear’s methods.

C.4 MULTISTEP METHODS

While regular explicit methods like Runge-Kutta schemes start from informationabout the actual state, multistep methods use the state history to predict the stateat the next interval. This allows one to obtain high order accuracy with fewerfunction evaluations (per time step). On the other hand, special care has to betaken to start the method and to cope with discontinuities.

Multistep methods are formally obtained by integrating the differential equationsy = f(y, y) from time tn−p (p is the number of previous steps) to tn+1:

yn+1 = yn−p +

n+1∫

n−p

f(y, t)dt (C.18)


By approximating function f with a polynomial depending on fi = f(yi, ti) (i =n − p, ..., n), the state can be advance to the next time step. The polynomial canalso depend on fn+1, resulting in an implicit method.

C.4.1 ADAMS-BASHFORTH/MOULTON METHODS

If the function f is approximated using a Newton backward difference polynomial,the resulting scheme is called an Adams-Bashforth method of order p (number ofprevious points), which is probably the most popular multistep method. A detailedderivation of the method is given in [Haug, 1989]. The 4th order Adams-Bashforthscheme takes following form:

yn+1 = yn + (55fn − 59fn−1 + 37fn−2 − 9fn−3)∆t

24(C.19)

The implicit variants are called Adams-Moulton schemes. Here is the 4th orderone:

yn+1 = yn + (9fn+1 + 19fn − 5fn−1 + fn−2)∆t

24(C.20)

The multistep methods are not self-starting : the values of fi for the previous timesteps are unknown at the start of the simulation. A solution is to commence theprocedure with the first order Adams-Bashforth scheme, use second order schemefor the next time step and so on, till the desired number of previous points areavailable. There is a caveat: the lower order schemes have lesser accuracy andintroduce errors which cannot be compensated when continuing with the high orderscheme. To achieve a given accuracy, the time steps need to be decreased at thestart of the simulation and can take their final size when the initialization phaseis completed. Adjusting the time step is cumbersome, as the method needs statecoordinates at equally spaced time intervals. Moreover, the method needs to berestarted each time a discontinuity occurs, which can be detrimental in simulationswith lots of impacts.

Both the Adams-Bashforth and Adams-Moulton schemes can be used in a predictor-corrector iteration. This method is widely spread and provides good accuracy atlow cost. First the Adams-Bashforth scheme is used to obtain a prediction y

pn+1

of the state at the next time step. Then the function fpn+1 = f(yp

n+1, tn+1) is com-puted. Subsequently the Adams-Moulton scheme is used to get a corrected valueyc

n+1. Finally f cn+1 is evaluated for use in the next iteration.

C.5 ERROR CONTROL

The error introduced at each time step is dependent on the step size, but alsoon the motion itself. Fast and strongly nonlinear movements are likely to induce

156 APPENDIX C

larger errors than, for example a steady motion. When using fixed steps, the overallaccuracy of the simulation is imposed by the largest error obtained for one step(the weakest link). There is no point in calculating a steady motion with highaccuracy (and thus tiny time steps) if an enormous error was introduced a fewmoments earlier. It is therefore interesting to adapt the step size to the scale ofthe local errors. Large time steps can be used for slow movements, while the stepsize can be decreased for the more difficult parts of the simulation. A tremendousperformance boost can be obtained when using adaptive time steps. There are twopopular methods for adjusting the step size: the use of high order schemes with anembedded lower order scheme and the step doubling method.

C.5.1 EMBEDDED METHODS

The Runge-Kutta-Fehlberg and the Cash-Karp methods for example, provide anestimation of the error for one more function evaluation. Both are 4th order Runge-Kutta methods embedded in a 5th order one. The 4th order scheme (using fivefunction evaluations) is used to get an approximation of the solution. The 5thorder approximation, which requires only one more function evaluation, is used toobtain an error estimation. Sometimes, the 5th order approximation is used assolution, it is indeed likely to produce even better results.

C.5.2 STEP DOUBLING METHOD

Other methods may use step doubling to get an estimation of the error. Thesimulation is run twice, once using a single step ∆t and a second time using twosteps ∆t

2 . Comparing both solutions gives an indication of the error. This methodis obviously more expensive than the previous one, but less expensive than it mayseem. For example, using a simple 4th order Runge-Kutta method, the overheadis a factor 1.375 [Press et al., 1992].

C.6 SOLVING DAEs

There are different ways of treating differential algebraic equations. One methodis to reduce the number of coordinates such as to express the equations of motionwith independent coordinates only, which results in a set of ordinary differentialequations. This can be done for open-loop systems for example, by relying onrelative coordinates. Sometimes, this is not an interesting solution, because theresulting equations typically display a strong nonlinear behavior or simply becausethe coordinate reduction is too complicated (closed-loop systems) or even not pos-sible.

One can also apply specialized numerical integration methods directly on theDAE. This can be done using backward difference formulas for example. There


are some stability issues coupled to the direct numerical integration of DAEs how-ever. In multibody dynamics, the equations of motion of a constrained system aretypically formulated as

Mq = Q − λT ∂Φ

∂q(C.21)

Φ(q, t) = 0 (C.22)

Such a set of equations is characterized by a so-called differential index, which is thenumber of times that the constraint equations need to be differentiated in order tobe able to eliminate the Lagrange multipliers, plus one. Above equations have index3. It is well-known that index 3 DAEs are difficult to solve by direct discretization[Gear & Petzold, 1981, Lodstedt & Petzold, 1986, Brenan et al., 1989] and robustmethods to solve DAEs with order higher than 2 are not known [Ascher et al., 1995,Ascher & Petzold, 1998].

One could instead prefer to express the constraints at velocity level and would endup with index 2. A further reduction of the index can be obtained by expressingthe constraints equations at acceleration level, resulting in following index 1 DAE:

Mq = Q − λT ∂Φ

∂q(C.23)

Φqq = −Φqq − Φt (C.24)

Note that λ can indeed be readily eliminated from these equations. The index1 version of the equations can be solved by using the standard ODE methods,but these methods become mildly unstable due to the fact that the constraintequations are introduced at acceleration level. If the acceleration constraints arenot completely fulfilled due to tiny numerical errors, a drift will occur, as the errorswill be integrated twice. This will result in a quickly growing constraint violationat position level. Stabilization approaches can be adopted to solve this problem,as will be discussed later.

C.6.1 DAE SOLVERS

Well-known direct solvers for differential algebraic equations are DASSL (Differential-Algebraic System Solver)[Petzold, 1982] and RADAU5, respectively based on back-ward difference formulae and implicit Runge-Kutta methods. A short example ofthe Euler backward difference method (1st order) is given below to demonstratethe general approach. Consider following DAE, given in implicit form:

F(y,y, t) = 0 (C.25)

158 APPENDIX C

The Euler backward difference approximation of y is

y ≃yn+1 − yn

∆t(C.26)

Substitution in (C.25) results in following discretized form of the DAE

F(yn+1 − yn

∆t,yn, tn) = 0 (C.27)

These equations can be solved for yn+1 using a Newton-Raphson iteration proce-dure. An analytical expression for the Jacobian is strongly recommended.

C.7 CONSTRAINT VIOLATION ERRORS

Constraint equations are generally nonlinear algebraic equations. Although theymay be directly embedded in the problem by using integrators for DAEs, it is oftenpreferable to differentiate them once or twice to reduce the differential index.

Φqq = −Φt (C.28)

Methods involving accelerations may even need the second derivative:

Φqq = −Φqq − Φt (C.29)

While the use of equations (C.28) and (C.29) is equivalent to the original algebraicequations, they may introduce numerical errors during integration. Indeed, a smallroundoff error may result in a small motion of a point which should not move atall. As the simulation progresses, the point will diverge from its initial positionand eventually result in an unrealistic motion. The problem is even more stringentwhen the equations are inserted at acceleration level. The violation error will thengrow quadratically (at worst), leading even faster to an unacceptable behavior.Fortunately, there exist methods to repress these constraint violation errors. Apopular solution is the coordinate partitioning method [Haug, 1989]. Another setof solutions are the so-called penalty formulations which tolerate small errors. Athird set of solutions using a differential geometric approach are the projectivemethods.

C.7.1 COORDINATE PARTITIONING METHOD

This method consists in choosing a set of independent coordinates and in only inte-grating these coordinates. When the independent coordinates of the new state are


obtained, the dependent coordinates are calculated using the constraint equations.Consider following DAE:

y = f(y, t) (C.30)

Φ(y, t) = 0 (C.31)

Differentiating the constraint equations once yields

Φyy = Φt (C.32)

y can now be partitioned in a set of independent coordinates yi and a set of depen-dent coordinates yd. The derivative of the dependent coordinates takes followingform:

yd = −Φ−1yd

(Φyiyi + Φt) (C.33)

The partition is valid if the Jacobian Φydis regular. There exist different methods

to make sure this is the case [Haug, 1989]. When the partition is made, yi isintegrated to obtain the independent coordinates at the next time step. yd can thenbe computed using (C.33) if desired. The dependent coordinates yd are obtainedby performing a Newton-Raphson iteration on the constraint equations (C.31).

C.7.2 PENALTY FORMULATIONS

Instead of completely eliminating the constraint errors, one can choose to stabilizethe equations of motion. Constraint errors are allowed, but some penalty termsare added to make sure these errors remain bounded. The Baumgarte stabilizationmethod is a popular one for its simplicity. It is based on the observation that, whilethe numerical integration of Φ = 0 can become unstable, the modified equation

Φ + 2αΦ + β2Φ = 0 (C.34)

is always stable (for α > 0 and β 6= 0)[Baumgarte, 1972, Baumgarte, 1983]. Usingthese modified constraint equations and performing a direct integration of the statecoordinates yields a stable and fast simulation. Unfortunately, there is no generalmethod for selecting the parameters α and β. Moreover, problems may be encoun-tered in the vicinity of singular configurations.

In [Bayo et al., 1988], a different penalty formulation based on the augmentedLagrangian approach is proposed. In chapter B, the augmented Lagrangian wasintroduced as

L∗ = L + λT Φ (C.35)

160 APPENDIX C

The second term in the rhs can be replaced by penalty terms consisting of fictitiouskinetic energy and a fictitious potential energy:

L∗ = L +1

2Φ

TαΦ −

1

2ΦT αβ2Φ (C.36)

Additionally a dissipative generalized force Qp = −2αβµΦ can be added. Theresulting equations of motion take the form

Mq + ΦTqα(Φ + 2βµΦ + β2Φ) = Q (C.37)

The convergence of these equations is assured for a wide range of penalty values.Moreover, the method behaves very well in singular positions[Bayo & Avello, 1994].

C.7.3 PROJECTIVE METHODS

Gradient projective methods are based on the concept of manifolds [Arnold, 1978,Jose & Saletan, 2001]. It is not the purpose of this work to expose these methodsin detail, interested readers may find more information in the specialized litera-ture[Blajer, 2002, Terze et al., 2001]. Nice introductions to tensor calculus may befound in [Simmonds, 1994, Papastavridis, 1999]

Figure C.1: Gradient projective methods

Projective methods are related to the coordinate partitioning method, inasmuchthat a partition in independent and dependent coordinates is performed. The dif-ference resides in the fact that all derivatives of the coordinates are integrated, notjust the independent ones, and that the constraint corrections are performed after-wards. Consider figure C.7.3 which represents the basic idea behind the gradient


projective method [Terze et al., 2003]. The configuration manifold is representedby the two-dimensional plane. The motion is constrained to a curve C that rep-resents the constraint manifold. The configuration of the system is given by twodependent coordinates q1 and q2. Presume the initial configuration is in 1. Afterintegration, a new (incorrect) configuration 3 is obtained, while the correct resultshould be 2. To be consistent with the constraint, point 3 should be projected ontothe constraint manifold C. In the gradient projective method, this happens bychoosing one coordinate as independent and by correcting the second dependentcoordinate till the constraint is fulfilled, by means of the constraint equations. Sayq2 is chosen as independent coordinate. Correcting q1 (moving along the q1-curve)leads to a new configuration 4. Conversely, choosing q1 as independent leads toconfiguration 5. The former is clearly the better choice. The dependent coordinateshould be the one that leads as directly as possible to the constraint manifold.

C.8 CONSIDERATIONS FOR REALTIME SIM-ULATIONS

Realtime simulations need a processing time which is smaller than the respectivesimulation time. In other words, the time required to process a new timestep hasto be smaller than the step size. Therefore, the choice of numerical integrators islimited. Fixed step, explicit methods require a fixed processing time and may bevalid choices, as far as they do not become unstable. Variable step methods arenot a sensible choice, as the processing time may become unpredictable. Multi-steps method may be efficient, but can cause severe problems for simulations wherediscontinuities occur in the system variables. This happens in situations such asimpacts, sudden appearances of constraints or external forces, Coulomb friction,stiction... Implicit methods do not seem to be a convenient choice, as the numberof required iterations is not predictable. One may however find out experimen-tally that a certain amount of iterations is sufficient for the task. In that case,one can benefit from the good stability properties. In practice, the implicit mid-point and implicit Runge-Kutta methods are a good choice for realtime simulations[de Jalon & Bayo, 1994, Dopico et al., 2005].

C.9 CONCLUSION

In this section, an overview of different numerical approaches to solve nonlineardifferential equations has been given. The explicit methods, in particular the highorder Runge-Kutta methods, proved to provide high accuracy at a moderate cost.Unfortunately, they do not guarantee the stability of the simulation and they donot perform well on stiff systems. Implicit methods on the other hand have goodstability properties and some specific schemes are very efficient on stiff systems. The

162 APPENDIX C

solution of a set of nonlinear equations is needed however, and implicit methods areonly interesting if that computational burden is overcompensated by the larger timesteps the method can take. In case of mixed differential-algebraic equations, somestability issues are encountered. The index of DAEs may be reduced, but certainprecautions have to be taken to eliminate or stabilize the constraint violation errors.The coordinate partitioning method, penalty methods and projective methods havebeen briefly discussed.

Appendix D

ADDITIONAL CALCULATIONS

“Proof is the idol before whom the pure mathematician tortures himself.”-Sir Arthur Eddington

In this appendix, one can find all lengthy or tedious calculations that were skippedin the main text for the sake of clarity.

D.1 CALCULUS OF VARIATIONS

A fundamental problem in the calculus of variations [Courant & Hilbert, 1953,Pars, 1962] aims at finding the path for which a given function reaches an ex-tremum, it involves finding a stationary value for the functional

I =

x2∫

x1

f(y, y, x)dx (D.1)

It can also be expressed as

δI = δ

x2∫

x1

f(y, y, x)dx = 0 (D.2)

I has an extremum if the Euler-Lagrange variational equations are satisfied:

d

dx(∂f

∂y) −

∂f

∂y= 0 (D.3)

If the variables x and y are subject to boundary conditions (constraints) Φ(x,y) =0, one can construct a new functional I∗ using the technique of Lagrange multipli-ers:

163

164 APPENDIX D

δI∗ = I + ΦT λ (D.4)

λ are the Lagrange multipliers. This results in a new set of Euler-Lagrange equa-tions, which can be solved together with the constraint equations.

D.2 BASIC VECTOR FORMULAS

A few basic formulas are given below. The proofs are quite easy and are notdiscussed.

D.2.1 PROPERTIES OF SKEW-SYMMETRIC MATRICES

ab = baT − aT bI (D.5)

˜ab = baT − abT = ab − ba (D.6)

D.2.2 DOUBLE VECTOR PRODUCTS

EXPULSION FORMULA

a × (b × c) = (a c)b − (ab)c (D.7)

This formula is used to obtain the inertia tensor, it does appear in a slightly differentform in the main text:

a × (b × a) = (aT aI − aaT )b (D.8)

CYCLIC PROPERTY

This property follows from the expulsion formula:

a × (b × c) + c × (a × b) + b × (c × a) = 0 (D.9)

It appears in the derivation of the algorithm under the following alternative shape:

a b c = −c ab − b ca (D.10)

ADDITIONAL CALCULATIONS 165

D.3 DERIVATION IN MOVING REFERENCEFRAMES

When talking about the derivative of a vector, it is important to specify whichreference frame the derivation is taken to. Consider a rigid body, a body-fixedreference frame K and an inertial reference frame O for instance. The derivationof the position of the center of mass G with respect to the local frame will be zero.The derivation of position G with respect to the inertial frame will generally bedifferent from zero.

The relationship between the time derivatives with respect to different referenceframes is:

d0x

dt=

dKx

dt+ ωrK

× x (D.11)

with ωrKthe relative angular velocity between frames K and O. For a spatial

vector X, following equation holds:

d0X

dt=

dKX

dt+

(ωrK

× 0

0 ωrK×

)X (D.12)

Note that the relative translation of the reference frames has no influence, thismeans that the notation Ω× cannot be used.

D.4 DERIVATION OF VECTORS AND MATRI-CES

D.4.1 DERIVATION OF TRANSFORMATION OPERA-TORS

Two transformation operators are used throughout the algorithm, one for forceand momentum transport and one for calculating the velocity distributions. Thematrix notation of the former operator is:

KT FL =

(I 0

OKOL× I

)(D.13)

Position vector OKOL is dependent on the coordinates that describe the trans-lation between both reference frames. It is however independent of the relativeorientation. One can describe the corresponding time derivative using the relative

linear velocity vrL= dK

dt (OKOL). This results in

166 APPENDIX D

dK

dt(KT F

L ) = KT FL =

(0 0

vrL× 0

)= KT F

L

(0 0

vrL× 0

)(D.14)

The force shift dyadic is added in the last step to keep the symmetry in the equa-tions. The force and velocity transformations are related to each other:

KT FL = (LT V

K )T (D.15)

The time derivative of the velocity transformation is therefore:

dK

dt(LT V

K ) = (KT FL )T =

(0 −vrL

×0 0

)LT V

K (D.16)

Be careful with the indices and the dot-symbol. The latter was defined as the

time-derivative in the local axes: LT VK = dL

dt (LT VK ).

D.4.2 DERIVATION OF TRANSFORMED QUANTITIES

The articulated momentum vector of a multibody system is defined as

P∗

K = PK +∑

i

KT Fi Pi (D.17)

If one wishes to calculate the time derivative of the articulated momentum vector,one needs to derive the reduced quantity KT F

L PL with respect to reference frameK. Using (D.14) and (D.11):

dK

dt(KT F

L PL) = KT FL

(0 0

vrL× 0

)PL + KT F

L [PL +

(ωrL

× 0

0 ωrL×

)PL]

= KT FL [PL +

(ωrL

× 0

vrL× ωrL

×

)PL]

= KT FL (PL + ΩrL

× PL) (D.18)

with vr, ωr and Ωr respectively the relative linear, angular and spatial velocities.Analogously, the time derivative of a transformed velocity can be required. Toobtain joint accelerations for examples, the time derivative of the spatial velocityvectors need to be calculated. ΩK can be obtained from (K = Pa(L)):

ΩL = LT VK ΩK + ΩrL

(D.19)


The time derivative of the first term of the rhs with respect to reference frame Kis

dL

dt(LT V

K ΩK) =dK

dt(LT V

K ΩK) −

(ωrL

× 0

0 ωrL×

)LT V

K ΩK

= LT VK ΩK +

(0 −vrL

×0 0

)LT V

K ΩK −

(ωrL

× 0

0 ωrL×

)LT V

K ΩK

= LT VK ΩK + ΩT

rL× LT V

K ΩK (D.20)

Hence, the expression for ΩK is

ΩL = LT VK ΩK + ΩT

rL× LT V

K ΩK + ΩrL(D.21)

The vector product of the transposed of a spatial vector is defined as:

ΩT× = (Ω×)T =

(ω× v×0 ω×

)(D.22)

D.4.3 EQUATIONS OF MOTION OF (CONSTRAINED)ARTICULATED BODY

During the derivation of the equations of motion for open-loop and closed-loopmultibody systems, the combination of several bodies into one articulated body isencountered. It is interesting to take a close look at the following expression:

(PK + ΩK × PK − TK) + KT FL ZL(PL + ΩL × PL − TL) (D.23)

K and L do not need to be adjacent. This form appears several times in the thesis.Z can be the unity dyadic I for open-loop systems, or C∗

qET for dependent bodies

in closed-loop systems. Define

PzK = PK + KT F

L ZLPL (D.24)

By means of (D.18), the time derivative of this spatial vector becomes

PzK = PK + KT F

L ZLPL + KT FL ZLPL + KT F

L (ΩrL× ZLPL) (D.25)

ΩrLis the relative motion between reference frames K and L. Expression (D.23)

can now be transformed into

PzK + ΩK × Pz

K − TK − KT FL ZLTL − KT F

L (ZL + ΩrL× ZL)PL

168 APPENDIX D

− ΩK × KT FL ZLPL + KT F

L ZL(ΩL × PL)

= PzK + ΩK × Pz

K − TzK (D.26)

with1

TzK = TK + KT F

L ZLTL + KT FL [ZL + (ΩL × I)ZL − ZL(ΩL × I)]PL (D.27)

If ZL = I, the spatial force vector TzK reduces to

TzK = TK + KT F

L TL (D.28)

Above expressions can easily be extended to combinations of more than two bodies.

D.5 REDUCING THE CONSTRAINTEQUATIONS

The derivation of the algorithm to establish the equations motion of constrainedmultibody systems using coordinate reduction requires the reduction of the con-straint equations to all the system’s dependent bodies. The expression is given forsystems of the type illustrated on fig. 5.2, i.e. a chain with both ends attached tothe fixed environment. The type of joints is arbitrary, N +1 denotes the constraintjoint. The different forms of the constraint equations are given by

CK(KT VK−1ΩK−1 +

∑

i∈iDes(K−1)

KT Vi Eiqi) = 0 (D.29)

with

CK = KT FK+1CK+1

K+1T VK CK (D.30)

CK = I ∀K ∈ Ind (D.31)

CK = I + EKCTqK

∀K ∈ Dep (D.32)

CTqK

= −(ETK

KT FK+1CK+1

K+1T VK EK)−1ET

KKT F

K+1CK+1K+1T V

K (D.33)

1Use following identity:

KT FL (ΩL × ZLPL) = KT F

L (ΩrL× ZLPL) + KT F

L (LT VK ΩK × ZLPL)

= KT FL (ΩrL

× ZLPL) + ΩK × KT FL ZLPL

The last step is not trivial, but it can be easily proved by comparing the components explicitly.


The constraint joint N + 1 is always chosen as dependent, following boundaryequations can therefore be used:

CN+1N+1T V

N ΩN = 0 (D.34)

with

CN+1 = CN+1 = I − (ETN+1EN+1)

−1ETN+1

N+1T VN ΩN (D.35)

The generic relationships can be shown to be valid using an induction method.Presume the equations hold for body K. It can then be shown that they hold forthe parent body K − 1 as well. Indeed, decomposing the spatial velocity gives:

CK [KT VK−1(

K−1T VK−2ΩK−2 + EK−1qK−1) +

∑

i∈iDes(K−1)

KT Vi Eiqi] = 0 (D.36)

In case K − 1 is an independent body, above equation can be reformulated byshifting the second term into the summation and premultiplying by K−1T F

K .

CK−1(K−1T V

K−2ΩK−2 +∑

i∈iDes(K−2)

K−1T Vi Eiqi) = 0 (D.37)

with

CK−1 = K−1T FK CK

KT VK−1 (D.38)

In case K − 1 is a dependent body, its joint velocities can be calculated explicitlyby premultiplying equations (D.36) by ET

K−1K−1T F

K :

qK−1 = −(ETK−1

K−1T FK CK

KT VK−1EK−1)

−1[ETK−1

K−1T FK CK(KT V

K−2ΩK−2

+∑

i∈iDes(K−1)

KT Vi Eiqi)]

= CTqK−1

(K−1TK−2ΩK−2 +∑

i∈iDes(K−1)

K−1T Vi Eiqi) (D.39)

with

CTqK−1

= −(ETK−1

K−1T FK CK

KT VK−1EK−1)

−1ETK−1

K−1T FK CK

KT VK−1 (D.40)

170 APPENDIX D

Substitution in (D.36) and premultiplying by K−1T FK results in

K−1T FK CK

KT VK−1(I + EK−1C

TqK−1

)(K−1T VK−2ΩK−2 +

∑

i∈iDes(K−2)

K−1T Vi Eiqi)

=CK−1(K−1T V

K−2ΩK−2 +∑

i∈iDes(K−2)

K−1T Vi Eiqi) = 0 (D.41)

with

CK−1 = K−1T FK CK

KT VK−1CK−1 (D.42)

CK−1 = I + EK−1CTqK−1

(D.43)

These are exactly the same expressions as for body K and this completes the proofby induction.

D.6 PARTIAL DERIVATIVES OF THE SPATIALVELOCITIES

The calculation of the canonical momenta of constrained multibody systems usingminimal coordinates requires the partial derivative of the spatial velocities withrespect to the independent joint velocities. The same type of structure is consideredas in the previous section. There are two different expressions for these partialderivatives, depending on the relative position of the two considered bodies. Foran independent body K and a body i situated outboard of body K (it also holdsfor i = K), the expression is

∂Ωi

∂qK= iT V

K EK +∑

d∈dAsc(i)

iT Vd EdC

Tqd

dT VK EK (D.44)

with

CTqd

= CTqd

(I +∑

j∈dAsc(d)

dT Vj EjC

Tqj

jT Vd ) (D.45)

dAsc(K) stands for the set of all dependent ascendants of body d. Equation (D.45)gives a recursive relationship between the dependent bodies only. Asc(K) standsfor the set of all ascendants of body K, including K body itself. dAsc(K) is the


set of dependent bodies in Asc(K). If body i is located inboard of body K, theexpression is slightly different:

∂Ωi

∂qK=

∑

d∈dAsc(i)

iT Vd EdC

Tqd

dT VK EK (D.46)

The expressions (D.44) and (D.46) can be proved by induction. First, it will beshown that (D.46) holds. To do this, a boundary condition is needed. Considerbody D1, which is the first dependent body encountered when starting from thebase body and travelling in the outboard direction. Assume K is outboard of D1.Using equation (D.39), one obtains

∂ΩD1

∂qK= D1T V

D1−1

∂ΩD1−1

∂qK+ ED1

∂qD1

∂qK(D.47)

= 0 + ED1CT

qD1

∂

∂qK(

∑

i∈iDes(D1)

D1T Vi Eiqi) (D.48)

= ED1CT

qD1

D1T VK EK (D.49)

Above expression is of the form (D.46) for the boundary condition CqD1= CqD1

.The zero in (D.48) comes from the fact that all bodies inboard of D1 (thus D1 − 1as well) are independent. Note that K is indeed an element of iDes(D1). Considernow another body situated between K and D1. There are two possibilities: thatbody is either independent or dependent. In the case it is independent, call it I2,otherwise call it D2. Assume equations (D.46) hold for respectively I2−1 or D2−1.For the partial derivative of the spatial velocity ΩI2

, one gets

∂ΩI2

∂qK= I2T V

I2−1

∂ΩI2−1

∂qK+ EI2

∂qI2

∂qK(D.50)

= I2T VI2−1

∑

d∈dAsc(I2−1)

I2−1T Vd EdC

Tqd

dT VK EK + 0 (D.51)

=∑

d∈dAsc(I2)

I2T Vd EdC

Tqd

dT VK EK (D.52)

One recognizes the form of equations (D.46). Now, if the spatial velocity of depen-dent body D2 is considered, the expression becomes (using (D.39)):

∂ΩD2

∂qK= D2T V

D2−1

∂ΩD2−1

∂qK+ ED2

∂qD2

∂qK

172 APPENDIX D

= D2T VD2−1

∑

d∈dAsc(D2−1)

D2−1T Vd EdC

Tqd

dT VK EK

+ ED2CT

qD2

∑

d∈dAsc(D2−1)

(D2−1T Vd EdC

Tqd

dT VK EK)

+ ED2CT

qD2

∂

∂qK(

∑

i∈iDes(D2)

D2T Vi Eiqi)

=∑

d∈dAsc(D2)

D2T Vd EdC

Tqd

dT VK EK (D.53)

with

CTqD2

= CTqD2

(I +∑

d∈dAsc(D2)

D2T Vd EdC

Tqd

dT VD2

) (D.54)

All possibilities have now been considered for a spatial velocity inboard of K andthe proof by induction of expression (D.46) is completed. For spatial velocitiesoutboard of K, a new boundary condition is required. Consider therefore thespatial velocity of body K:

∂ΩK

∂qK= KT V

K−1

∂ΩK−1

∂qK+ EK

∂qK

∂qK(D.55)

= KT VK−1

∑

d∈dAsc(K−1)

K−1T Vd EdC

Tqd

dT VK EK + EK (D.56)

= EK +∑

d∈dAsc(K)

KT Vd EdC

Tqd

dT VK EK (D.57)

Take now any outboard body of K and assume expression (D.44) is true. If thebody is independent (I3), one gets:

∂ΩI3

∂qK= I3T V

I3−1

∂ΩI3−1

∂qK+ EI3

∂qI3

∂qK

= I3T VI3−1[

I3−1T VK EK +

∑

d∈dAsc(I3−1)

I3−1T Vd EdC

Tqd

dT VK EK ] + 0

= I3T VK EK +

∑

d∈dAsc(I3)

I3T Vd EdC

Tqd

dT VK EK (D.58)


This has indeed the other form (D.44). If the body outboard of K is dependent(D3), one finally obtains:

∂ΩD3

∂qK= D3T V

D3−1

∂ΩD3−1

∂qK+ ED3

∂qD3

∂qK

= D3T VK EK + (I + ED3

CTqD3

)∑

d∈dAsc(D3−1)

D3T Vd EdC

Tqd

dT VK EK

+ ED3CT

qD3

D3T VK EK

= D3T VK EK +

∑

d∈dAsc(D3)

D3T Vd EdC

Tqd

dT VK EK (D.59)

with

CTqD3

= CTqD3

(I +∑

d∈dAsc(D3)

D3T Vd EdC

Tqd

dT VD3

) (D.60)

Again, this is of the from (D.44) and the proof by induction is completed.

D.7 CONSTRAINED MOMENTUM VECTOR

To calculate the canonical momenta of a constrained multibody system using min-imal coordinates, following expression needs to be developed:

∑

i∈MBS

∂ΩTi

∂qKBi (D.61)

Replacing Bi by Pi provides the canonical momenta. Bi can however be replacedby Pi + Ωi ×Pi −Ti as well, this is done in the principle of virtual power. Usingthe results from the previous section, the expression transforms into (mind thetransposition)

∑

i∈MBS

∂ΩTi

∂qKBi =

∑

i∈MBS

[(ETK

KT Fi +

∑

d∈Dep

ETK

KT Vd Cqd

ETd

dT Fi )Bi]

= ETK [B∗

K +∑

d∈Dep

KT Vd Cqd

ETd B∗

d] (D.62)

174 APPENDIX D

Bibliography

[Abramowitz & Stegun, 1972] Abramowitz, M. & Stegun, I. A. (1972). Handbookof Mathematical Functions with Formulas, Graphs and Mathematical Tables.Dover, New York.

[AEGIA, 2005] AEGIA (2005). http://www.ageia.com/technology.html.

[Anderson & Critchley, 2003] Anderson, K. S. & Critchley, J. H. (2003). Order-N Performance Algorithm for the Simulation of Constrained Multi-Rigid-BodyDynamic Systems. Multibody System Dynamics, 9, 185–212.

[Anitescu & Potra, 1997] Anitescu, M. & Potra, F. (1997). Formulating dynamicmulti-rigidbody contact problems with friction as solvable linear complementar-ity problems. Nonlinear Dynamics, 14, 231–247.

[Appell, 1899] Appell, P. (1899). Sur une Forme Generale des Equations de laDynamique. C.R. Academie des Sciences de Paris, 129, 459460.

[Armstrong & Green, 1985] Armstrong, W. W. & Green, M. W. (1985). The dy-namics of articulated rigid bodies for purposes of animation. Proceedings ofGraphics Interface 1985, 1, 407–415.

[Arnold, 1978] Arnold, V. I. (1978). Mathematical Methods of Classical Mechanics.Springer-Verlag.

[Ascher et al., 1995] Ascher, U., Chin, H., Petzold, L., & Reich, S. (1995). Stabi-lization of constrained mechanical systems with daes and invariant manifolds. J.Mech. Struct. Machines, 23, 135–158.

[Ascher et al., 1997] Ascher, U., Pai, D., & Cloutier, B. (1997). Forward dynam-ics, elimination methods, and formulation stiffness in robot simulation. Int. J.Robotics Res., 16, 749–758.

175

176 APPENDIX D

[Ascher & Petzold, 1998] Ascher, U. M. & Petzold, L. R. (1998). Computer Meth-ods for Ordinary Differential Equations and Differential-Algebraic Equations.SIAM.

[Atkinson, 1978] Atkinson, K. E. (1978). An Introduction to Numerical Analysis.John Wiley and sons.

[Bae et al., 1987] Bae, D. S., , & Haug, E. J. (1987). A recursive formulation forconstrained mechanical system dynamics: Part II. closed-loop systems. Mechan-ics of Structures and Machines, 15(4), 481–506.

[Bae & Haug, 1987] Bae, D. S. & Haug, E. J. (1987). A Recursive Formulationfor Constrained Mechanical System Dynamics: Part I. Open Loop Systems.Mechanics of Structures and Machines, 15(3), 359–382.

[Bae et al., 1988] Bae, D. S., Kuhl, J. G., & Haug, E. J. (1988). A RecursiveFormulation for Constrained Mechanical System Dynamics: Part III. ParallelProcessor Implementation. Mechanics of Structures and Machines, 16(2), 249–269.

[Baraff, 1994] Baraff, D. (1994). Fast contact force computation for nonpenetratingrigid bodies. Proceedings of SIGGRAPH 1994, 28, 23–34.

[Baraff, 1996] Baraff, D. (1996). Linear-Time Dynamics using Lagrange multipli-ers. Computer Graphics Proceedings, Annual Conference Series, (pp. 137–146).

[Baraff et al., 2003] Baraff, D., Witkin, A., & Kass, M. (2003). Untangling cloth.ACM Transactions on Graphics, Proceedings of ACM SIGGRAPH 2003, 22(3),862–870.

[Barzel, 1997] Barzel, R. (1997). Faking dynamics of ropes and springs. IEEEComputer Graphics and Applications, 17(3), 31–39.

[Barzel et al., 1996] Barzel, R., Hugues, J. F., & Wood, D. N. (1996). Plausiblemotion simulation for computer graphics animation. Proceedings of the Euro-graphics Workshop on Computer Animation and Simulation, (pp. 183–197).

[Baumgarte, 1972] Baumgarte, J. W. (1972). Stabilization of constraints and inte-grals of motion in dynamical systems. Computer Methods in Applied Mechanicsand Engineering, 1, 1–16.

[Baumgarte, 1983] Baumgarte, J. W. (1983). A New Method of Stabilization forHolonomic Constraints. ASME Journal of Applied Mechanics, 50, 869–870.

[Bayo & Avello, 1994] Bayo, E. & Avello, A. (1994). Singularity-Free AugmentedLagrangian Algorithms for Constrained Multibody Dynamics. Nonlinear Dy-namics, 5, 209–231.

BIBLIOGRAPHY 177

[Bayo et al., 1988] Bayo, E., de Jalon, J. G., & Serna, M. A. (1988). A modifiedlagrangian formulation for the dynamic analysis of constrained mechanical sys-tems. Computer Methods in Applied Mechanics and Engineering, 71, 183–195.

[Blajer, 2002] Blajer, W. (2002). Elimination of Constraint Violation and Accu-racy Aspects in Numerical Simulation of Multibody Systems. Multibody SystemDynamics, 7, 265–284.

[Brenan et al., 1989] Brenan, K. E., Campbell, S. L., & Petzold, L. R. (1989). TheNumerical Solution of Initial Value Problems in Differential-Algebraic Equations.Elsevier Science Publishing.

[Chenney & Forsyth, 2000] Chenney, S. & Forsyth, D. A. (2000). Sampling plau-sible solutions to multi-body constraint problems. SIGGRAPH 2000 ConferenceProceedings, (pp. 219–228).

[Clifford, 1878] Clifford, W. (1878). Applications of grassmann’s extensive algebra.American Journal of Mathematics Pure an Applied, 1, 350–358.

[Courant & Hilbert, 1953] Courant, R. & Hilbert, D. (1953). Methods of Mathe-matical Physics: Volume 1. New York: Interscience Publishers.

[Coutinho, 2001] Coutinho, M. G. (2001). Dynamic Simulations of Multibody Sys-tems. New York: Springer-Verlag.

[Craig, 1989] Craig, J. J. (1989). Introduction to Robotics: Mechanics and Control(2nd ed.). Boston: Addison-Wesley Longman Publishing Co.

[Critchley & Anderson, 2003] Critchley, J. H. & Anderson, K. S. (2003). A gen-eralized recursive coordinate reduction method for multibody dynamic systems.Journal of Multiscale Computational Engineering, 1(2), 181–200.

[Critchley & Anderson, 2004] Critchley, J. H. & Anderson, K. S. (2004). A parallellogarithmic order algorithm for general multibody system dynamics. MultibodySystem Dynamics, 12(1), 75–93.

[de Jalon & Bayo, 1994] de Jalon, J. G. & Bayo, E. (1994). Kinematic and Dy-namic Simulation of Multibody Systems: The Real-Time Challenge. New York:Springer-Verlag.

[de Maupertuis, 1756] de Maupertuis, P. L. M. (1756). Oeuvres de M. de Mauper-tuis Tome IV. Lyon.

[Denavit & Hartenberg, 1955] Denavit, J. & Hartenberg, R. S. (1955). A kine-matic notation for lower-pair mechanisms based on matrices. Journal of AppliedMechanics, (pp. 215–221).

178 APPENDIX D

[Dopico et al., 2005] Dopico, D., Lugrs, U., Gonzalez, M., & Cuadrado, J. (2005).On the aptitude of irk integrators for the real-time dynamics of multibodysystems. International Symposium on Multibody Systems and Mechatronics,MuSMe2005, 6.

[Eddington, 1967] Eddington, A. (1967). The Nature of the Physical World. AnnArbor: University of Michigan Press.

[Featherstone, 1987] Featherstone, R. (1987). Robot Dynamics Algorithms. KluwerAcademic Publishers.

[Featherstone, 1999] Featherstone, R. (1999). A divide-and-conquer articulated-body algorithm for parallel o(log(n)) calculation of rigid-body dynamics. part 1:Basic algorithm. International Journal of Robotics Research, 18(9), 867–875.

[Featherstone, 2001] Featherstone, R. (2001). The acceleration vector of a rigidbody. International Journal of Robotics Research, 20(11), 841–846.

[Gear, 1971] Gear, C. W. (1971). Numerical Initial Value Problems in OrdinaryDifferential Equations. Englewood Cliffs,N.J.: Prentice Hall.

[Gear & Petzold, 1981] Gear, C. W. & Petzold, L. R. (1981). Ode methods for thesolution of differential/algebraic systems. Journal of Computational and AppliedMathematics, 7, 67–75.

[Goldstein, 2002] Goldstein, H. (2002). Classical Mechanics (3rd edition). Addison-Wesley Publishing Company.

[Hamilton, 1834] Hamilton, W. R. (1834). On the application to dynamics of ageneral mathematical method previously applied to optics. British AssociationReport, (pp. 513–518).

[Hamilton, 1850] Hamilton, W. R. (1850). On Quaternions and the Rotation of aSolid Body. Proceedings of the Royal Irish Academy, 4, 38–56.

[Haug, 1989] Haug, E. J. (1989). Computer-Aided Kinematics and Dynamics ofMechanical Systems. Volume I: Basic Methods. Allyn and Bacon.

[James & Pai, 2004] James, D. L. & Pai, D. K. (2004). Bd-tree: Output-sensitivecollision detection for reduced deformable models. ACM Transactions on Graph-ics (SIGGRAPH 2004), 23(3).

[Jerkovsky, 1978] Jerkovsky, W. (1978). The structure of multibody dynamicsequations. Journal of Guidance and Control, 1(3), 173–182.

[Jimenez et al., 2001] Jimenez, P., Thomas, F., & Torras, C. (2001). 3d collisiondetection: a survey. Computers and Graphics, 25(2), 269–285.

BIBLIOGRAPHY 179

[Jose & Saletan, 2001] Jose, J. & Saletan, E. J. (2001). Classical Dynamics: aContemporary Approach. Cambridge University Press.

[Kane & Levinson, 1985] Kane, T. R. & Levinson, D. A. (1985). Dynamics: Theoryand Applications. McGraw-Hill.

[Lagrange, 1853] Lagrange, J. L. (1853). Mecanique Analytique. Paris: MalletBachelier.

[Lankarani & Nikravesh, 1988] Lankarani, H. M. & Nikravesh, P. E. (1988). Appli-cation of the Canonical Equations of Motion in Problems of Constrained Multi-body Systems with Intermittent Motion. Advances in Design Automation, 1,417–423.

[Lankarani & Nikravesh, 1994] Lankarani, H. M. & Nikravesh, P. E. (1994). Con-tinuous contact force models for impact analysis in multibody systems. Interna-tional Journal of Nonlinear Dynamics, 5, 193–207.

[Laszlo et al., 2000] Laszlo, J., van de Panne, M., & Fiume, E. (2000). Interactivecontrol for physically-based animation. Proceedings of the 27th Annual Confer-ence on Computer Graphics and interactive techniques, (pp. 201–208).

[Lilly & Orin, 1994] Lilly, K. W. & Orin, D. E. (1994). Efficient dynamic sim-ulation of multiple chain robotic mechanisms. Journal of Dynamic Systems,Measurement and Control, 116, 223–231.

[LMSnews, 2003] LMSnews (2003). Engineered to Carry 320 Tons. EmpoweringEngineering Innovation, 5, 27–29.

[Lodstedt & Petzold, 1986] Lodstedt, P. & Petzold, L. (1986). Numerical Solutionof Nonlinear Differential Equations with Algebraic Contraints I: ConvergenceResults for Backward Differentiation Formulas. Mathematics of Computation,46(174), 491–516.

[Lotstedt, 1982] Lotstedt, P. (1982). Mechanical systems of rigid bodies subject tounilateral constraints. SIAM Journal on Applied Mathematics, 42(2), 281–296.

[Mitiguy & Kane, 1996] Mitiguy, P. C. & Kane, T. R. (1996). Motion variablesleading to efficient equations of motion. International Journal of Robotics Re-search, 15(5), 522–532.

[Naudet & Lefeber, 2005] Naudet, J. & Lefeber, D. (2005). General Formulationof an Efficient Recursive Algorithm based on Canonical Momenta for ForwardDynamics of Closed-Loop Multibody Systems. Proceedings of the XI DINAME(to be published).

[Naudet et al., 2003] Naudet, J., Lefeber, D., & Terze, Z. (2003). Forward Dynam-ics of Open-Loop Multibody Mechanisms Using an Efficient Recursive AlgorithmBased on Canonical Momenta. Multibody Systems Dynamics, 10(1), 45–59.

180 APPENDIX D

[Novodex, 2004] Novodex (2004). http://www.novodex.com/.

[Papastavridis, 1999] Papastavridis, J. G. (1999). Tensor Calculus and AnalyticalDynamics. CRC Press.

[Pars, 1962] Pars, L. A. (1962). An Introduction to the Calculus of Variations.London: Heinemann.

[Pars, 1965] Pars, L. A. (1965). A Treatise of Analytical Dynamics. London:William Heinemann,Ltd.

[Pereira & Nikravesh, 1996] Pereira, M. S. & Nikravesh, P. E. (1996). Impact dy-namics of multibody systems with frictional contact using joint coordinates andcanonical equations of motion. Nonlinear Dynamics, 9, 53–71.

[Pesce, 2003] Pesce, C. P. (2003). The application of lagrange equations to me-chanical systems with mass explicitly dependent on position. Journal of AppliedMechanics, 70, 751–756.

[Petzold, 1982] Petzold, L. R. (1982). A description of dassl: A differen-tial/algebraic system solver. IMACS Transactions on Scientific Computation,1.

[Pfeiffer & Glocker, 1996] Pfeiffer, F. & Glocker, C. (1996). Multibody Dynamicswith Unilateral Contacts. New York: John Wiley and sons.

[Press et al., 1992] Press, W. H., Teukolsky, S. A., Vetterling, W. T., & Flannery,B. P. (1992). Numerical Recipies in C: The Art of Scientific Computing (2nd.ed.). Cambridge University Press.

[Prince & Dormand, 1981] Prince, P. J. & Dormand, J. R. (1981). High orderembedded runge-kutta formulae. Journal of Computational and Applied Mathe-matics, 7, 67–75.

[Rein, 1993] Rein, U. (1993). Efficient Object Oriented Programming of MultibodyDynamics Formalisms. Proceedings of the NATO-Advanced Study Institute onComputer Aided Analysis of Rigid and Flexible Mechanical Systems, 2, 59–69.

[Roberson & Schwertassek, 1988] Roberson, R. E. & Schwertassek, R. (1988). Dy-namics of Multibody Systems. Berlin: Springer-Verlag.

[Rosenthal, 1990] Rosenthal, D. E. (1990). An Order n Formulation for RoboticSystems. The Journal of the Astronautical Sciences, 38(4), 511–529.

[Ruhoff et al., 1996] Ruhoff, P. T., Præstgaard, E., & Perram, J. W. (1996). Ef-ficient algorithms for simulating complex mechanical systems using constraintdynamics. Proc. Roy. Soc. Lond. A, 452, 1139–1165.

BIBLIOGRAPHY 181

[Samin & Fisette, 2003] Samin, J. C. & Fisette, P. (2003). Symbolic Modeling ofMultibody Systems. Kluwer Academic Publishers.

[Shabana, 1989] Shabana, A. A. (1989). Dynamics of Multibody Systems. NewYork: Wiley.

[Shin et al., 2003] Shin, H. J., Kovar, L., & Gleicher, M. (2003). Physical touch-upof human motions. 11th Proceedings of Pacific Graphics.

[Shoemake, 1985] Shoemake, K. (1985). Animating rotation with quaternioncurves. Proceedings of SIGGRAPH 1985, 19, 245–254.

[Simmonds, 1994] Simmonds, J. G. (1994). A Brief on Tensor Analysis (2nd ed.).New York: Springer-Verlag.

[Stejskal & Valasek, 1996] Stejskal, V. & Valasek, M. (1996). Kinematics and Dy-namics of Machinery. Marcel Dekker.

[Stoianovici & Hurmuzlu, 1996] Stoianovici, D. & Hurmuzlu, Y. (1996). A criticalstudy of the applicability of rigid body collision theory. ASME Journal of AppliedMechanics, 63(2), 307–316.

[Terze et al., 2001] Terze, Z., Lefeber, D., & Muftic, O. (2001). Null space inte-gration method for constrained multibody systems with no constraint violation.Multibody System Dynamics, 6(3), 229–244.

[Terze et al., 2003] Terze, Z., Naudet, J., & Lefeber, D. (2003). Constraint Gradi-ent Projective Method for Stabilized Dynamic Simulation of Constrained Multi-body Systems. ASME International Design Engineering Technical Conferences,DETC2003/VIB-48314.

[Vereshchagin, 1974] Vereshchagin, A. F. (1974). Computer simulation of the dy-namics of complicated mechanisms of robot manipulations. Engineering Cyber-netics, 6, 65–70.

[Vukobratovic et al., 1994] Vukobratovic, M. K., Filaretov, V. F., & Korzun, A. I.(1994). A unified approach to mathematical modelling of robotic manipulatordynamics. Robotica, 12, 411–420.

[Walker & Orin, 1982] Walker, M. W. & Orin, D. E. (1982). Efficient DynamicComputer Simulation of Robotic Mechanisms. Journal of Dynamic Systems,Measurement and Control, 104, 205–211.

[Wittenburg, 1977] Wittenburg, J. (1977). Dynamics of Systems of Rigid Bodies.Stuttgart: B.G. Teubner.

[Zomaya, 1992] Zomaya, A. Y. (1992). Modelling and Simulation of Robot Manip-ulators: a Parallel Processing Approach. World Scientific.

Vrije Universiteit Brussel - pdfs. · PDF fileProf. dr. ir. Herman Bruyninckx (Katholieke...

Documents

Transcript of Vrije Universiteit Brussel - pdfs. · PDF fileProf. dr. ir. Herman Bruyninckx (Katholieke...