FRIEDRICH-ALEXANDER-UNIVERSITAT¨ ERLANGEN-NURNBERG¨ INSTITUT FUR¨ INFORMATIK (MATHEMATISCHE MASCHINEN UND DATENVERARBEITUNG)

Lehrstuhl f¨urInformatik 10 (Systemsimulation)

Rigid Body Dynamics: Links and Joints

Kristina Pickl

Master Thesis

Rigid Body Dynamics: Links and Joints

Kristina Pickl Master Thesis

Aufgabensteller: Prof. Dr. Ulrich R¨ude Betreuer: Klaus Iglberger, M. Sc. Bearbeitungszeitraum: 16.03.2009 – 16.09.2009

Declaration:

I confirm that I developed this thesis on my own, without any help of others and that no sources and facilities other than those mentioned in this thesis were used. This thesis has never been submitted in total, in part or in modified form to any other examination board. All quotations taken from other sources are referenced accordingly.

Erkl¨arung:

Ich versichere, dass ich die Arbeit ohne fremde Hilfe und ohne Benutzung anderer als der angegebenen Quellen angefertigt habe und dass die Arbeit in gleicher oder ¨ahnlicher Form noch keiner anderen Prufungsbeh¨ ¨orde vorgelegen hat und von dieser als Teil einer Prufungs-¨ leistung angenommen wurde. Alle Ausfuhrungen,¨ die w¨ortlich oder sinngem¨aß ubernommen¨ wurden, sind als solche gekennzeichnet.

Erlangen, den 16.09.2009 ......

Abstract

Physics-based multibody simulations have a wide range of applications. In order to be capable of analyzing complex problem formulations, e.g. the motion of the human skeleton, joints are a necessary requirement for multibody simulation frameworks. The major focus of this thesis lies on the development and implementation of a general scheme to integrate joint motion constraints into the pe framework, which is developed at the Chair for System Simulation at the Friedrich-Alexander University Erlangen-Nuremberg. The mathematical background based on complementarity constraints is elucidated profoundly. Full-Coordinate methods are used to preserve the motion constraints. The complex mechan- ical background is elaborated. An abstract formulation, which incorporates joint, contact and limit constraints, and which is based on analytical mechanics is deduced. The Jacobian matrix notation is introduced to achieve a unified complementarity formulation. Several example joints are modeled and explained in detail. Error correction parameters for drifting problems due to the Full-Coordinate method are introduced in order to keep the er- rors to a minimum. Special emphasis lies on the modular class design, which enables the easy addition of other joint types in the future. In order to demonstrate the suitability of the extension selected joints are implemented. The correctness is substantiated with the help of several test scenarios, which test the specific char- acteristics of each implemented joint type. The results of these validation tests are analyzed and further interesting joint types and augmentations to the implementation are suggested.

Zusammenfassung

Physikalisch korrekte Starrk¨orpersimulationen k¨onnen vielf¨altig angewandt werden. Um kom- plexe Problemstellungen, wie z.B. die Bewegung des menschlichen Skeletts analysieren zu k¨onnen, ben¨otigt man in einem System zur Simulation von Mehrk¨orpern Verbindungen zwi- schen den K¨orpern. Das Hauptaugenmerk dieser Arbeit liegt in der Entwicklung und Implementierung eines allge- meinen Modells zur Einbindung von Zwangsbedingungen auf die Bewegung von Starrk¨orpern in das Simulationstool pe, das am Lehrstuhl fur¨ Systemsimulation der Friedrich-Alexander Universit¨at Erlangen-Nurnberg¨ entwickelt wird. Der mathematische Hintergrund, der auf linearen Komplementarit¨atsproblemen basiert, wird tiefgehend erl¨autert. Full-Coordinate Methoden werden dazu benutzt, die Zwangsbewegungen einzuhalten. Der komplizierte mechanische Hintergrund wird sorgf¨altig ausgearbeitet. Eine abstrakte Formulierung, die Verbindungen, Kontakte und Grenzen miteinbezieht und auf ana- lytischer Mechanik basiert, wird hergeleitet. Die Notation mit Hilfe von Jacobimatrizen wird eingefuhrt¨ um eine ganzheitliche Komplementarit¨atsformulierung zu erhalten. Zahlreiche Beispielverbindungen werden modelliert und detailgetreu erl¨autert. Parameter zur Fehlerkorrektur bezuglich¨ Abdriften der K¨orper, was auf die Full-Coordinate Methode zuruck-¨ zufuhren¨ ist, werden eingefuhrt¨ um die Fehler minimal zu halten. Ein besonderer Schwerpunkt liegt auf dem modularen Klassendesign, welches eine leichte zukunftige¨ Einbindung von wei- teren Verbindungsarten erm¨oglicht. Um die Eignung der Erweiterung zu zeigen, werden ausgew¨ahlte Beispielverbindungen imple- mentiert. Die Richtigkeit wird durch mehrere Testszenarien untermalt, welche die spezifischen Charakteristika jeder implementierten Verbindung prufen.¨ Die Ergebnisse dieser Validierungs- tests werden analysiert und weitere interessante Arten von Verbindungen sowie zukunftige¨ Erweiterungen der Implementierung vorgeschlagen.

Acknowledgements

I would like to thank Prof. Dr. Ulrich R¨udefor providing me with this interesting master thesis topic, for tutoring me throughout my studies and for offering me the great opportunity to attend the Multibody Dynamics Conference in Warsaw, which gave me a profound insight into the multibody community. Very special thanks to my supervisor Klaus Iglberger for his overall support and advice, not only in the context of this thesis, but also throughout the rest of my studies. Also, very special thanks to my “contact person” Tobias Preclik for several helpful hints and discussions and, moreover, for proofreading this thesis. Additionally, I would like to thank Tobias Gradl and Stefan Donath for several hints on the LaTeX layout of this thesis. Furthermore, many thanks to Dr. Harald K¨ostlerfor supporting and encouraging me during my studies.

Contents

Contents

1 Introduction1 1.1 Motivation...... 1 1.2 Overview...... 2

2 Mechanical Background3 2.1 Classical Mechanics...... 3 2.1.1 Newton-Euler Equations for Contact Mechanics...... 4 2.1.2 Integration of Joints...... 6 2.2 Analytical Mechanics...... 6 2.2.1 Velocity-based Constraint Formulation...... 7 2.2.2 Bilateral Constraints...... 7 2.2.3 Unilateral Constraints...... 8 2.3 Unified Constraint Formulation...... 9

3 Numerical Background 11 3.1 Linear Complementarity Problem...... 11 3.2 Solver...... 12 3.3 Error Reduction...... 12 3.4 Limits...... 13 3.5 Unified Complementarity Notation...... 14

4 Joint Modeling 17 4.1 Joint Types...... 17 4.1.1 Fixed Joint...... 18 4.1.2 Slider Joint...... 20 4.1.3 Hinge Joint...... 22 4.1.4 Contact Joint...... 24 4.2 Joint Limits...... 25 4.2.1 Linear Limits...... 26 4.2.2 Angular Limits...... 27

5 Implementation 31 5.1 Data Preparation...... 31 5.1.1 Data Storage and Allocation...... 32 5.1.2 Data Insertion...... 33 5.2 Class Design...... 37 5.3 Visualization...... 38

i Contents

6 Validation and Results 41 6.1 Fixed Joint...... 41 6.1.1 Test Scenario...... 41 6.1.2 Results...... 42 6.2 Slider Joint...... 46 6.2.1 Test Scenario...... 46 6.2.2 Results...... 46 6.3 Hinge Joint...... 50 6.3.1 Test Scenario...... 50 6.3.2 Results...... 51 6.4 Planar Slider Crank Mechanism...... 54 6.4.1 Test Scenario...... 54 6.4.2 Results...... 54

7 Conclusions and Future Work 59 7.1 Conclusions...... 59 7.2 Future Work...... 60

Bibliography 63

ii List of Figures

List of Figures

2.1 Sketch of a slider joint which can only move along one axis...... 6

3.1 Dislocated ball-in-socket joint...... 13

4.1 A fixed joint...... 18 4.2 Sketch of a fixed joint w.r.t. the global coordinate system...... 18 4.3 A slider joint...... 20 4.4 Sketch of a slider joint w.r.t. the global coordinate system...... 20 4.5 A hinge joint...... 22 4.6 Sketch of a hinge joint w.r.t. the global coordinate system...... 22 4.7 A contact joint...... 24 4.8 Sketch of a contact point w.r.t. the global coordinate system...... 25 4.9 Sign convention of the aperture angle of a hinge joint...... 28 4.10 An arbitrary initial angle of a hinge joint...... 29 4.11 Calculation of an arbitrary initial angle of a hinge joint...... 30

5.1 Hierarchy of the collision response module...... 31 5.2 Example scenario...... 34 5.3 Global Jacobian matrix of the example scenario...... 34 5.4 Class design of the implementation...... 38

6.1 Test scenario for the fixed joint...... 41 6.2 Distribution of the forces in the fixed joint during the simulation...... 42 6.3 Screenshots from the fixed joint simulation...... 43 6.4 The constant distance between the two spheres w.r.t. the time steps...... 44 6.5 The angular velocity around the y-axis of both spheres within fixed joint and union w.r.t. the time steps...... 44 6.6 The position of sphere 1 within fixed joint and union w.r.t. the time steps.... 45 6.7 The position of sphere 2 within fixed joint and union w.r.t. the time steps.... 45 6.8 Test scenario for the slider joint...... 46 6.9 Screenshots from the slider joint simulation...... 47 6.10 Distribution of the forces in the slider joint during the simulation...... 48 6.11 The length of the slider joint w.r.t. the time steps...... 49 6.12 The angular velocity of both spheres w.r.t. the time steps...... 49 6.13 Test scenario for the hinge joint...... 50 6.14 Aperture angle of the hinge joint w.r.t. the time steps...... 51 6.15 Angular velocity of capsule 2 around the axis of rotation w.r.t. the time steps.. 52 6.16 Screenshots from the hinge joint simulation...... 53 6.17 Test scenario for the slider crank mechanism...... 54

iii List of Figures

6.18 The length of the slider joint within the planar slider crank mechanism w.r.t. the time steps...... 56 6.19 Screenshots from the planar slider crank joint simulation...... 57

iv List of Tables

List of Tables

6.1 Test conditions for the validation of the fixed joint...... 42 6.2 Test conditions for the validation of the slider joint...... 46 6.3 Test conditions for the validation of the hinge joint...... 50 6.4 Test conditions for the validation of the slider crank mechanism...... 55

v

List of Algorithms

List of Algorithms

5.1 Determination of the rows and columns of the data members of the boxLCP.. 32 5.2 Allocation of the data members of the boxLCP...... 33 5.3 Data insertion in the matrix J of the boxLCP...... 35 5.4 Data insertion in the vector btemp of the boxLCP...... 36 5.5 Postprocessing step...... 37 5.6 Subtraction of ~berror...... 37 5.7 The POV-Ray visualization of the fixed joint...... 39

vii

Chapter 1: Introduction

Chapter 1

Introduction

1.1 Motivation

In the last few years, physics-based multibody simulations gained more and more importance due to their versatility (cf. [Sch97]). In the following, three research topics were chosen in order to give an overview on the state of the art of multibody dynamics. One of the numerous research fields is vehicle dynamics. Characteristic problems concern the suspension of a vehicle, maximization of the ride comfort and lifetime predictions. In the field of railway research, the wheel-rail contact points are an example of contact modeling problems to estimate the rate of wear on both parts (cf. [FMMR09], [PAP+09]). The key challenges on these models are the precise representation of external loads, track geometry, and boundary conditions as well as the detection of all necessary contact points. The main advantage of these multibody simulations is that the optimal design solution can be obtained by means of several test runs with different parameters in a much more cost-effective way than with real prototypes and physical tests. Another application area are game-physics engines. For instance, Havok Physics ([Hav09]) and Euphoria ([Eup09]) are only two among a vast number of commercially licensed physics engines, used for physics-based modeling and animation in recent computer games. In game- physics, speed outweighs the accuracy of the simulation. Nevertheless, maximum realistic behavior should be achieved throughout the scenarios. For instance, walls should collapse in a realistic way, bullets should fly on an accurate path of motion, ropes should swing realist- ically as well as the movement of a flag blowing in the wind should look credible. The most challenging and important part of a game-physics engine is a naturalistic character animation, which also includes the modeling of the human skeleton. This brings us to another complex research field of multibody dynamics: biomechanics. In general, the problem formulations are based on biomechanical models combined with physics- based multibody dynamics. Muscle forces and even the behavior of the central nervous system are included into the formulation, e.g. in the work done by Rodrigo et al. [RA09], which stud- ies the human gait by means of “a whole-body response biomechanical model with forty-three degrees of freedom”. Another interesting paper by Machado et al. [MFC+09] describes the modeling of the human knee joint based on multibody dynamics. Even ligaments are included into the joint model by means of non-linear elastic springs. This short survey clearly shows that joints are a necessity for every multibody simulation framework. In order to be capable of modeling complex problem formulations of all the previ- ously mentioned research fields, several basic joint types are required. Throughout this thesis

1 Chapter 1: Introduction a general scheme was developed in order to enable the integration of joints into the physics- based simulation framework pe, which is developed at the Chair for System Simulation at the Friedrich-Alexander University Erlangen-Nuremberg.

1.2 Overview

This thesis is structured in the following way. It starts with the elucidation of the mechan- ical background, which forms the basis of our physics-based simulation in Chapter2. Here, the equations, taken from classical mechanics and necessary for the joint modeling, are intro- duced and abstracted to a unified complementarity formulation, based on Jacobian matrices. In Chapter3, the special kind of mathematical problem which arises when joints are modeled is described: linear complementarity problems. Moreover, the numerical means of error re- duction are considered. Furthermore, joint limits are introduced in order to complete the requirements of joint modeling. Chapter4 describes how we modeled several example joints to demonstrate the capabilities of the extension. The Jacobian matrices, error reduction para- meters and joint limits for the respective joint types are derived. Afterwards, in Chapter5 several examples of the implementation are selected in order to present the work from an algorithmic point of view. Here, the data storage, allocation and insertion is shown. Addi- tionally, the modular class design is presented, which allows to easily integrate other joint types into the pe framework. Selected validation scenarios are presented in Chapter6 to demonstrate the correctness and suitability of our implementation. Finally, in Chapter7a conclusion of the achieved results is given and propositions for future work related to the topic of this thesis are made.

2 Chapter 2: Mechanical Background

Chapter 2

Mechanical Background

In this chapter, the mechanical background is explained in detail on which the simulation of constrained multibody systems is based. This elucidation starts with the approach of classical mechanics to describe the dynamics of a rigid body. In the beginning of following the chapter, only contacts are in the center of attention but joints will be integrated gradually. In the analytical mechanics section, the necessary Jacobian matrix constraint formulation is introduced. In the end, the more abstract and general Lagrange multiplier formulation is obtained, which allows to combine arbitrary joints and contacts into one notation.

2.1 Classical Mechanics

The model of a multibody system follows the principles of classical mechanics. The well- known Newton-Euler equations ([Pfe92, section 1.7.3]) describe the motion of rigid bodies in a system. In general, there are two possibilities to integrate constraints into the equation system. Either the number of coordinates is reduced (Reduced Coordinate methods) or additional forces are inserted (Full-Coordinate methods) in order to preserve the constraints (cf. [Bar96], [Erl05]). In the Reduced Coordinate methods, only the relative motion of the bodies is described and, thus, only the relative coordinates of them are needed. These methods are often computation- ally faster as they are frequently implemented recursively. Nevertheless, they have problems in handling contacts and have a confusing notation based on Coriolis and centripetal acceler- ation. In this approach, Full-Coordinate methods are used. Here, all bodies are represented by a full set of rigid body coordinates. Joint parameters are derived only if needed. The method is computationally more complex because all constraints on every single rigid body need to be described. Nonetheless, it benefits from a more understandable notation, can handle contact constraints, and allows to design the algorithmic implementation very modularly. Considering the visualization, the most significant advantage resulting from the Full-Coordinate methods, from a debugging point of view, is that errors can be recognized visually. The errors (in body relative coordinates) will introduce a drifting effect. The two parts of the joint will start to move apart. In contrast, the bodies will always stay connected when using Reduced Coordin- ate methods in spite of big errors being present. In Section 3.3 these correlations will be explained in more detail.

3 Chapter 2: Mechanical Background

2.1.1 Newton-Euler Equations for Contact Mechanics For a contact mechanics problem, the Newton-Euler equations describe the motion for all n ∈ N rigid bodies of a system. For the i-th body (with summation over all contacts K ∈ N ) holds ˙ ~ri = ~vi, (2.1) 1 q˙ = ~ω q , (2.2) i 2 i i ˙ −1 X ~ −1 X ~ −1 ~ext ~vi = mi fk − mi fk + mi fi , (2.3) k,jk=i k,ik=i ˙ −1 X ~ −1 X ~ −1 −1 ext ~ωi = Θi ~rkj × fk − Θi ~rki × fk − Θi ~ωi × Θi~ωi + Θi ~τi . (2.4) k,jk=i k,ik=i

The indices ik and jk denote the two rigid bodies involved in the contact k. It can never d happen that ik = jk. The dot-notation for the time derivative dt is used in favor of clarity. The mass of the body is given by mi, the inertia tensor by Θi, the vector from the center of mass ~ri to the contact point ~pk by ~rki = ~pk − ~ri , the velocity and the angular velocity at the center of mass by ~vi and ~ωi respectively. The orientation of the rigid body is denoted by a ~ quaternion qi. The forces are split into contact forces fk (k ∈ {1,...,K}) and external forces ~ext ext fi plus the total torque from all external forces ~τi . Matrix notation and concatenation lead to a formulation, which is more general and easier to read (cf. [Erl05, chapter 4]). Thus, the Newton-Euler equations for the frictional case can be rewritten as

~s˙ = S~u, (2.5) ˙ −1     ~u = M C Nf~ + Dβ~ + f~ext . (2.6)

For a system of n rigid bodies, the generalized position and orientation vector is given by

 T T T T T T T 7n ~s = ~r1 , q1 , ~r2 , q2 , . . . , ~rn , qn ∈ R (2.7) and the matrix composed of rotations by

I 0   Q   1   ..  7n×6n S =  .  ∈ R , (2.8)    I  0 Qn where   −xi −yi −zi 1  si zi −yi 4×3 Qi =   ∈ R (2.9) 2 −zi si xi  yi −xi si

T 4 results from the given quaternion qi = [si, xi, yi, zi] ∈ R . Additionally, I denotes the identity matrix. The generalized velocity vector is represented by

 T T T T T T T 6n ~u = ~v1 , ~ω1 ,~v2 , ~ω2 , . . . ,~vn , ~ωn ∈ R , (2.10)

4 Chapter 2: Mechanical Background the generalized mass matrix by   m1 I 0  Θ1     ..  6n×6n M =  .  ∈ R (2.11)    mn I  0 Θn and the matrix of contact conditions by  −I for l = 2ik − 1  × −r for l = 2ik  kik  6×3 Clk = I for l = 2jk − 1 ∈ R , (2.12)  × r for l = 2jk  kjk 0 otherwise where   0 −r3 r2 × 3×3 r =  r3 0 −r1 ∈ R (2.13) −r2 r1 0 is a skew-symmetric matrix. The matrix of contact normals is given by   ~n1 0  ~n2  N =   ∈ 3K×K (2.14)  ..  R  .  0 ~nk and ~ T K f = [f1, f2, . . . , fK ] ∈ R (2.15) concatenates the magnitudes of the normal forces into a single vector. D contains the matrices Dk on its diagonal. The η direction vectors approximate the friction cone at the k-th contact (cf. [AP97]), where η = 2i for all i ∈ N and i ≥ 1:   D1 0  D2  D =   ∈ 3K×ηK , (2.16)  ..  R  .  0 DK h i with D = d~T ,..., d~T ∈ 3×η. The vector of all friction components is given by k 1k ηk R ~ T η βk = [β1k , . . . , βηk ] ∈ R (2.17) and the vector of all external forces by T ~ h ~extT ext
T ~extT ext
T i 6n fext = f1 , ~τ1 − ~ω1 × Θ1~ω1 ,..., fn , ~τn − ~ωn × Θn~ωn ∈ R . (2.18) Discretizing equations (2.5) with an semi-implicit Euler-step in time leads to ~s t+∆t = ~s t + ∆tS~ut+∆t, (2.19) t+∆t t −1    ~u = ~u + ∆tM C Nf~ + Dβ +f~ext | {z } f~contact (2.20) t −1   = ~u + ∆tM f~contact + f~ext .

5 Chapter 2: Mechanical Background

Equation (2.19) denotes the position update and equation (2.20) the velocity update. For ease of readability, the constraint forces resulting from the contacts are unified into the vector ~ 6n fcontact ∈ R . Thus, contact forces additionally constrain the motion of rigid bodies along with external forces. The problem of contact mechanics has already been analyzed and discussed in [Pre08]. The resulting solvers for contact constraints have been integrated into the pe framework.

2.1.2 Integration of Joints Let us now address the main task of this thesis, the integration of joints into the pe. Considering different literature and papers (e.g. [Smi04], [Erl05], [Sha98], [Cra86]), the terms “joint” and “constraint” are used interchangeably. In general, a joint does constrain the motion of a rigid body system as well as contacts and external forces do. The key aspect of joint modeling is the number of degrees of freedom (DOF) of a rigid body. It corresponds to the possibilities of movement. In free space, there are six DOF: three for the translational movement along the x-, y- and z-axis and three for the rotational movement around the x-, y- and z-axis. A joint reduces the possibilities of movement of two bodies. Only certain positions and orientations can be reached by a constrained rigid body. Thus, the DOF are decreased. According to the kind of joint, c DOF are removed, thus

c = 6 − DOFleft (2.21) holds. The slider joint shown in Figure 2.1 serves as an example to demonstrate this correl- ation. The movement is limited to a translation along the axis of symmetry. Hence, there is only one DOF left and, as a consequence, c = 5.

Figure 2.1: Sketch of a slider joint which can only move along one axis.

As mentioned before, Full-Coordinate methods are used in the scope of this thesis. Thus, in order to include joints into the discretized Newton-Euler equations of motion, an extra term ~ 6n fjoint ∈ R for the resulting joint forces is added to equation (2.20). This leads to

t+∆t t −1   ~u = ~u + ∆tM f~contact + f~joint + f~ext . (2.22)

2.2 Analytical Mechanics

So far, classical mechanics sufficed to model contact problems. In this section, a more general analytical mechanics formulation is introduced. It aims to treat bodies, forces, and constraints as abstract as possible. This concept has the advantage of unifying contacts and joints into one notation. First of all, the different types of constraints are explained. After that, the section is concluded with the newly obtained unified constraint notation based on Lagrange multipliers. Most of this chapter is based on [Erl05, section 4.7]. Other sources are stated explicitly.

6 Chapter 2: Mechanical Background

2.2.1 Velocity-based Constraint Formulation In general, there are two different approaches to describe the motion of rigid bodies in a system: velocity-based (also known as impulse-based) and acceleration-based formulations. The pe framework adopts the velocity-based formulations, which do not consider accelerations at one particular point of time. Instead, they consider the motion of the bodies within a whole time interval, in which the velocity changes are calculated. Moreover, in contrast to the acceleration-based formulations, velocity-based ones can handle collisions, which are required to generate contacts in the simulation scenario.

In the following, constraints between two rigid bodies Bi and Bj are considered, i.e. the generalized velocity vector shortens to

~  T T T T T 12 ue = ~vi , ~ωi ,~vj , ~ωj ∈ R (2.23) and the generalized position vector to

~  T T T T T 14 se = ~ri , qi , ~rj , qj ∈ R . (2.24) In order to integrate both, joints and contacts, into the velocity-based approach kinematic con- straints are needed. For this purpose, two different types of constraint models are introduced: bilateral and unilateral constraints.

2.2.2 Bilateral Constraints

For a system of L ∈ N joints a bilateral constraint, also known as holonomic constraint, is defined as a function Φ of time and generalized position vector ~ Φ(t, se) = 0. (2.25) In the case of all joints mentioned in this thesis the function Φ is time-independent. This special case of a bilateral constraint is called scleronomic constraint. Typical examples of such constraints are a slider or a hinge joint. With this type of constraint, the global motion of both attached bodies is limited. For the l-th joint (l ∈ {1,...,L} ) there are c bilateral constraints. c corresponds to the number of DOF the joint removes from the freemoving rigid body. These can be concatenated into a vector of c independent constraint equations   Φ (~s) 1l e Φ (~s) ~ ~  2l e  Φl(s) =  .  = 0. (2.26) e  .   .  Φ (~s) cl e For the velocity-based approach, the required kinematic constraints are obtained by differen- tiation of the vector (2.26) w.r.t. time: ~ ~ ˙ ~ ∂Φl~˙ ∂Φl ~ ~ Φ~ l(s) = s = S u = JΦ u = 0. (2.27) e ∂~s e ∂~s e l e e | {ze } J Φl c×12 JΦl ∈ R is called local Jacobian matrix of joints. From the principle of virtual work and Lagrange’s equation ([GHS02, sections 4.2 and 4.3]) follows that the generalized constraint force of the l-th joint results in f~ = JT ~λ . (2.28) Φl Φl Φl

7 Chapter 2: Mechanical Background

~ c λΦl ∈ R is the vector of Lagrange multipliers, which can take any positive or negative real value, i.e. ~ − ∞ ≤ λΦl ≤ ∞. (2.29) For a detailed explanation and derivation of the principle of virtual work and Lagrange’s equa- tion in the context of multibody dynamics consider [Sha98, sections 3.3 and 3.4] or [Wit97]. The problem formulation including Lagrange multipliers is also used in [Bar96].

2.2.3 Unilateral Constraints So far only joint constraints have been modeled. In order to include contact constraints into the velocity-based formulation, unilateral constraints are introduced, which are a special type of non-holonomic constraints. They are based on the same concept as bilateral constraints. For a system of K ∈ N contacts a unilateral constraint is defined as a function Ψ of time and generalized position vector ~ Ψ(t, se) ≥ 0. (2.30) For this type of constraints, the time can also be neglected. Following the concept of bilateral constraints, the kinematic constraint for the k-th contact (k ∈ {1,...,K}) is obtained by differentiation of the vector of the (1 + η) independent constraint equations w.r.t. time:

~ ~ ˙ ~ ∂Ψk~˙ ∂Ψk ~ ~ Ψ~ k(s) = s = S u = JΨ u ≥ 0. (2.31) e ∂~s e ∂~s e k e e | {ze } J Ψk

(1+η)×12 JΨk ∈ R is called the local Jacobian matrix of contacts. In analogous manner to bilateral constraints, the generalized constraint force for the k-th contact results in f~ = JT ~λ , (2.32) Ψk Ψk Ψk ~ 1+η where λΨk ∈ R is the vector of Lagrange multipliers for the contact k. As mentioned above, these kinds of constraints cannot be handled with a Reduced Coordinate method, which is one of the reasons why the pe is based on a Full-Coordinate method. Within the context of this thesis, the main emphasis lies on the integration of joints and not the best approximation of the friction forces. Therefore, the friction forces are approximated by box friction. For our purposes, it is sufficient to model the friction cone at the k-th contact with the contact normal and two direction vectors, namely the tangents in x- and y-direction. For this case η = 2. Additionally, to maintain these constraints the values of the vector of Lagrange multipliers have to be inbetween the vectors of the lower and upper limits, ~λ and Ψk ~ λΨ respectively: k     0 λ  ∞ Ψk1 −µλeΨ  ≤ λΨ ≤ µλeΨ  . (2.33)  k1   k2   k1  −µλ λΨ µλ eΨk1 k3 eΨk1 | {z } | {z } | {z } ~ ~λ ~ λ Ψk λ Ψk Ψk The first row constrains that the normal force will always take a value greater than or equal to zero. Moreover, the last two rows enforce that the absolute value of the tangential forces in x- and y-direction will always stay within the box friction limits.

8 Chapter 2: Mechanical Background

2.3 Unified Constraint Formulation

In this section, we switch back to a system of n rigid bodies. In order to integrate all L joints, the global Jacobian matrix Jjoint of all joints is assembled to form J ...... J  Φ11 Φn1 . .  . .    PL   ( l=1 cl)×6n Jjoint = JΦ1 ... JΦi ... JΦj ... JΦn  ∈ R . (2.34)  l l l l   . .   . .  J ...... J Φ1L ΦnL

The same follows for the global Jacobian matrix Jcontact for all K contacts:  J ...... J  Ψl1 Ψn1  . .   . .    3K×6n Jcontact = JΨ1 ... JΨi ... JΨj ... JΨn  ∈ R . (2.35)  k k k k   . .   . .  J ...... J Ψ1K ΨnK

Both, Jjoint and Jcontact are very sparse because each of the constraints affects only two bodies. In the l-th (k-th) row the only non-zero entries are J (J ) in column i, and J (J ) in Φil Ψik Φjl Ψjk column j corresponding to the bodies Bi and Bj respectively. Thus, each row in both matrices contains only 12 non-zero entries. When we also concatenate all Lagrange multipliers into one vector, we obtain

h iT PL ~ l=1 cl λjoint = λΦ1 , . . . , λΦ1 , . . . , λΦL , . . . , λΦL ∈ (2.36) 1 c1 1 cL R and h iT ~λ = λ , . . . , λ , . . . , λ , . . . , λ ∈ 3K (2.37) contact Ψ11 Ψ13 ΨK1 ΨK3 R respectively. Finally, the generalized constraint forces of both, bilateral and unilateral constraints, result in ~ T ~ fjoint = Jjointλjoint, (2.38) ~ T ~ fcontact = Jcontactλcontact. (2.39)

To conclude this chapter, the proof from [Erl05, section 4.7.2] is stated in order to show that the Jacobian formulation for contacts (2.39) is equal to the formulation (2.20) in Section 2.1.1 based on classical mechanics.

Proof. First we establish with some straightforward computations that " #   f~ C Nf~ + Dβ~ = CNf~ + CDβ~ = CN CD . (2.40) β~

Simple comparison of the equations above results in " # f~ h i CN CD ≡ JT  ~λ . (2.41) β~ contact contact

9 Chapter 2: Mechanical Background

Finally, we have to swap rows and columns such that for the k-th contact holds

λ = f , Ψk1 k λ = β , Ψk2 1k . (2.42) .

λΨ = βη k(1+η) k to obtain " # f~  h i CN CD = π JT  ~λ . (2.43) β~ contact contact With the permutation π(· ) the correlation is clear.



10 Chapter 3: Numerical Background

Chapter 3

Numerical Background

This chapter describes which numerical means are required to model constraints. First of all, it introduces the special kind of the problem, a linear complementarity problem, and shows how to solve this problem numerically. After that, a closer look at the Jacobian matrix delivers insight into the appearing numerical errors. Furthermore, the details of the minimization of these errors are elucidated. In order to finalize the problem formulation of the joints, another class of constraints is introduced: joint limits. Finally, the unified complementarity formulation is stated to conclude the chapter.

3.1 Linear Complementarity Problem

The contact conditions cause non-smooth relationships between the rigid bodies of a system. Consequently, this leads to a non-smooth problem formulation. The mathematical model which arises from such constraints is called linear complementarity problem (LCP). It is closely related to the linear and quadratic programming problem. Both can be rephrased to LCPs. The general LCP is defined as: n find ~x ∈ R , subject to ~x,~y ≥ 0, ~x ⊥ ~y, (3.1) where ~y = A~x +~b, n n×n ~y,~b ∈ R , A ∈ R . Complementarity formulations are well known to the research community (e.g. [AT08], [AP97], [PT96], [Erl05]). Most of the papers are only treating contacts, which are building up a sys- tem of complementarity conditions. Integrating joint constraints is straightforward. They just add further complementarity conditions to the existing system of contacts. An overview of different complementarity problems can be found in [CPS92]. The complementarity problem relevant to this thesis is the box-constrained linear comple- mentarity problem (boxLCP): n find ~x ∈ R ,

subject to yi ≥ 0 → xi = xi, yi ≤ 0 → xi = xi, (3.2) yi = 0 → xi < xi < xi, where ~y = A~x +~b, n n×n n ~y,~b ∈ R , A ∈ R , ~x, ~x ∈ R .

11 Chapter 3: Numerical Background

~x is the vector of lower and ~x the vector of upper limits. Additionally, the matrix A is symmetric. Depending on the problem, the matrix can be positive semi-definite or even positive definite. According to the characteristics of the matrix, different solvers can achieve considerable advantages in efficiency.

3.2 Solver

In general, the solvers for an LCP can be classified into two different categories: iterative and pivoting methods (see [CPS92, chapters 4 and 5] for more details). The pe offers a variety of solvers for LCPs: a conjugate projected gradient solver (iterative), a Lemke solver (pivoting), non-smooth Newton solvers (iterative), which differ according to the LCP, and different matrix-splitting solvers (iterative). In this thesis, only the projected Gauss-Seidel solver has been used, which belongs to the group of matrix-splitting solvers. It will be explained shortly in the following. The first step corresponds to a normal Gauss-Seidel algorithm. The matrix A is split into three components A = (D + L + U) ↔ (D + L + U)x = b, (3.3) where L is a strictly lower triangular matrix, U is a strictly upper triangular matrix, and D is a diagonal matrix. The requirement to the matrix A is that it is positive definite. The complete iterationstep results in   xk+1 = (D + L)−1 b − Uxk . (3.4)

The only difference to a normal Gauss-Seidel is the crucial projection step. During the itera- tion, it enforces the complementarity constraints and is given by
xˆj = max min (xj, xj) , xj ∀j = 0, . . . , n − 1. (3.5)

3.3 Error Reduction

Every numerical method suffers from inaccuracies. In the following, the concept of error reduction according to [Erl05] and [Smi02] will be outlined. As mentioned in section 2.1a Full-Coordinate method is used to model the constraints. First of all, let us take a closer look at the Jacobian matrix. The following examinations are again limited to a system consisting of the two bodies Bi and Bj. For the two-body system the generalized velocity vector shortens to four components ~  T T T T T ue = ~vi , ~ωi ,~vj , ~ωj . (3.6) When one considers only the l-th constraint, one obtains ~ Jluel = 0. (3.7) For ease of readability the l is left out in the following equations. According to the subcompon- ~ ents of ue, the Jacobian matrix is split into four submatrices. Finally, the kinematic constraint equation system results in   ~vi h i i j j i  ~ωi  Jlin Jang Jlin Jang   = 0. (3.8)  ~vj  ~ωj.

12 Chapter 3: Numerical Background

The linear parts constrain the linear velocity ~v and the angular parts limit the angular velocity ~ω of both bodies respectively. The Jacobian matrix has 12 columns. The number of rows corresponds to the number of the degrees of freedom the constraint removes from the system. Though it should be completely obvious now, it is nevertheless noteworthy that the joint constraints are restricting the velocities and not the positions of the bodies. Hence, both numerical and roundoff errors might creep in the computations of the positions. During the simulation they are calculated by means of numerical integration, which is quite error prone. In the worst case, the errors could lead to dislocations or even joints that drift apart. This phenomenon is also known as joint error. In Figure 3.1 an example of a dislocated ball-in-

Figure 3.1: Dislocated ball-in-socket joint. socket joint is depicted. The ball and the socket drifted apart. Under worse conditions, they might not even line up on one common axis anymore. To reduce the drifting problem the velocities have to be adapted in every time step. Therefore, ~ an error correction term eb is introduced into the above equation system:

~ ~ Jue = eb. (3.9)

For each joint, an error reduction parameter (ERP) kerp (with 0 ≤ kerp ≤ 1) is introduced. It specifies to what extent the error should be corrected in the next time step ∆t. If kerp = 0, no error correction is performed. Nevertheless, setting kerp to 1 does not correct all errors entirely due to various internal approximations. The authors of the Open Dynamics Engine [ODE09] recommend to use a value between 0.1 and 0.8 for kerp. In order to control the amount of error correction for all joints separately each joint obtains a coefficient kcor, which is defined as kcor = kerpkfps, (3.10) 1 with kfps = ∆t as the measure of the rate of change. In equation (3.9), the correction parameter ~ is contained in the vector eb. Its concrete composition for each joint is derived in Chapter4.

3.4 Limits

So far, everything required to model joints correctly has been introduced except for joint limits. The relative motion of the two rigid bodies is already constrained, however, the extent of the joint is not. A slider joint can move unlimited on its infinite joint axis for example. In this section, merely the generalized concept of joint limits (cf. [Erl05]) is explained. Later on, in Section 4.2, the linear and angular joint limits are explained in detail. Both, the Open Dynamics Engine [ODE09] and the OpenTissue framework [OT09] are based on the same concept of joint limits. The limits are also defined by means of Jacobian matrices. The reachable space Ω of a joint is

13 Chapter 3: Numerical Background defined by an implicit function C (~q (~s)) ∈ R of the generalized joint parameter vector ~q, which itself is a function of the generalized position vector ~s. Angle or displacement are examples for the joint parameter. It has the following properties:  < 0 if ∈/ Ω  C (~q (~s)) = 0 if ∈ ∂Ω , (3.11) > 0 if ∈ Ω where ∂Ω is the boundary of the reachable space Ω. Thus, the positional constraints can be reformulated as C (~q (~s)) ≥ 0. (3.12) Applying the same concept as for unilateral and bilateral constraints, we obtain the kinematic constraints by differentiation w.r.t. time. Moreover, an error term is added to the equation system ~ JC~u ≥ bC . (3.13) The resulting reaction forces are ~ T ~ fC = JC λC , (3.14) ~ where λC is the vector of Lagrange multipliers that have to be strictly positive. In summary, the joint limits augment additional complementarity constraints to the equation system ~ ~ JC~u − bC ≥ 0 ⊥ λC ≥ 0. (3.15) These constraints are only added to the system when a joint limit is violated or the boundary of the allowable configuration space of a joint has been attained. Therefore, in each iteration step it has to be verified whether any limits of the joints have been violated. The collision detection step of the pe works in a completely analogous way.

3.5 Unified Complementarity Notation

Let us now conclude this chapter with a review of all introduced motion constraints of a multibody simulation. In the end, the final boxLCP formulation is obtained with its comple- mentarity conditions on the Lagrange multipliers. Throughout this chapter, joint, contact, and limit constraints have been introduced in the form of the Jacobian matrices

Jjoint~u = ~bjoint, (3.16)

Jcontact~u ≥ ~bcontact, (3.17) ~ Jlimit~u ≥ blimit. (3.18) The resulting constraint forces are obtained by means of the Lagrange multipliers ~ T ~ fjoint = Jjointλjoint, (3.19) ~ T ~ fcontact = Jcontactλcontact, (3.20) ~ T ~ flimit = Jlimitλlimit. (3.21) Following the concept of a Full-Coordinate method, the additional forces are integrated in the generalized acceleration vector to preserve the motion constraints

˙ −1  ~ ~ ~ ~  ~u = M fjoint + fcontact + flimit + fext . (3.22)

14 Chapter 3: Numerical Background

For ease of readability, the Jacobian matrices are concatenated into a single matrix   Jjoint J = Jcontact , (3.23) Jlimit the Lagrange multipliers into a single vector

      −∞ ~λjoint ∞ ~ ≤ ~  ≤ ~  (3.24) λcontact λcontact λcontact ~ 0 λlimit ∞ | {z } ~ | {z } | {z } λ ~λ ~λ and the error correction terms as well into a single vector   ~bjoint ~ ~  berror = bcontact . (3.25) ~ blimit

The matrix and vectors are sorted into the matrix and vector of the boxLCP formulation (3.2). Finally, after the time discretization step, we obtain our final problem formulation to

 −1 T  ~ h  −1 ~  ~ i ~ ~ ~ JM J λe + J ~u + ∆tM fext − berror ≥ 0 ⊥ λ ≤ λe ≤ λ. | {z } (3.26) A | {z } ~b Observe that in the last step we included the time step ∆t into the vector of Lagrange mul- ~ ~ tipliers λe. Thus, actually λe = ∆t~λ. Moreover, the solution is in magnitudes of impulses rather than magnitudes of forces. But in the velocity-based formulation we want to solve for magnitudes of forces. Therefore, we have to be careful not to forget to multiply the solution ~ vector λe with ∆t−1.

15

Chapter 4: Joint Modeling

Chapter 4

Joint Modeling

In this chapter the required Jacobian matrices and error correction vectors of all kinds of joints, modeled within the context of this thesis, are presented. Furthermore, it explains in detail the physical relationships and conditions of each joint. Initially, the different joint types are described profoundly. After that, the characteristic limits of each joint are elucidated.

4.1 Joint Types

In the following, each joint constrains two bodies Bi and Bj. For each joint a Jacobian matrix J is derived. As mentioned in Section 3.3, the Jacobian matrices of the two-body system have the following structure: h i i j j i c×12 J = Jlin Jang Jlin Jang ∈ R , (4.1) where c is the number of DOF the joint removes from the system. Moreover, an error correction c vector ~b ∈ R is computed. The vectors required for the matrix calculations can be represented in global coordinates (world frame WF) or body coordinates w.r.t. the center of mass of the respective rigid body (body frame BF). For the transformation of an arbitrary vector ~x between WF and BF holds

WF BF ~x = ~ri + R(qi)~x , (4.2) where ~ri is the vector to the center of mass of the body Bi, R(qi) is the 3 × 3 rotation matrix deduced from the given quaternion qi. The transformation from BF to WF is defined in analogous manner by BF T WF ~x = R(qi) (~x − ~ri). (4.3) It is slightly different when one considers the transformation of axes. The vectors to the center of mass must be omitted. Thus, the following two relationships hold:

−−→ WF −−→ BF axis = R(qi) axis , (4.4) −−→ BF T −−→ WF axis = R(qi) axis . (4.5) For ease of readability, we omit the WFs for every vector and axis in the global coordinate system throughout the rest of this chapter.

× T Moreover, the operation ~a of an arbitrary vector ~a = [a1, a2, a3] is defined as   0 −a3 a2 × ~a =  a3 0 −a1 . (4.6) −a2 a1 0

17 Chapter 4: Joint Modeling

The illustration starts with the fixed joint, removing all DOF, and gradually loosen the con- straints to allow the rigid bodies more movement. The implementation in the pe framework allows to attach all joints to arbitrary anchor points of the rigid bodies. In order to ease the readability throughout the following calculations, all joints are attached to the centers of mass of the rigid bodies. If they should be attached to arbitrary anchor points, one has to replace the vectors to the center of mass (~ri and ~rj) by the vectors to these anchor points.

4.1.1 Fixed Joint A fixed joint, shown in Figure 4.1, restrains two rigid bodies completely from any relative motion. Thus, it removes all six DOF from both bodies and the Jacobian matrix of the fixed joint results in h i i j j i 6×12 Jfixed = Jlin Jang Jlin Jang ∈ R . (4.7)

Figure 4.1: A fixed joint.

Figure 4.2: Sketch of a fixed joint w.r.t. the global coordinate system.

Figure 4.2 shows the relevant vectors for the calculation of the Jacobian submatrices. In some respects, the fixed joint just “glues” two rigid bodies together with a constant distance between. Thus, the two connected rigid bodies must have the same angular velocity (~ωi = ~ωj). Moreover, the following condition must hold for the linear velocities:

× ~vj = ~vi + ~ωi ~roff × (4.8) = ~vi − ~roff ~ωi, where ~roff = ~rj − ~ri. (4.9)

18 Chapter 4: Joint Modeling

Consequently, the four submatrices result in 1 0 0   × 0 1 0  −~r     off  i 0 0 1 i   J =   , J =   , lin 0 0 0 ang 1 0 0     0 0 0 0 1 0 0 0 0 0 0 1 (4.10) −1 0 0   0 0 0   0 −1 0   0 0 0      j  0 0 −1 j  0 0 0  J =   , J =   . lin  0 0 0  ang −1 0 0       0 0 0   0 −1 0  0 0 0 0 0 −1 The first three rows constrain the translational movement and the last three rows constrain the rotational movement. As already shown in Figure 4.2, the midpoint between the two bodies is introduced for the −−→BF −−→BF error term. The vectors midi and midj are stored in terms of the according BFs. The fixed joint has no positional error if

−−→ BF −−→ BF ~ri + R(qi)midi = ~rj + R(qj)midj (4.11) holds.

Additionally, three rows to correct any rotational errors are required. Let the angle Θerr be the relative error in radians. In order to correct this error by Θcor radians within the time step ∆t, a correcting angular velocity ~ωcor of magnitude Θ k Θ k~ω k = cor = erp err = k k Θ = k Θ (4.12) cor ∆t ∆t erp fps err cor err is needed. The direction of this correcting angular velocity is given by a rotation axis, i.e. a unit vector ~xaxis. Thus, holds

~ωcor = k~ωcork~xaxis = kcorΘerr~xaxis. (4.13)

The angle Θerr is calculated from the vector part ~v of the quaternion  Θ  Θ   q = (r, i, j, k) = (r,~v) = cos err , sin err ~x . (4.14) err 2 2 axis

−1 First of all, the initial relative orientation qini = qj qi is calculated once when the fixed joint is set up initially. During the simulation, the current orientation qcur of the two bodies is given by −1 qcur = qj qi. (4.15) Thus, the rotational error amounts to

−1 qerr = qini qcur. (4.16) The error is expected to be small. Therefore, the small angle approximation holds: Θ  Θ sin err ~x ≈ err ~x = ~v. (4.17) 2 axis 2 axis

19 Chapter 4: Joint Modeling

After inserting this into equation (4.13), we obtain

~ωcor = kcor2~v. (4.18)

Finally, the complete error term of the fixed joint results in "−−→ −−→ # mid − mid ~b = k j i ∈ 6. (4.19) fixed cor 2~v R

4.1.2 Slider Joint A slider joint, depicted in Figure 4.3, removes five DOF from the free floating rigid body. Thus, the Jacobian matrix results in

h i i j j i 5×12 Jslider = Jlin Jang Jlin Jang ∈ R . (4.20)

Figure 4.3: A slider joint.

Figure 4.4: Sketch of a slider joint w.r.t. the global coordinate system.

The slider joint is described by three unit vectors: ~t1, ~t2, and ~xaxis. All of them are orthogonal to each other and span a local coordinate frame, as shown in Figure 4.4. The basis of this coordinate system does not necessarily have to be right-handed.

The two rigid bodies Bi and Bj can only perform a translational movement along the axis ~xaxis. They do not rotate relative to each other and, hence, have identical angular velocity, i.e. ~ωi = ~ωj.

20 Chapter 4: Joint Modeling

For the linear velocities, the following condition must hold

~vj = ~vi + ~ωi × ~c + ~vslider, (4.21) where ~c = ~rj − ~ri is the current distance between the two rigid bodies during the simulation, and ~vslider the joint velocity along the slider axis. With some straightforward computations, we receive

−~vslider = ~vi − ~vj + ~ωi × ~c ~ω + ~ω (4.22) = ~v − ~v + i j × ~c. i j 2

In case of a slider joint, no relative velocities occur in the directions of both vectors ~t1 and ~t2. Hence, the two rows restricting the translational movement to only one axis result from the conditions

~ T 0 = t1 (−~vslider), (4.23) ~ T 0 = t2 (−~vslider). (4.24)

The rows constraining the rotational movement can be adopted from the fixed joint. Finally, the Jacobian submatrices of the slider joint result in

0 0 0 1 0 0 0 0 0 0 1 0 i   i   Jlin = 0 0 0 , Jang = 0 0 1 ,  ~ T   1 ~ T   (t1)   ( 2~c × t1)  ~ T 1 ~ T (t2) ( 2~c × t2) (4.25) 0 0 0 −1 0 0  0 0 0  0 −1 0  j   j   Jlin = 0 0 0 , Jang =  0 0 −1 .  ~ T   1 ~ T   −(t1)   ( 2~c × t1)  ~ T 1 ~ T −(t2) ( 2~c × t2) Here, the first three rows constrain the rotational movement and the last two rows the trans- lational movement. For the error vector of the slider joint, one can reuse the term 2~v from the rotational error correction of the fixed joint. Furthermore, the slider will have no translational error if the vector ~ h = ~c − ~roff (4.26) BF will have no components orthogonal to ~xaxis. The vector ~roff = ~rj − ~ri is computed once when the joint is set up initially. This offset vector has to be transformed into the world frame via BF ~roff = R(qi)~roff . Finally, the error term of the slider joint results in

 2~v  ~ ~ T~ 5 bslider = kcor t1 h ∈ R . (4.27) ~ T~ t2 h

21 Chapter 4: Joint Modeling

4.1.3 Hinge Joint In case of a hinge joint, shown in Figure 4.5, only relative rotation around one defined axis is allowed. Hence five DOF are removed and one rotational DOF remains. Consequently, the Jacobian matrix for the hinge results in

h i i j j i 5×12 Jhinge = Jlin Jang Jlin Jang ∈ R . (4.28)

Figure 4.5: A hinge joint.

Figure 4.6: Sketch of a hinge joint w.r.t. the global coordinate system. −→ A hinge joint is specified by means of a pivotal point rot, located on an axis of rotation ~xaxis. The latter has to be a unit vector in the global coordinate system. Additionally, two orthogonal unit vectors ~t1 and ~t2 are defined, which both also have to be orthogonal to the hinge’s axis of rotation. As in the case of a slider joint, all three unit vectors form a coordinate system, located at the pivotal point. It does not necessarily have to be right-handed. All geometric correlations, concerning the Jacobian matrix of the hinge joint, are shown in Figure 4.6. The first three rows of the Jacobian matrix constrain the translational motion of the hinge. No translational DOF are allowed. Thus, the position of the pivotal point w.r.t. both rigid bodies has to be identical. Hence, the following condition must hold: −→ −→ roti = rotj, (4.29) −→BF −→BF ~ri + R(qi)roti = ~rj + R(qj)rotj . In order to obtain a kinematic constraint, the time derivative has to be taken:

d  −→BF d  −→BF ~ri + R(qi)roti = ~rj + R(qj)rotj , dt dt (4.30) d  −→ d  −→ ~v + R(q ) rotBF = ~v + R(q ) rotBF. i dt i i j dt j j

22 Chapter 4: Joint Modeling

The time derivative of the rotation matrix R(q), given by three unit vectors ~u1, ~u2, and ~u3, is defined as (cf. [Smi02])

d R =  d ~u d ~u d ~u  = ~ω × ~u ~ω × ~u ~ω × ~u  = ~ω × R = ~ω×R. (4.31) dt dt 1 dt 2 dt 3 1 2 3 Finally, the first three rows of the Jacobian matrix result in

× −→BF × −→BF ~vi + ~ωi R(qi)roti = ~vj + ~ωj R(qj)rotj , × × (4.32)  −→BF  −→BF ~vi − R(qi)roti ~ωi = ~vj − R(qj)rotj ~ωj.

The last two rows constrain the rotational movement. Only rotations around the hinge’s axis of rotation are allowed, i.e. T ~xaxis(~ωi − ~ωj) 6= 0. (4.33)

Rotations around any other axis, orthogonal to ~xaxis, are not allowed. Thus, the angular velocity around the rest of the axes of the local coordinate system in the pivotal point has to be zero. Hence, the following conditions must hold:

~ T t1 (~ωi − ~ωj) = 0, (4.34) ~ T t2 (~ωi − ~ωj) = 0. (4.35)

In conclusion, the four submatrices of the local Jacobian matrix of the hinge joint are obtained to 1 0 0 ×   −→BF  0 1 0 − R(qi)roti i   i J = 0 0 1 , J =  ~ T  , lin   ang  (t1)  0 0 0 T   (~t2) 0 0 0 (4.36) −1 0 0  ×   −→BF   0 −1 0  R(qj)rotj j   j J =  0 0 −1 , J =  ~ T  . lin   ang  −(t1)  0 0 0 T   −(~t2) 0 0 0

Let us now derive the error correction parameter of the hinge joint. In order to obtain no positional errors, equation (4.29) has to be fulfilled. Additionally, two measures for the remaining two DOF of the rotational errors are required. The hinge’s axis of rotation is stored in both BFs. Thus, its position can be computed in the WF w.r.t. both bodies. If the global positions are equal, then the two bodies are not rotationally misaligned. If not, they are dislocated by an angle of Θerr radians, measured around the axis

~u = ~xaxisi × ~xaxisj . (4.37)

The following computations are analogous to the ones of the error term of the rotational misalignment of the fixed joint in Section 4.1.1. Therefore, some of the intermediate steps are skipped.

In order to fix the rotational error, a correcting angular velocity ~ωcor with magnitude

k~ωcork = kcorΘerr. (4.38)

23 Chapter 4: Joint Modeling is required and will be applied in the direction of the previously calculated vector ~u. Thus, follows ~u ~u ~ω = k~ω k = k Θ . (4.39) cor cor k~uk cor err k~uk

Due to the fact that k~uk = sinΘerr and the small angle theorem, sinΘerr ≈ Θerr is obtained. Therefore, one can deduce ~ωcor = kcor~u. (4.40) Thus, the final error correction vector of the hinge joint results in −→ −→  rotj − roti ~ ~ T 5 bhinge = kcor  t1 ~u  ∈ R . (4.41) ~ T t2 ~u

4.1.4 Contact Joint

Figure 4.7: A contact joint.

Actually, a contact point, as shown in Figure 4.7, is not a real joint like the previously mentioned ones. But as shown in Section 2.2.3, the contact constraints have to be remodeled with Jacobian matrices to obtain the unified complementarity system. Furthermore, instead of using η direction vectors it is sufficient to model the friction cone with two direction vectors, namely the tangents in x- and y-direction, ~tx and ~ty, respectively. Thus, the constraint Jacobian of a contact point is given by

h i i j j i 3×12 Jcontact = Jlin Jang Jlin Jang ∈ R . (4.42)

Here, the first row of the matrix constrains the normal force as the last two rows constrain the tangential friction forces. In Figure 4.8 the geometric correlations of a contact point are depicted. The derivation of the submatrices is analogous to the one of the hinge joint in Section 4.1.3. Therefore, the first straightforward computations are skipped and the kinematic constraints are stated instantaneously. It must hold that the linear and angular velocities along the direction of the

24 Chapter 4: Joint Modeling

Figure 4.8: Sketch of a contact point w.r.t. the global coordinate system.

normal ~n and both tangents ~tx and ~ty w.r.t. body Bi must be the same as w.r.t. body Bj. Consequently, the four submatrices have the following composition:  T   × T  ~n (ai ~n) i ~ T i ×~ T Jlin = tx  , Jang = (ai tx)  , ~ T ×~ T ty (ai ty) (4.43)  T   × T  −~n −(aj ~n) j ~ T j ×~ T Jlin = −tx  , Jang = −(aj tx)  . ~ T ×~ T −ty −(aj ty) There is no real joint error for contacts. The only problem that might occur for contacts is over- lapping. Thus, the error correction vector only consists of the penetration depth dpenetration, which can only occur in direction of the contact normal:   −dpenetration ~ 3 bcontact = kcor  0  ∈ R . (4.44) 0 Moreover, handling impacts can be easily integrated into the formulation by adding the vector ~ bbounce to the contact subequation of the k-th contact: ~  T 3 bbounce = −e vreln , 0, 0 ∈ R , (4.45) where vreln is the relative velocity in direction of the contact normal. According to Newton’s Impact Law [GHS02, section 1.2.5] the coefficient of restitution has to fulfill 0 ≤ e ≤ 1. The border line case e = 0 stands for a perfectly plastic, or inelastic, collision while e = 1 stands for a perfectly elastic collision. The values inbetween the border line cases stand for partially elastic collisions.

4.2 Joint Limits

All of the previously introduced joints, except for the fixed and contact joint, are moving infinitely within their constrained motion paths. In this section, two different kinds of joint limits are derived (cf. [Erl05, section 4.10]). As already mentioned in Section 3.4, they are modeled by means of two Jacobian matrices

h i i j j i 1×12 Jlow = Jlin Jang Jlin Jang ∈ R , (4.46)

25 Chapter 4: Joint Modeling

h i i j j i 1×12 Jhigh = Jlin Jang Jlin Jang ∈ R , (4.47) and two error correction terms blow, bhigh of the upper and lower limits of the joint motion respectively.

4.2.1 Linear Limits As the name already implies, linear joint limits prevent the linear movement from being infin- ite. They are applied in case of a slider joint for instance. The movement is limited inbetween a minimum length lmin and a maximum length lmax on the slider axis ~xaxis. Therefore, the following condition, concerning the current length lcur, must always be satisfied:

lmin < lcur < lmax. (4.48) If one of the two requirements is violated, a new row will be added to the Jacobian matrix of the limit constraints Jlimit. In the following, the case that lmin ≤ lcur is violated will be slider considered, and thus Jlow calculated. The linear limits establish that the relative velocity along the joint axis does not force the slider joint to violate its upper or lower limit. Thus, for the linear velocities the following condition must hold: T ~xaxis(~vj − ~vi) ≥ 0. (4.49) Unfortunately, these limit forces produce torques. Therefore, additional constraints on the angular velocities are required.

i j First of all, let us denote ~plim and ~plim as the vectors from the respective centers of mass to ~ i the positions of the joint limit. Then, a limiting force flim acting at ~plim, which also means ~ j −flim at ~plim (“actio = reactio”, cf. [GHS02, p.34]), is required to be parallel to the joint axis ~xaxis. Both of them cause the torques i i ~ ~τ = ~p × flim, lim lim (4.50) j j ~ ~τlim = −~plim × flim. One has to observe that the angular momentum remains unchanged. This implies that i j Jang~ωi + Jang~ωj = 0 (4.51) slider must hold. Thus, the Jacobian submatrices of the lower limit Jlow result in i
T Jlin = − ~xaxis , i i
T J = ~p × ~xaxis , ang lim (4.52) j
T Jlin = ~xaxis , j j
T Jang = − ~plim × ~xaxis . j i The current offset vector ~c = ~rj −~ri of the slider joint can be rewritten to ~c = ~plim −~plim. After inserting ~c into equation (4.50) and some straightforward computations (cf. [Erl05, section slider 4.10.1]), we gain the final Jacobian submatrices of the lower limit Jlow : i
T Jlin = − ~xaxis , i 1
T Jang = ~c × ~xaxis , 2 (4.53) j
T Jlin = ~xaxis , 1 Jj = − ~c × ~x
T . ang 2 axis

26 Chapter 4: Joint Modeling

The error correction term of the lower limit results in

low bslider = kcor(lmin − lcur). (4.54)

slider slider Concerning the upper limit, it should be obvious that Jhigh = −Jlow . Furthermore, the error correction term of the upper limit results in

high bslider = kcor(lcur − lmax). (4.55)

4.2.2 Angular Limits If we have an unconstrained rotational movement, angular joint limits prevent it from an infinite rotation around an axis. For instance, they are applied in case of a hinge joint. The minor difference between setting linear and angular joint limits is simply the measures. In case of linear joint limits, distances are used and in case of angular joint limits, angles in radian measure are used.

The movement of a hinge is limited by means of a minimum and maximum angle, Θmin and Θmax respectively. The following condition must always be satisfied:

Θmin < Θcur < Θmax. (4.56)

hinge Let us start with the easier part of the limits: the derivation of the Jacobian matrices Jlow hinge and Jhigh .

In the following, consider the case in which the lower joint limit is violated, i.e. Θcur ≤ Θmin. hinge Thus, Jlow is calculated. It has to be ensured that the following condition is satisfied: T ~xaxis(~ωi − ~ωj) ≥ 0. (4.57)

hinge Hence, the Jacobian submatrices of the lower limit Jlow result in i ~ Jlin = 0, i T Jang = ~xaxis, (4.58) j ~ Jlin = 0, j T Jang = −~xaxis. The error correction term of the lower limit results in

low bhinge = kcor(Θmin − Θcur). (4.59)

hinge hinge Jhigh = −Jlow does apply, completely analogous to the linear joint limits. Moreover, the error correction term of the upper limit is:

high bhinge = kcor(Θcur − Θmax). (4.60) Let us now regard the more complex part of the angular joint limits: the determination of the angle. In general, an angle Θ can be determined by means of quaternions. In case of a hinge, consider body Bi to be “fixed”. Thus, body Bj will move relative to body Bi. This means that the angle has to be calculated w.r.t. the hinge’s axis of rotation in body coordinates w.r.t. body B , i.e. ~x BF . The sign convention of Θ is shown in Figure 4.9. The calculation of j axisj the relative orientation qrel is analogous to the calculations, needed for the error orientation of −1 the fixed joint, in Section 4.1.1. The initial orientation qini = qj qi is calculated once, when

27 Chapter 4: Joint Modeling

Figure 4.9: Sign convention of the aperture angle of a hinge joint.

−1 the hinge is set up initially. The current orientation qcur = qj qi of the two bodies is computed throughout the simulation. Hence, the relative orientation of body Bj to Bi, i.e. how much the BF of Bj has rotated w.r.t. the BF of Bi, is computed by means of  Θ  Θ   q = q−1q = (r, i, j, k) = (r,~v) = cos cur , sin cur ~u . (4.61) rel ini cur 2 2 axis

Now, the angle has to be extracted from the quaternion qrel. This is done in the following way Θ
(cf. [OT09]). For the angle calculation, the quaternion is split into scalar part s = cos 2 Θ
and vector part |~v| = |sin 2 |. The problem which occurs with quaternions is that their representation is not unique:  Θ Θ    Θ  Θ  q = cos , sin ~u = cos − , sin − ~u 2 2 axis 2 2 axis (4.62)  Θ Θ  = cos , −sin ~u = −q. 2 2 axis In order to solve this problem, the angle will be determined via the std::atan2 function. It y has two arguments y and x and calculates the arc tangent of x . Both signs of the arguments are used to determine the quadrant of the angle. A positive sign for the x argument is used, if the axis of rotation ~x BF and the axis ~u of the vector part ~v of the quaternion point in axisj axis the same direction, and a negative sign otherwise. This determination is done by means of a scalar product. In summary, we have:  2 · atan2 y
if ~v T ~x BF
≥ 0  x axisj Θ =  y  (4.63) 2 · atan2 −x otherwise.

The angles obtained are in the interval [0, 2π] radians but should be inbetween (−π, π). Thus, if the angle obtained is larger than π, 2π is subtracted. Moreover, it should be measured how much body Bj has rotated relative to body Bi. Instead, a rotation matrix is given, which transforms the coordinates. Therefore, the sign of the angle has to be inverted additionally. These calculations will provide us with the correct angle, as long as the initial position of the hinge corresponds to an angle of zero radians. Since it is possible to set an arbitrary pivotal point for the hinge, the initial angle does not necessarily have to be zero. Furthermore, an initial angle Θini 6= 0 does not necessarily imply qini 6= 0 because the orientations of the two rigid bodies will not be changed if a new pivotal point is set. Consequently, the computation of the initial angle has to be independent of the quaternions.

28 Chapter 4: Joint Modeling

Figure 4.10: An arbitrary initial angle of a hinge joint.

In the following, it is explained how the functionality of handling arbitrary initial angles of a hinge joint in 3D, like the one shown in Figure 4.10, is integrated into the pe framework. The method is based on a coordinate transformation followed by a projection into the plane space orthogonal to the hinge’s axis of rotation. Figure 4.11 depicts the relevant steps in consecutive order. The vectors ~vi and ~vj represent the vectors from the respective centers of mass of the two rigid bodies to the pivotal point. First of all, a new hinge coordinate system (HCS) (x0,y0,z0) is set up, which has its origin in the pivotal point of the hinge joint. Moreover, the y0-axis always corresponds to the hinge’s axis of rotation. The new HCS is obtained by means of a translation by the vector ~b and a rotation by the 3 × 3 matrix   D = ~ux0 ~uy0 ~uz0 , (4.64) where −→ ~ux0 = rot − ~ri = −~vi, (4.65)

~uy0 = ~xaxis, (4.66)

~uz0 = ~ux0 × ~uy0 . (4.67)

Observe that the three vectors have to be normalized to unit vectors. Figure 4.11(a) shows −→ that the translation by the vector ~b is done by means of subtracting ~b = rot from the vectors 0 ~vi and ~vj. For instance, the vector ~vj given in the WCS is transformed to the point ~v j in the HCS by means of 0 T −→ ~v j = D ~vj − rot. (4.68) Figure 4.11(b) depicts the rotation of the coordinate system. This has already been done by 0 setting the x -axis of the HCS to the normalized vector −~vi. 0 After transforming both vectors into the new coordinate system, a projection of ~v j along the normal of the plane space orthogonal to the hinge’s axis of rotation has to be performed. The plane space is given by the x0-z0 plane of the new HCS. The projection along the normal is 0 0 00 done by setting the y -component of ~v j to zero, resulting in the vector ~v j . In order to obtain the initial aperture angle of the hinge, one has to take the arc cosine of the scalar product 00T 0
~v j ~x , as shown in Figure 4.11(c). Additionally, one has to consider the following cases of the angle Θ in order to receive the correct initial aperture angle:

 0 0 −Θ if ~v i < ~v j  x x −Θ if ~v 00T ~x 0
= 0 and ~v 00 = −1 Θ = j jz (4.69)  0 if Θ = π   Θ otherwise.

29 Chapter 4: Joint Modeling

(a) Translation.

(b) Rotation.

(c) Projection into the plane space.

Figure 4.11: Calculation of an arbitrary initial angle of a hinge joint.

Finally, the correct initial hinge angle is obtained. If it differs from zero, this angle is added to the angle resulting from the quaternion qrel, representing the relative orientation between the two rigid bodies during the simulation. This guarantees that the angular joint limits will work correctly.

30 Chapter 5: Implementation

Chapter 5

Implementation

This chapter presents the main parts of the algorithmic implementation. First, the details of the data storage, allocation and insertion are considered. Here, the relevant parts are explained and underlined with several code examples. Afterwards, the implemented class design is demonstrated, and finally the enhancements made to the special kind of visualization used are explained.

5.1 Data Preparation

Figure 5.1 depicts the call sequence of the collision response module. This section elucidates how the allocation and data insertion of the complementarity system has been restructured in order to handle joints. The following two sections focus on the static function construct of the ConstraintConstructorDriver. The relevant parts of this method will be explained in detail and the illustrations will be underlined with several code snippets.

Figure 5.1: Hierarchy of the collision response module.

31 Chapter 5: Implementation

5.1.1 Data Storage and Allocation First of all, let us recall the matrix and right-hand side of the final boxLCP (3.26) of Section 3.5: A = JM−1JT  −1  ~b = J ~u + ∆tM f~ext −~berror (5.1) | {z } ~btemp

The data members, which have to be allocated for matrix A are

SMat A_, SMat Minv_, SMat J_, (5.2)

and for vector ~b are

VecN b_, VecN b_temp_. (5.3)

Additionally, the solution vector ~x and its upper and lower bounds, ~xmin and ~xmax respectively are required:

VecN x_, VecN x_min_, VecN x_max_. (5.4)

The SMat datastructure of the pe framework is used for the matrices. It is a very efficient implementation, which is based on a compressed row storage format but even allows to insert values into each row afterwards. It can be used for every M × N sparse matrix. First of all, the rows and columns of the data members need to be calculated. Algorithm 5.1 presents the determination of these parameters.

1 ... 2 // Setting the number of columns of the system matrix 3 const std::size_t jacCols ( 6 * bodies.size() ); 4 5 std::size_t contactRows ( contacts[0]->getRows() * contacts.size() ); 6 7 for( std::size_t i = 0; i < joints.size(); ++i ){ 8 jointRows += joints[i]->getRows(); 9 10 if ( joints[i]->isViolatedHigh() ){ 11 ++limitRows; 12 } 13 14 if ( joints[i]->isViolatedLow() ){ 15 ++limitRows; 16 } 17 } 18 19 // Setting the number of rows of the system matrix 20 const std::size_t jacRows ( contactRows + jointRows + limitRows ); 21 22 // Setting the number of elements which should be allocated in advance 23 const std::vector< std::size_t > cap ( jacRows, 12 ); 24 ... Algorithm 5.1: Determination of the rows and columns of the data members of the boxLCP.

32 Chapter 5: Implementation

The Jacobian columns are always six times the size of all rigid bodies in the simulation. In order to obtain the Jacobian rows, the rows of all contacts are first determined. As mentioned in Section 4.1.4, a fixed number of three rows per contact is used. Nevertheless, a getRows() operation is performed on the first element of the contacts container, which contains all con- tacts of the rigid body simulation, in order to keep the code modular for further extensions of the approximation of the friction cone. After that, an iteration over the joints container, which holds all joints of the rigid body simulation, is performed. In each iteration loop, the rows of the specific joint are ascertained and added to the Jacobian rows. Moreover, it is verified whether any limits of the joints have been violated and the Jacobian rows are incremented if necessary. Recall from Section 4.1 that each row in the matrix contains a maximum number of 12 non-zero entries. For the allocation, a vector cap of size jacRows is required, which contains the number of non-zero entries for each row. Since the number of rows and columns has now been determined, the allocate method of the BoxFrictionalConstraintConstructor, shown in Algorithm 5.2, can be called. The matrix data members of the boxLCP are allocated according to the determined rows, columns, and capacity vector cap. For the capacity of the inverse of the mass matrix an additional tem- porary vector capM is needed. Furthermore, the vectors of the boxLCP are allocated via the resize function, which resizes the N-dimensional vector to a vector of dimension jacRows but performs no element initialization. Observe that the data members A_ and b_ of the system have not been allocated yet. This will be done automatically in the last step of the data insertion, which will be explained in the next section.

1 template< typenameD> 2 inline void BoxFrictionalConstraintConstructor:: 3 allocate ( const std::size_t rows, const std::size_t cols, 4 const std::vector& cap ){ 5 std::vector< std::size_t > capM ( cols ); 6 7 for( std::size_t i = 0; i < cols; i += 6 ){ 8 capM[i] = capM[i+1] = capM[i+2] = 1; 9 capM[i+3] = capM[i+4] = capM[i+5] = 3; 10 } 11 12 data_.Inv_ = SMat( cols, cols, capM); 13 data_.J_ = SMat( rows, cols, cap ); 14 data_.b_.resize( rows, false); 15 data_.x_.resize( rows, false); 16 b_temp_.resize ( cols, false); 17 } Algorithm 5.2: Allocation of the data members of the boxLCP.

5.1.2 Data Insertion After the allocation of the data members of the boxLCP the corresponding data has to be inserted into the system. First of all, the insertion into the matrices is considered. In general, the BoxFrictionalConstraintConstructor class offers three different methods to insert values into a SMat matrix:

• void insertMatrices(std::size_t b1, std::size_t b2, std::size_t start, std::size_t rows, const MatMN& JL1, const MatMN& JA1, const MatMN& JL2, const MatMN& JA2 );

33 Chapter 5: Implementation

• void insertMatrixDiag( std::size_t i, const Mat3& mat );

• void insertValueDiag ( std::size_t i, const real val ); All of these functions operate on an SMat data structure. To insert a value into the matrix, they call the appendRowElement( std::size_t m, std::size_t n, const Type &value ) function, which appends an element to the m-th row in the n-th column. The only restrictions on the value of this function are that its column index has to be larger than the column index of the previously inserted element, and that there has to be enough reserved space in the row. Algorithm 5.3 shows an extract of the construct method of the Jacobian matrix J. In this part of the code the four local Jacobian matrices of the joints and joint limits are calculated and inserted into the global Jacobian matrix J. By means of the example scenario in Figure 5.2, the code sequence will be explained in detail.

Figure 5.2: Example scenario.

Figure 5.3: Global Jacobian matrix of the example scenario.

First of all, one has to iterate over the joints container. It contains the slider joint between body 2 and body 0. In the first step during the loop, the local Jacobian matrices of the slider joint are calculated. Afterwards, one has to compute the indices i_b1 and i_b2, which specify the start column of the matrices of body 1 and 2 respectively. They are computed via the function getMID(). During the setup of a rigid body simulation, the user will assign every rigid body a user ID. Unfortunately, this ID neither has to be unique, nor does it have to be consecutive numbers in ascending order. Therefore, a matrix ID (MID) is introduced. Before the collision handling, one has to iterate over the bodies container, which contains all rigid bodies of the simulation, and each of them is assigned a unique consecutive number beginning with 0 (cf. Figure 5.2). Thus, i_b1=12 is obtained and i_b2=0. A slider joint constrains five DOF, hence, rows=5. With these indices we end up in the else case of the J_joint insertion and write the matrices, as shown in Figure 5.3, into the first five rows of the global Jacobian matrix. Moreover, the slider joint is at its upper limit. The total number of joint and contact rows has already been calculated . So, instead of iterating twice over the joints container, the limit is inserted along with the joint rows in the same iteration loop. The function isViolatedHigh() returns true and, thus, the limit matrices of the upper limit of the slider joint are inserted in row eight of the global Jacobian matrix.

34 Chapter 5: Implementation

The relevant code parts for the insertion of the contacts and the inverse of the mass matrix are omitted since this is done in completely analogous manner.

1 ... 2 // ctor is is an object of the BoxFrictionalConstraintConstructor 3 // with configuration type BoxLcp 4 std::size_t start ( 0 ), rows ( 0 ); 5 std::size_t rowL ( jointRows + contactRows ); 6 7 for( std::size_t k = 0; k < joints.size(); ++k ){ 8 9 MatMN Lin1, Ang1, Lin2, Ang2; 10 joints[k]->calcMat( Lin1, Ang1, Lin2, Ang2 ); 11 12 std::size_t i_b1 ( 6 * joints[k]->getBody1()->getMID() ); 13 std::size_t i_b2 ( 6 * joints[k]->getBody2()->getMID() ); 14 std::size_t rows ( joints[k]->getRows() ); 15 start = temp; 16 temp += rows; 17 18 // Insert J_joint 19 if( i_b1 > i_b2 ){ 20 ctor.insertMatrices( i_b2, i_b1, start, rows, Lin2, Ang2, Lin1, Ang1 ); 21 }else{ 22 ctor.insertMatrices( i_b1, i_b2, start, rows, Lin1, Ang1, Lin2, Ang2 ); 23 } 24 25 // Insert J_limit_high 26 if( joints[k]->isViolatedHigh() ){ 27 28 // J_limit_high=- J_limit_low: 29 // J_Lin1_high=-J_Lin2_high, J_Ang1_high=-J_Ang2_high 30 // function calculates J_limit_low, but just switch the matrix 31 // parameters and reuse the calcLimitMat function for J_limit_high 32 joints[k]->calcLimitMat( Lin2, Ang2, Lin1, Ang1 ); 33 34 if( i_b1 > i_b2 ){ 35 ctor.insertMatrices( i_b2, i_b1, rowL, 1, Lin2, Ang2, Lin1, Ang1 ); 36 }else{ 37 ctor.insertMatrices( i_b1, i_b2, rowL, 1, Lin1, Ang1, Lin2, Ang2 ); 38 } 39 ++ rowL ; 40 } 41 42 // Insert J_limit_low 43 if( joints[k]->isViolatedLow() ){ 44 45 joints[k]->calcLimitMat( Lin1, Ang1, Lin2, Ang2 ); 46 47 if( i_b1 > i_b2 ){ 48 ctor.insertMatrices( i_b2, i_b1, rowL, 1, Lin2, Ang2, Lin1, Ang1 ); 49 }else{ 50 ctor.insertMatrices( i_b1, i_b2, rowL, 1, Lin1, Ang1, Lin2, Ang2 ); 51 } 52 ++ rowL ; 53 } 54 } 55 ... Algorithm 5.3: Data insertion in the matrix J of the boxLCP.

35 Chapter 5: Implementation

Now, let us consider the vectors of the boxLCP. First, the values are inserted into the vector b_temp_. Each rigid body has six rows. The first three contain the linear parts of their velocities and external forces, and the last three the angular parts respectively. Algorithm 5.4 shows a code snippet of the insertion of the linear (v) and angular parts (w) into the vector b_temp_ of the construct method. In case of a fixed rigid body, both insertion steps are skipped.

1 ... 2 for( std::size_t i=0; i< bodies.size(); ++i ){ 3 4 std::size_t index ( 6 * bodies[i]->getMID() ); 5 ConstBodyID b (bodies[i]); 6 Vec3 temp ( 0.0 ); 7 8 //v 9 if( !b->isFixed() ){ 10 11 temp = b->getLinearVel() + timestep * ( b->getInvMass() 12 * b->getForce() + Settings::gravity() ); 13 } 14 15 ctor.insertVector( index, temp ); 16 17 //w 18 if ( !b->isFixed() ){ 19 20 temp = b->getAngularVel() + timestep * ( b->getInvInertia() * 21 ( b->getTorque() + ( b->getInertia() * b->getAngularVel() ) 22 b->getAngularVel() ) ); 23 } 24 ctor.insertVector( index + 3, temp ); 25 } 26 ...

Algorithm 5.4: Data insertion in the vector btemp of the boxLCP.

So far, data has been inserted in all of the data members, except for A_, x_, and b_, which represent the main system of complementarity constraints of the boxLCP. As mentioned, no memory has been allocated for A_ and b_ as well. Both steps are done in the postprocessing method of the BoxFrictionalConstraintConstructor, shown in Algorithm 5.5. The matrix A_ is composed of its relevant matrices J_ and Minv_. Furthermore, the vector b_temp_ is multiplied by the Jacobian matrix J_. In both cases the assignment operator adjusts the rows and columns for the left-hand side matrix and vector automatically. Moreover, the solution vector x_ is set to zero.

In the final step of the system construction, the vector ~berror has to be subtracted from b_ in order to obtain the final right-hand side. As an example, consider Algorithm 5.6, which shows the insertion of the error correction vector of the contacts. An iteration over the contacts container is performed, the error term is calculated and then inserted via the insertErrVector function. In this function, a threshold for the components of the error correction vector is introduced. Due to the fact that the error correction does not stop, even if the bodies are not moving anymore, the solver might perform too many unnecessary iterations. Therefore, if the absolute value of an error component is smaller than this threshold, its insertion will be skipped. This is done analogously for the error correction of joints and joint limits.

36 Chapter 5: Implementation

1 template< typenameD> 2 inline void BoxFrictionalConstraintConstructor::postprocess( ) 3 { 4 //|A|=|J|*|M|^-1|J |^T 5 data_.A_ = data_.J_ * data_.Inv_ * data_.J_.getTranspose(); 6 7 //b=|J|* b_temp 8 data_.b_ = data_.J_ * b_temp_; 9 10 data_.x_ = real( 0 ); 11 } Algorithm 5.5: Postprocessing step.

1 ... 2 std::size_t errRows ( 0 ); 3 std::size_t insRows ( jointRows ); 4 5 // error term for contacts 6 for( std::size_t i = 0; i < contacts.size(); ++i ){ 7 8 VecN error ; 9 contacts[i]->calcErrorVec ( error ); 10 11 insRows += errRows; 12 errRows = contacts[i]->getRows(); 13 14 ctor.insertErrVector( insRows, error, errRows ); 15 } 16 ...

Algorithm 5.6: Subtraction of ~berror.

5.2 Class Design

This thesis focuses on the development of a suitable extension of the pe framework in order to integrate joints. The main emphasis lies on modularity and an effortless addition of further joint types in the future. Therefore, suitable data structures are a necessity. The implemented class design is shown in Figure 5.4. It only shows the functions relevant to the object-oriented integration of joints. The Constraint class serves as an abstract base class for all types of constraints. There are two classes derived from this base class: Contact and Joint. The Contact class is only augmented with the functions from the base class. On the one hand, this is done with the purpose to achieve a uniform treatment of contacts and joints in our new approach. On the other, changes to the already existing Contact class, coordinated with the collision detection module of the pe, should be kept to a minimum. The Joint class is a newly developed general base class for all kinds of joints. It provides all necessary functions and member variables, which all types of joints have in common. The member functions of the joints are classified into visualization and limit functions. The latter ones determine whether any joint limits are violated and calculate the respective matrices and error correction terms if necessary. In conclusion, the object oriented class design enables a very easy integration of new joint types. Additionally, it is kept very modular in order to effortlessly add new kinds of joints to the pe. Moreover, only minor modifications were made to the already existing relevant classes.

37 Chapter 5: Implementation

Figure 5.4: Class design of the implementation.

5.3 Visualization

Another important part of the implementation throughout this thesis is the integration of the joints into the visualization. The pe framework uses the POV-Ray raytracer [POV09]. Every joint has a writeXJoint method in the writer class of the POV-Ray visualization part of the pe (the X has to be replaced by the corresponding joint name). The implemented changes are explained using the example of the fixed joint. The relevant code is shown in Algorithm 5.7. First of all, every joint obtains a member parameter vis_ for its visualization. It has to be within the interval (0 ... 1]. Here, 1 corresponds to a scale of 100%. With this para- meter, the user can scale the visualization of the joint. As an example, consider a fixed joint attached to the two spheres sphere1 and sphere2. It can be created by means of attachFixedJoint( sphere1, sphere2, 0.5 ). Here, the last parameter denotes the visu- alization parameter. Another scaling parameter is the axis-aligned bounding box (AABB) of the rigid body. In con- trast to the visualization parameter, the user has no influence on this scaling parameter. In the code example above, the function getMinLength() returns the length of the smallest edge of all the edges of the AABB. This procedure is done for both rigid bodies. In the end, the length of the smallest edge of both attached rigid bodies will be chosen. In case of the fixed joint, the result of the visualization will simply be a cylinder between the two geometric primitives. The radius r of this cylinder is now scaled according to the

38 Chapter 5: Implementation

1 void Writer::writeFixedJoint( std::ostream& os, ConstFixedJointID fixedJoint, 2 const Texture& texture ) const{ 3 4 os << std::setiosflags(std::ios::left) 5 << "/*======\n" 6 << "== Fixed joint between body " << fixedJoint->getBody1()->getID() 7 << " and body " << std::setw(4) << fixedJoint->getBody2()->getID() 8 << " ==" << "\n" 9 << "======*/\n\n" 10 << std::resetiosflags(std::ios::left); 11 12 const Vec3 pos1( fixedJoint->getAnchor1WF() ); 13 const Vec3 pos2( fixedJoint->getAnchor2WF() ); 14 const real minLB1(real( fixedJoint->getBody1()->getAABB().getMinLength())); 15 const real minLB2(real( fixedJoint->getBody2()->getAABB().getMinLength())); 16 const real r( fixedJoint->getVisParam() * 0.25 * ( min( minLB1, minLB2 ))); 17 18 os << "cylinder{\n" 19 << " <" << pos1[0] << "," << pos1[2] << "," << pos1[1] << ">, <" 20 << pos2[0] << "," << pos2[2] << "," << pos2[1] << ">, " 21 << r << "\n"; 22 texture.print( os, " " ); 23 os << "}\n\n\n"; 24 } Algorithm 5.7: The POV-Ray visualization of the fixed joint.

user’s visualization parameter and the length of the smallest edge. Additionally, the cylinder is defined by a cap point pos1 and a base point pos2, which are the two anchor points of the fixed joint. The required output is written to a .pov file, which contains all rigid bodies of the simulation scenario. This adjustment, by means of visualization parameter and axis-aligned bounding boxes, is done for every kind of joint. The visualization results of the different joint types are presented throughout the next chapter.

39

Chapter 6: Validation and Results

Chapter 6

Validation and Results

In this chapter, a validation test for each implemented joint is presented. Moreover, the correctness and suitability of the extension of joints to the pe is evaluated. The results are presented in form of screenshots from the visualization, accompanied by plots of the charac- teristic parameters of each joint. All tests have been performed with a time step of 0.005 and all impacts have zero restitution. Moreover, the gravity is set to g = (0, 0, −0.4)T . The small bouncing, which occurs after a collision between two bodies or when a joint limit has been reached, results from the error correction. It corresponds to adding a small coefficient of restitution. Thus, both bodies will bounce apart slightly. Moreover, an extra test case for contacts will be omitted since they are contained in each of the other test scenarios indirectly.

6.1 Fixed Joint

6.1.1 Test Scenario Figure 6.1 shows the initial positions of the rigid bodies in the test scenario. The two spheres are connected by a fixed joint and are at rest. The box is set to a fixed rigid body, i.e. the center of mass is fixed. Thus, the box is not going to move at all during the whole simulation. The used test conditions are collocated in Table 6.1.

Figure 6.1: Test scenario for the fixed joint.

41 Chapter 6: Validation and Results

T Initial position of sphere 1 ~r1 = (1, 0, 0.5) with radius 0.5 T Initial linear velocity of sphere 1 ~v1 = (10, 0, 0) T Initial angular velocity of sphere 1 ~ω1 = (0, 0, 0) T Initial position of sphere 2 ~r2 = (4, 0, 0.5) with radius 0.5 T Initial linear velocity of sphere 2 ~v2 = (10, 0, 0) T Initial angular velocity of sphere 2 ~ω2 = (0, 0, 0) T Initial position of the box ~rb = (9, 0, 1) with side length 2.0 Initial distance between the spheres 3.0 Friction limits for the boxLCP ~λ = (0, −10, −10)T , contactk ~ T λcontactk = (∞, 10, 10)

Table 6.1: Test conditions for the validation of the fixed joint.

The pe framework additionally offers a Union environment. It is a compound of an arbitrary number of rigid bodies. The contained rigid bodies are represented as a single rigid body with its own global position, mass, velocity, and orientation. The union can be compared to a fixed joint. It also “glues” two bodies together at a constant distance. Therefore, the simulation results of a fixed joint are compared with those of a union between the two spheres because their trajectories should coincide.

6.1.2 Results Figure 6.3 depicts the results of the simulation. The large coefficient of friction is very un- realistic. Therefore, sphere 2 performs a rolling motion on its surface but still sticks to the ground. Thus, the fixed joint is rotated and as a consequence sphere 1 lifts off the ground. The fixed joint with both attached spheres starts to rotate Figures 6.3(a)-6.3(c)
. At the beginning, the path of motion of the center of mass of the compound rigid body is a cycloid. ~ ~ If the centrifugal force (fZ) is greater than the centripetal force (fr), the compound rigid body will lift off the ground, shown in 6.3(d). Figure 6.2 shows the distribution of the forces when the rigid body still touches the ground plane. At the point of lift off, the path of motion is a smooth transition from a cycloid to a parabola. The rotation is decelerated when sphere 2 collides with the fixed box, depicted in Figure 6.3(m). The energy is not sufficient to per- form another rotation over the box (Figure 6.3(n) shows the maximum height of sphere 1). Therefore, sphere 1 descends until it finally rests on the ground.

Figure 6.2: Distribution of the forces in the fixed joint during the simulation.

42 Chapter 6: Validation and Results

(a) (b) (c)

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

(m) (n) (o)

(p) (q) (r)

Figure 6.3: Screenshots from the fixed joint simulation.

43 Chapter 6: Validation and Results

Figure 6.4 shows the plot of the constant distance between the two spheres w.r.t. time. It sub- stantiates that the error correction of the fixed joint works. The constant distance between the two spheres is maintained and the positions of them are corrected if necessary. Furthermore, in Figure 6.5 the angular velocity of both spheres in comparison to the union is depicted. They all have the same angular velocity, which is also a confirmation that the fixed joint implementation is correct. Another evidence, why the fixed joint implementation is correct, is that the results for the positions of the spheres within a union and fixed joint are nearly identical. The deviations, depicted in Figure 6.6 and 6.7, result from the fact that the union just adds the resulting forces to its center of mass, regardless of solving a boxLCP with other constraints in every time step. In case of a fixed joint, a boxLCP has to be solved in every time step because a constraint is added to the system.

3,0005

3,0004

3,0003

3,0002

3,0001 distance

3,0000

2,9999

2,9998 0 10 20 30 40 50 60 70 80 time steps

Figure 6.4: The constant distance between the two spheres w.r.t. the time steps.

1,20 1,20

1,00 union 1,00 union fixed fixed 0,80 0,80

0,60 0,60

0,40 0,40

0,20 0,20

0,00 0,00

-0,20 -0,20

-0,40 -0,40 angularvelocity aroundy-axis angularvelocity aroundy-axis -0,60 -0,60 0 10 20 30 40 50 60 70 80 0 10 20 30 40 50 60 70 80 time steps time steps

(a) Angular velocity around the y-axis of sphere 1. (b) Angular velocity around the y-axis of sphere 2.

Figure 6.5: The angular velocity around the y-axis of both spheres within fixed joint and union w.r.t. the time steps.

44 Chapter 6: Validation and Results

7,00 4,50

4,00 union 6,00 fixed 3,50 5,00 3,00

4,00 2,50

3,00 2,00 z-position x-position 1,50 2,00 1,00 union 1,00 fixed 0,50 0,00 0,00 0 10 20 30 40 50 60 70 80 0 10 20 30 40 50 60 70 80 time steps time steps

(a) X-position of sphere 1. (b) Z-position of sphere 1.

Figure 6.6: The position of sphere 1 within fixed joint and union w.r.t. the time steps.

9,00 6,00

8,00 union 5,00 fixed 7,00

6,00 4,00

5,00 3,00 4,00 z-position x-position 3,00 2,00

2,00 union 1,00 1,00 fixed 0,00 0,00 0 10 20 30 40 50 60 70 80 0 10 20 30 40 50 60 70 80 time steps time steps

(a) X-position of sphere 2. (b) Z-position of sphere 2.

Figure 6.7: The position of sphere 2 within fixed joint and union w.r.t. the time steps.

45 Chapter 6: Validation and Results

6.2 Slider Joint

6.2.1 Test Scenario The initial positions of the rigid bodies in the test scenario are depicted in Figure 6.8. The same positions as for the validation of the fixed joint are used. The two spheres connected by a slider joint are at rest. Moreover, the slider is at no upper or lower joint limit. The box is again set to a fixed rigid body. The test conditions of the simulation can be found in Table 6.2.

Figure 6.8: Test scenario for the slider joint.

T Initial position of sphere 1 ~r1 = (1, 0, 0.5) with radius 0.5 T Initial linear velocity of sphere 1 ~v1 = (0, 0, 0) T Initial angular velocity of sphere 1 ~ω1 = (0, 0, 0) T Initial position of sphere 2 ~r2 = (4, 0, 0.5) with radius 0.5 T Initial linear velocity of sphere 2 ~v2 = (10, 0, 0) T Initial angular velocity of sphere 2 ~ω2 = (0, 0, 0) T Initial position of the box ~rb = (9, 0, 1) with side length 2.0 Initial distance between the spheres 3.0 Minimal distance between the spheres 1.5 Maximal distance between the spheres 3.2 Friction limits for the boxLCP ~λ = (0, −10, −10)T , contactk ~ T λcontactk = (∞, 10, 10)

Table 6.2: Test conditions for the validation of the slider joint.

6.2.2 Results The screenshots, displayed in Figure 6.9, show the motion of the slider joint throughout the simulation. The movement is based on a mechanical background, which is different to the validation example of the fixed joint because only sphere 2 has an initial linear velocity. At

46 Chapter 6: Validation and Results

(a) (b) (c)

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

(m) (n) (o)

Figure 6.9: Screenshots from the slider joint simulation.

47 Chapter 6: Validation and Results the beginning, sphere 2 performs a rolling motion on its surface because of the high friction. Consequently,the slider joint is rotated and sphere 1 lifts off the ground. Since the slider is not at its upper limit yet, the two spheres are treated as two distinct rigid bodies. The centrifugal ~ ~ force of sphere 1 (fZ1 ) is larger than its centripetal force (fr1 ), which is caused by its own weight. The correlations are shown in Figure 6.10. Thus, sphere 1 moves outwards until the maximal length of the slider joint is reached, shown in Figure 6.9(c). From this point on, the two rigid bodies are considered as one compound rigid body. The centrifugal force of sphere 1 ~ is smaller than or equal to the centripetal force of both bodies, sphere 1 and sphere 2 (fr1+2 ). Consequently, the compound rigid body does not lift off the ground. Instead, they continue to rotate around the contact point between sphere 2 and the ground plane. If the slider is at its upper limit, one has to consider the weight of sphere 1 (mg1) additionally. If the component of the weight of sphere 1 along the slider axis (mgaxis1 ) is less than the centrifugal force of sphere 1 the slider will continue to rotate around the contact at full length. If the component of the weight along the slider axis is greater than the centrifugal force of sphere 1, the latter will start to move inwards until the minimal length of the slider is attained, depicted in Figures ~ 6.9(e)- 6.9(i). In both cases, the centrifugal force of both bodies ( fZ1+2 ) is smaller than or equal to the centripetal force of both, and remains smaller or equal throughout the rest of the simulation. In the next step, the compound rigid body topples over because there is still a rest of the initial linear velocity, which has been transformed into a angular velocity, on sphere 2 (Figure 6.9(k)). Shortly before sphere 1 touches the ground, the slider is extended slightly because the component of the weight along the slider axis is smaller than the centrifugal force on sphere 1. This can hardly be recognized in Figure 6.9(n). Nevertheless, in Figure 6.9(o), the slider joint rests on the ground with its minimal length between the spheres. This results from the fact that the compound rigid body still has a rest of the initial angular velocity and tries to rotate around the new contact between sphere 1 and the ground plane. The energy is not sufficient for another rotation. Thus, sphere 2 only slightly lifts off the ground. The component of the weight of sphere 2 along the slider axis is greater than the centrifugal force of sphere 2. Consequently, the slider attains its minimal length. In order to validate the

Figure 6.10: Distribution of the forces in the slider joint during the simulation. correctness of the implementation of the slider joint, the plot in Figure 6.11 shows the length of the slider joint w.r.t. the time steps. The slider moves out from its initial length 3.0 to its maximal length 3.2. Then the joint moves in until its minimal length 1.5 is attained. The small increases during the time steps 13 and 22 result from the error correction of the lower bound which bounces the two bodies apart slightly. The time steps 29 to 34 correspond to the movement shortly before sphere 1 rests on the ground. The centrifugal force moves sphere 1 slightly away from sphere 2. Finally, the slider attains its minimal length and rests on the ground. Another way to verify the correctness of our simulation is to look at the angular velocity of both spheres. Figure 6.12 confirms that both spheres have identical angular velocity around the y-axis, which is a prerequisite for the correct modeling of the slider joint.

48 Chapter 6: Validation and Results

3,5

3

2,5

2

1,5 length

1

0,5

0 0 10 20 30 40 50 60 70 80 time Steps

Figure 6.11: The length of the slider joint w.r.t. the time steps.

0,7 sphere 1 0,6 sphere 2 0,5

0,4

0,3

0,2

0,1

0

-0,1 angularvelocity aroundy-axis -0,2 0 10 20 30 40 50 60 70 80 time steps

Figure 6.12: The angular velocity of both spheres w.r.t. the time steps.

49 Chapter 6: Validation and Results

6.3 Hinge Joint

6.3.1 Test Scenario In Figure 6.13, the initial positions of the two capsules in the test scenario are depicted. Capsule 1 is set to a fixed rigid body. Thus, only capsule 2 can rotate around the pivotal point located on the axis of rotation of the hinge joint. Table 6.3 lists the test conditions of the simulation.

Figure 6.13: Test scenario for the hinge joint.

T Initial position of capsule 1 ~r1 = (1, 0, 0.5) with radius 0.3, length 1.0 and T π rotated around axis (0, 1, 0) by − 2 rad T Initial linear velocity of capsule 1 ~v1 = (0, 0, 0) T Initial angular velocity of capsule 1 ~ω1 = (0, 0, 0) T Initial position of capsule 2 ~r√2 = (1.75, 0, 4.25) with radius 0.3, length 1.52 + 5.72 and rotated around axis (0, 1, 0)T 5.7

by π − atan 1.5 rad T Initial linear velocity of capsule 2 ~v2 = (0, 0, 0) Initial angular velocity of capsule 2 ~ω2 = (0, 0, 0) T Rotation axis ~xaxis = (0, 1, 0) −→ Position of the pivotal point rot = (1, 0, 1.3)T π Minimal aperture angle Θmin = − 2 rad π Maximal aperture angle Θmax = 2 rad 0.75
Initial aperture angle Θ = −atan 2.95 = −0.248963 rad T Linear velocity of capsule 2 w.r.t. time time step 30, 50, 70, 90: ~v2 = (0, 0, 3)

Table 6.3: Test conditions for the validation of the hinge joint.

50 Chapter 6: Validation and Results

6.3.2 Results The hinge has an initial aperture angle of Θ radians. During the first 29 simulation steps no external forces are applied. Thus, capsule 2 descends slowly to the ground plane due to the gravity acting on its center of mass. Instead of descending directly, capsule 2 rotates around the pivotal point on the hinge’s axis of rotation in a circular path of motion until the minimal aperture angle Θmin of the hinge is obtained. The screenshots in Figure 6.16 document the movement of capsule 2 from time step 29 to 69. In Figure 6.16(a), capsule 2 rests at the minimal angle Θmin. In time step 30, capsule 2 receives a linear velocity boost in z-direction, which is transformed directly into a negative angular velocity. Capsule 2 rotates around the pivotal point until the maximal aperture angle
Θmax is obtained Figures 6.16(b)–6.16(f) . Due to the error correction of the upper limit, capsule 2 slightly bounces upwards until a certain maximal height is reached, shown in Figure 6.16(g). Afterwards, it will descend again until the maximal aperture angle is obtained. In time step 50, capsule 2 once again receives a linear velocity boost in z-direction, which in this case is transformed directly into a positive angular velocity. Thus, capsule 2 performs another rotation around the pivotal point until the minimal aperture angle Θmin is obtained Figures 6.16(h)–6.16(m)
. The small bouncing effect, depicted in Figure 6.16(n), again results from the error correction term of the lower limit. This movement is repeated from lower to upper limit in time step 70 to 89, and from upper to lower limit in time step 90 to 109. The screenshots for the movement between time step 70 to 109 are omitted since they are identical to the ones in Figure 6.16. Figure 6.14 depicts the plot of the aperture angle w.r.t. the time steps. It corresponds to the expected results of the simulation. The minimal and maximal hinge angle are obtained. The error correction of the limits clearly works since the upper and lower limits are obtained “softly” due to the bouncing effect. Another evidence for the correctness of the hinge joint implementation is the changing angular velocity of capsule 2 around the axis of rotation, shown in Figure 6.15. The applied linear velocity is directly transformed into a negative or positive angular velocity correspondingly. Moreover, the run of the curves in both plots, 6.14 and 6.15, repeats itself correctly from time step 30 on.

1,8

1,4

1

0,6

0,2

-0,2

-0,6 apertureangle -1

-1,4

-1,8 0 10 20 30 40 50 60 70 80 90 100 110 120 time steps

Figure 6.14: Aperture angle of the hinge joint w.r.t. the time steps.

51 Chapter 6: Validation and Results

0,8

0,6

0,4

0,2

0

-0,2

-0,4

angularvelocity capsule 2 -0,6

-0,8 0 10 20 30 40 50 60 70 80 90 100 110 120 time steps

Figure 6.15: Angular velocity of capsule 2 around the axis of rotation w.r.t. the time steps.

−→ Additionally, the distance d between anchor point ~r2 and pivotal point rot is measured by means of the standard deviation (cf. [BKMM01, pp.774] ): v u N u 1 X σ = t (d − µ)2, (6.1) N i i where N 1 X µ = d (6.2) N i i corresponds to the mean value of the distance d.

The initial distance corresponds to d0 = 3.04385. The distance measured during the simulation is 3.04385±4.54742·10−14, and therefore lies in the dimension of the numerical roundoff error. This also substantiates that the hinge joint implementation is correct since capsule 2 does not leave its circular path of motion.

52 Chapter 6: Validation and Results

(a) (b) (c)

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

(m) (n) (o)

Figure 6.16: Screenshots from the hinge joint simulation.

53 Chapter 6: Validation and Results

6.4 Planar Slider Crank Mechanism

6.4.1 Test Scenario For multibody simulations, the test problem formulations or performance measurements have not been standardized yet. In the scope of this thesis, a planar slider crank mechanism is used as a validation example since it couples three different types of the implemented joints: hinge, slider and contacts. The slider crank mechanism has been proposed as a benchmarking problem (cf. [GDLC06]) and has been used for validation in latest research works, e.g. [FLG09]. The planar slider crank mechanism, shown in Figure 6.17, consists of six rigid bodies: four capsules and two spheres. Sphere 1 and capsule 4 form a union. Additionally, capsule 1 and sphere 2 are set to fixed rigid bodies. Consequently, they are not going to move at all throughout the simulation. Three hinge joints are connecting capsule 1 and capsule 2, capsule 2 and capsule 3, and capsule 3 and the union respectively. The slider joint is attached to the union and sphere 2. Its two anchor points correspond to the centers of mass of both spheres. Moreover, the validation test is performed frictionless since the motion between lower and upper limit of the slider joint can thus be seen more clearly. The initial positions of the rigid bodies in the test scenario are depicted in 6.17. Furthermore, the test conditions are stated in Table 6.4.

Figure 6.17: Test scenario for the slider crank mechanism.

6.4.2 Results The slider crank mechanism has one DOF. Only translational movement along the slider axis is allowed. The motion of the system is controlled by an angular velocity applied on capsule 2. The angular velocity is transformed directly into a linear movement along the axis of the slider joint. In the scope of this validation test, five angular velocity boosts are applied on capsule 2.

54 Chapter 6: Validation and Results

T Initial position of capsule 1 ~rc1 = (1, 0, 0.5) with radius 0.3, length 1.0 and T π rotated around axis (0, 1, 0) by − 2 rad T Initial linear velocity of capsule 1 ~vc1 = (0, 0, 0) T Initial angular velocity of capsule 1 ~ωc1 = (0, 0, 0) Initial position of capsule 2 ~r = (1.75, 0, 4.25)T with radius 0.3, length √c2 1.52 + 5.72 and rotated around axis (0, 1, 0)T 5.7

by π − atan 1.5 rad T Initial linear velocity of capsule 2 ~vc2 = (0, 0, 0) T Initial angular velocity of capsule 2 ~ωc2 = (0, 0, 0) Initial position of capsule 3 ~r = (5.75, 0, 5.25)T with radius 0.3, length √c3 3.72 + 6.52 and rotated around axis (0, 1, 0)T 3.7
by atan 6.5 rad T Initial linear velocity of capsule 3 ~vc3 = (0, 0, 0) T Initial angular velocity of capsule 3 ~ωc3 = (0, 0, 0) T Initial position of capsule 4 ~rc4 = (9, 0, 2.5) with radius 0.3, length 1.0 and T π rotated around axis (0, 1, 0) by − 2 rad T Initial linear velocity of capsule 4 ~vc4 = (0, 0, 0) T Initial angular velocity of capsule 4 ~ωc4 = (0, 0, 0) T Initial position of sphere 1 ~rs1 = (9, 0, 1) with radius 1 T Initial linear velocity of sphere 1 ~vs1 = (0, 0, 0) T Initial angular velocity of sphere 1 ~ωs1 = (0, 0, 0) T Initial position of sphere 2 ~rs2 = (14, 0, 1) with radius 1 T Initial linear velocity of sphere 2 ~vs2 = (0, 0, 0) T Initial angular velocity of sphere 2 ~ωs2 = (0, 0, 0) π Minimal aperture angle of hinge 1 Θmin1 = − 2 rad π Maximal aperture angle of hinge 1 Θmax1 = 2 rad 0.75
Initial aperture angle of hinge 1 Θ1 = −atan 2.95 = −0.248963 rad −→ T Pivotal point of hinge 1 rot1 = (1, 0, 1.3) T Axis of rotation of hinge 1 ~xaxis1 = (0, 1, 0) 3 Minimal aperture angle of hinge 2 Θmin2 = − 4 π rad 3 Maximal aperture angle of hinge 2 Θmax2 = 4 π rad 2.75
1.75

Initial aperture angle of hinge 2 Θ2 = − atan 0.75 + atan 3.25 = −1.79849 rad −→ T Pivotal point of hinge 2 rot2 = (2.5, 0, 7) T Axis of rotation of hinge 2 ~xaxis2 = (0, 1, 0) π Minimal aperture angle of hinge 3 Θmin3 = − 2 rad π Maximal aperture angle of hinge 3 Θmax3 = 2 rad 3.25
Initial aperture angle of hinge 3 Θ3 = −atan 1.95 = −1.03038 rad −→ T Pivotal point of hinge 3 rot3 = (9, 0, 3.3) T Axis of rotation of hinge 3 ~xaxis3 = (0, 1, 0) Initial length of the slider 5.0 Minimal length of the slider 4.0 Maximal length of the slider 8.0 Friction limits for the boxLCP ~λ = (0, 0, 0)T , ~λ = (∞, 0, 0)T contactk contactk T Angular velocity of capsule 2 time step 0, 30, 50: ~ω2 = (0, −10, 0) T w.r.t. time time step 20, 40: ~ω2 = (0, 10, 0)

Table 6.4: Test conditions for the validation of the slider crank mechanism. 55 Chapter 6: Validation and Results

The screenshots in Figure 6.19 show the movement of the planar slider crank mechanism between time step 0 and 35. In time step 0, capsule 2 receives a negative angular velocity boost around the axis of rotation of hinge 1. Consequently, capsule two performs a counterclockwise rotation around the pivotal point of hinge 1. This rotation is transmitted via hinge 2 and hinge 3 to the union, where it is transformed into a linear movement. Thus, the union moves closer to capsule 1 until the maximal length of the slider joint is reached, shown 6.19(d). Due to the error correction, the upper limit of the slider joint causes the union to bounce back into the direction of sphere 2. In time step 20 the movement towards sphere 2 is additionally accelerated because of the positive angular velocity boost of capsule 2. Here, the rotation in clockwise direction around the pivotal point of hinge 1 forces capsule 3 to transmit the linear motion by means of hinge 3 to the union. As a consequence, the slider joint attains its minimal length, depicted 6.19(h). At the lower limit, the union will again bounce away from sphere 2 into the direction of capsule 1 due to the error correction. In time step 30 capsule 2 again receives a negative angular velocity boost causing the union to move faster in direction of capsule 1. This acceleration can also be seen in Figure 6.18. The curve has a steeper increase between the time steps 30 and 35. The slider once again reaches its upper limit, which can also be seen in Figure 6.19(l). The following movement is similar to the one depicted in 6.19 and, thus, screenshots for this motion will be omitted. Instead, the further movement is described by means of the plot in Figure 6.18. In time step 35 the union bounces once more away from the upper limit due to the error correction. The steeper decline inbetween the time steps 40 and 45 results from the angular velocity boost applied on capsule 2 in time step 40. The union is already moving towards sphere 2 but is even accelerated due to the applied positive angular velocity. Again it bounces back from sphere 2 due to the error correction of the lower limit. In time step 50 the last angular velocity boost is applied. Since it is a negative velocity around the axis of rotation of hinge 1, the slider will attain its maximal limit and bounces backwards because of the error correction of the upper limit. Inbetween timestep 53 and 75 capsule 2 passes its dead center. The torque caused by the weights of capsule 2 and 3 is transformed into a linear force. This force moves the slider strictly backwards to its minimal limit. The error correction of the lower limit slightly moves the union into the direction of capsule 1. Nevertheless, the linear force, caused by the torque resulting from the weights of capsule 2 and 3, is higher than the resulting force from the error correction. Thus, the slider will finally attain its minmal length. These results sustain that the concatenation of several joint types works correctly.

9

8

7

6 length 5

4

3 0 10 20 30 40 50 60 70 80 90 100 time steps

Figure 6.18: The length of the slider joint within the planar slider crank mechanism w.r.t. the time steps.

56 Chapter 6: Validation and Results

(a) (b) (c)

(d) (e) (f)

(g) (h) (i)

(j) (k) (l)

Figure 6.19: Screenshots from the planar slider crank joint simulation.

57

Chapter 7: Conclusions and Future Work

Chapter 7

Conclusions and Future Work

7.1 Conclusions

In the scope of this thesis joint motion constraints were integrated into the pe framework, which is developed at the Chair for System Simulation at the Friedrich-Alexander University Erlangen-Nuremberg. The numerical approach presented in this thesis is clearly suitable for simulating joint motion constraints. The correctness of this augmentation to the pe framework has been substantiated by means of several validation scenarios, developed to test the specific characteristics of each joint type. All of them show that the joints are constraining the motion of the rigid bodies correctly and, thus, underline the capabilities of the implementation. Special emphasis has been placed on the implemented class design. It is kept very modular in order to integrate additional joint types effortlessly. In addition, with the help of several detailed explanations, advanced joint models, which are compound of the basic joint types presented in this thesis, can be modeled effortlessly. The mathematical formulation is based on complementarity conditions. Moreover, Full- Coordinate methods have been used, i.e. additional forces have been inserted into the sys- tem of complementarity conditions in order to preserve the motion constraints. The complex mechanical background has been elaborated profoundly. Beginning with the Newton-Euler equations for contact mechanics, the integration of joints was performed gradually through- out the thesis. The problem formulation has been deduced from analytical mechanics. The concept of Jacobian matrices has been introduced and elucidated. The unified problem for- mulation is based on unilateral (contacts) and bilateral (joints) constraints. Contacts have been integrated in an elegant way without major changes to the existing classes. The complementarity constraints for contacts, which were already present, have been refor- mulated by means of Jacobian matrices in order to fit into the unified constraint formulation. The joint modeling, which is based on the reduction of the DOF of a free-floating rigid body, is elucidated by means of selected example joints. These are fixed, slider, hinge and contact joint. The planar slider crank mechanism has been selected as a concluding test scenario. On the one hand, it served as a validation in order to demonstrate that concatenations of the implemented joint types work correctly. On the other, it is also a well-known standard example throughout the multibody community. Furthermore, joint limits have been included into the complementarity formulation to restrict the motion of the remaining DOF of the joints to a finite movement. Here, linear and angular movement have been constrained. For the latter, a method to calculate the initial aperture

59 Chapter 7: Conclusions and Future Work angle in 3D in case of hinge joints has been developed and thoroughly tested. Due to the use of Full-Coordinate methods, the so called drifting effect arises. Error correction parameters for joints, contacts and joint limits have also been integrated into the boxLCP formulation to keep this effect to a minimum. In addition, the visualization has been extended to depict all implemented joints. The screen- shots of the validation examples show that the visualization clearly helps to verify the cor- rectness of the implementation.

7.2 Future Work

The implementation provides a solid basis for further integration of several joint types. One of them is the ball-in-socket joint. It has three DOF and allows arbitrary rotation between two rigid bodies. Two points, one from body 1 and another from body 2, will always be connected. This constraint seems to be easy but its limits are not. A ball-in-socket joint requires spherical joint limits. The unconstrained motion of this joint corresponds to a sphere. If the motion is constrained, only a cone remains as the reachable region. The ball-in-socket joint limits are based on reach cones (cf. [WG01]). The limits get more complicated if the reach cones are extended beyond one hemisphere. I suggest to model them as a variation of the angular joint limits in all three dimensions in order to reduce the computational amount. One of the numerous applications of this joint limits is the modeling of the shoulder joint. Another interesting joint would be a cable or rope (cf. [Mil07, pp.126]). The two rigid bodies perform an unconstrained motion as long as the rope is not pulled taut. Since then, the mo- tion will be constrained. I would suggest to model this joint like a joint limit. The cable has a maximum length. If this upper limit is violated, a new constraint is added to the equation system. The constraint works similar to a contact joint. Whenever the upper limit is reached, a contact is generated between the two rigid bodies. The only difference to a “normal” contact is that the contact normal is reversed, which also means that the two attached rigid bodies will be pulled together instead of apart. Regarding a lower limit, which would correspond to a collision of the two involved rigid bodies, is not necessary since the collision detection will automatically generate a new contact. Furthermore, an error correction term can be modeled corresponding to the penetration depth, which in this case is the distance of how much the rope is stretched above its limit. Moreover, the rope could be modeled elastically by means of including a coefficient of restitution. Joint motors are also based on the Jacobian notation and therefore can be integrated effort- lessly in the modular implementation (cf. [Erl05]). Motors work similar to the error correction terms. They adjust velocities by adding energy to the system to obtain a defined position. In general a torque or force is applied in order to achieve a desired speed. Thus, joint motors control the relative motion of the rigid bodies constrained by joints. In addition, upper and lower limits can model the direction of the maximum force. A very interesting characteristic of these constraints results from the fact that they can model friction in joints. If the desired velocity is set to zero and the maximal applied force is set to a constant value, the joint motion slows down and seems to be restricted by friction. Considering the implementation, an important improvement would be to modify the already existing batch generation for contacts to be capable of handling joints. Throughout the batch generation, the simulation scenario is divided into independent batches. Each batch contains all constraints of a single constraint graph that potentially interact with each other. With this enhancement, an enormous performance gain could be achieved. On the one hand, the

60 Chapter 7: Conclusions and Future Work complementarity systems of the single batches can be proceeded in parallel. On the other, joints could also be set inactive, if no forces or torques are exerted on the batch. This would also save the solver from unnecessary iterations over a system of complementarity constraints.

61

Bibliography

Bibliography

[AP97] M. Anitescu and F.A. Potra. Formulating dynamic multi-rigid-body contact prob- lems with friction as solvable linear complementarity problems. ASME Nonlinear Dynamics, 14:231–247, 1997.

[AT08] M. Anitescu and A. Tasora. An iterative approach for cone complementarity prob- lems for nonsmooth dynamics. Computational Optimization and Applications, 2008. http://dx.doi.org/10.1007/s10589-008-9223-4 [Accessed 15.09.2009]. [Bar96] D. Baraff. Linear-time dynamics using lagrange multipliers. In Proceedings of SIGGRAPH 1996, pages 137–146, New Orleans, 1996.

[BKMM01] I.N. Bronstein, Semendjajew K.A., G. Musiol, and H. M¨uhlig. Taschenbuch der Mathematik. Verlag Harri Deutsch, 2001.

[CPS92] R.W. Cottle, J.S. Pang, and R.E. Stone. The Linear Complementarity Problem. Academic Press, Inc., 1992.

[Cra86] J.J. Craig. Introduction to Robotics. Addison-Wesley, 1986.

[Erl05] K. Erleben. Stable, Robust, and Versatile Multibody Dynamics Animation. PhD thesis, Department of Computer Science, University of Copenhagen (DIKU), 2005. ftp://ftp.diku.dk/diku/image/publications/erleben.050401.pdf [Accessed 15.09.2009].

[Eup09] Homepage of the Euphoria Engine. http://www.naturalmotion.com/euphoria. htm [Accessed 15.09.09], 2009. [FLG09] P. Flores, R. Leine, and C. Glocker. Modeling and analysis of rigid multibody systems with translational clearance joints based on the nonsmooth dynamics approach. In Proceedings of 2009 ECCOMAS Multibody Dynamics Conference, Warsaw, 2009.

[FMMR09] S. Falomi, M. Malvezzi, E. Meli, and M. Rinchi. Multibody modeling of railway vehicles: Innovative algorithms for the detection of wheel-rail contact points. In Proceedings of 2009 ECCOMAS Multibody Dynamics Conference, Warsaw, 2009.

[GDLC06] M. Gonz´alez,D. Dopico, U. Lugr´ıs,and J. Cuadrado. A benchmarking system for mbs simulation software: Problem standardization and performance measure- ment. Multibody System Dynamics, 16(2):179–190, 2006.

[GHS02] D. Gross, W. Hauger, and W. Schnell. Technische Mechanik Bd. 3, Kinetik. Springer, 2002.

[Hav09] Homepage of the Havok Physics Engine. http://www.havok.com/ [Accessed 15.09.09], 2009.

63 Bibliography

[MFC+09] M. Machado, P. Flores, J.C. Pimenta Claro, J.A.C. Ambr´osio,and M. Silva. Development of a dynamic multibody 2d-model of the human knee joint. In Proceedings of 2009 ECCOMAS Multibody Dynamics Conference, Warsaw, 2009. [Mil07] I. Millington. Game Physics Engine Development. Series in Interactive 3D Tech- nology. Morgan Kaufmann, 2007. [ODE09] Homepage of the Open Dynamics Engine (ODE). http://www.ode.org [Accessed 15.09.2009], 2009. [OT09] Homepage of the OpenTissue simulation framework. http://www.opentissue. org [Accessed 15.09.2009], 2009. [PAP+09] J. Pombo, J.A.C. Ambr´osio,M. Pereira, R. Lewis, R. Dwyer-Joyce, C. Ariaudo, and N. Kuka. A railway wheel wear prediction tool based on a multibody software. In Proceedings of 2009 ECCOMAS Multibody Dynamics Conference, Warsaw, 2009. [Pfe92] F. Pfeiffer. Einf¨uhrungin die Dynamik. Teubner, 1992. [POV09] Homepage of the Persistence of Vision Raytracer (POV-Ray). http://www. povray.org [Accessed 15.09.2009], 2009. [Pre08] T. Preclik. Iterative Rigid Body Dynamics. Diploma thesis, Computer Sci- ence Department 10 (System Simulation), University of Erlangen-Nuremberg, 2008. http://www10.informatik.uni-erlangen.de/Publications/Theses/ 2008/Preclik_DA08.pdf [Accessed 15.09.2009]. [PT96] J.S. Pang and J.C. Trinkle. Complementarity formulations and existence of solutions of dynamic multi-rigid-body contact problems with coulomb friction. Mathematical Programming, 73(2):199–226, 1996. http://dx.doi.org/10.1007/ BF02592103 [Accessed 15.09.2009]. [RA09] S.E. Rodrigo and J.A.C. Ambr´osio. An integrated methodology for computer simulation of human gait. In Proceedings of 2009 ECCOMAS Multibody Dynamics Conference, Warsaw, 2009. [Sch97] W. Schiehlen. Multibody system dynamics: Roots and perspectives. Multibody System Dynamics, 1(2):149–188, 1997. http://dx.doi.org/10.1023/A: 1009745432698 [Accessed 15.09.2009]. [Sha98] A. A. Shabana. Dynamics of Multibody Systems. Cambridge University Press, 1998. [Smi02] R. Smith. How to make new joints in ODE. http://www.ode.org/joints.pdf [Accessed 15.09.2009], 2002. [Smi04] R. Smith. Constraints in Rigid Body Dynamics. In A. Kirmse, editor, Game Programming Gems 4, pages 241–251. Charles River Media, 2004. [WG01] J. Wilhelms and A. Van Gelder. Efficient spherical joint limits with reach cones. Technical report, Computer Science Dept., University of California, 2001. [Wit97] A. Witkin. Physically Based Modeling: Principles and Practice, Constrained Dy- namics (Online Siggraph ’97 Course notes). http://www.cs.cmu.edu/~baraff/ sigcourse/notesf.pdf [Accessed 15.09.2009], 1997.

64