A Quick Tutorial on Multibodmy Dynamics

8/20/2019 A Quick Tutorial on Multibodmy Dynamics

1/25

A Quick Tutorial on Multibody Dynamics

C. Karen LiuSumit Jain

School of Interactive ComputingGeorgia Institute of Technology

i


2/25

Contents

1 Introduction 2

2 Lagrangian Dynamics 3

3 Review: Newton-Euler equations 6

4 Rigid Body Dynamics: Lagrange’s equations 8

5 Articulated Rigid Body Dynamics 135.1 Denitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135.2 Cartesian and generalized velocities . . . . . . . . . . . . . . . . . . . . . . . 145.3 Equations of motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

6 Conversion between Cartesian and Generalized Coordinates 176.1 Velocity conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176.2 Force conversion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

7 Recursive Inverse Dynamics 217.1 Dynamics in the local frame . . . . . . . . . . . . . . . . . . . . . . . . . . . 217.2 Pass 1: Compute velocity and acceleration . . . . . . . . . . . . . . . . . . . 227.3 Pass 2: Compute force and torque . . . . . . . . . . . . . . . . . . . . . . . . 24

1


3/25

1 Introduction

If you have not read the excellent SIGGRAPH course notes on physics-based animationby Witkin and Baraff, you can stop reading further right now. Go look for those notes athttp://www.cs.cmu.edu/~baraff/sigcourse/ and come back when you fully understandeverything in those notes.

If you are still reading this document, you probably t the following prole. You are a com-puter scientist with no mechanical engineering background and minimal training in physicsin high school but you are seriously interested in physics-based character animation. Youhave read Witkin and Baraff’s SIGGRAPH course notes a few times but don’t know whereto go from simulating rigid bodies to human gures. You have played with some commercialphysics engines like ODE (Open Dynamic Engine), PhysX, Havok, or Bullet, but you wishto simulate human behaviors more interesting than ragdoll effects.

Physics-based character animation consists of two parts: simulation and control. This doc-

ument focuses on the simulation part. It’s quite likely that you do not need to understandhow the underlying simulation works if your control algorithm is simple enough. However,complex human behaviors often require sophisticated controllers that exploit the dynamicsof a multibody system. A good understanding of multibody dynamics is paramount fordesigning effective controllers.

There are many ways to learn multibody dynamics. Reading a textbook on this topic ortaking a course from the mechanical engineering department will both do the job. However,if you only want to learn the minimal set of multibody dynamics necessary to jump startyour research in physics-based character animation, this document might be what you arelooking for. In particular, this document attempts to answer the following questions.

• I know how to derive the equations of motion for one rigid body and I have seen peopleuse the following equations for articulated rigid bodies, but I don’t know how they arederived.

M (q )q̈ + C (q , q̇ ) = Q

• I have seen the Euler-Lagrange equation in the following form before, but I don’t knowhow it is related to the equations of motion above.

ddt

∂T i∂ q̇

− ∂T i∂ q

− Q = 0

• I use generalized coordinates to compute the control forces, how do I convert themto Cartesian forces such that I can use simulators like ODE, PhysX, or Bullet whichrepresent rigid bodies in the maximal coordinates?

• I heard that inverse dynamics can be computed efficiently using a recursive formulation.How does that work?

2


4/25

2 Lagrangian Dynamics

Articulated human motions can be described by a set of dynamic equations of motion of multibody systems. Since the direct application of Newton’s second law becomes difficultwhen a complex articulated rigid body system is considered, we use Lagrange’s equations derived from D’Alembert’s principle to describe the dynamics of motion. To simplify themath, let’s temporarily imagine that the entire human skeleton consists of a collection of particles {r 1, r 2, . . . , r n p }. Each particle, r i , is dened by Cartesian coordinates that describethe translation with respective to the world coordinates. We can represent r i by a set of generalized coordinates that indicate the joint conguration of the human skeleton:

r i = r i (q 1, q 2, . . . , q n j , t ) (1)

where t is the time and q j is a joint degree of freedom (DOF) in the skeleton. Each q j is afunction of time but we assume that r i is not an explicit function of time.

The virtual displacement δ r i refers to an innitesimal change in the system coordinates suchthat the constraint remains satised. In the context of human skeleton, the system coor-dinates are the generalized coordinates q j and the constraint manifold lies in the Cartesianspace. The virtual displacement δ r i is a tangent vector to the constraint manifold at a xedtime, written as

δ r i = j

∂ r i∂q j

δq j (2)

We can now write the virtual work done by a force f i acting on particle r i as

f i · δ r i = f i · j

∂ r i∂q j δq j ≡ j Q ij δq j = Q i · δ q (3)

where Qij = ∂ r i∂q jT

f i is dened as the component of the generalized force associated withcoordinate q j . In vector form, Q i is the generalized force corresponding to the Cartesianforce f i with the relation Q i = J T i f i , where J i is the Jacobian matrix with the j th columndened as ∂ r i∂q j .

From D’Alembert’s principle, we know that the sum of the differences between the forcesacting on a system and the inertial force of the system along any virtual displacementconsistent with the constraints of the system, is zero. Therefore, the virtual work at r i can

be written asδW i = f i · δ r i = µi r̈ i · δ r i =

j

µi r̈ i · ∂ r i∂q j

δq j (4)

where µi is the innitesimal mass associated with r i . The component of inertial force asso-ciated with q j can be written as


= ddt

µi ṙ i · ∂ r i∂q j

− µi ṙ i · ddt

∂ r i∂q j

(5)

3


5/25

Now let us consider the velocity of r i in terms of joint velocity q̇ j

ṙ i = j

∂ r i∂q j

q̇ j (6)

from which we derive the following two identities:∂ ṙ i∂ q̇ j

= ∂ r i

∂q j(7)

∂ ṙ i∂q j

=k

∂ 2r i∂q j ∂q k

q̇ k = ddt

∂ r i∂q j

(8)

Using these two identities, we rewrite Equation (5) as


= ddt

∂ ∂ q̇ j

12

µi ṙ T i ṙ i − ∂ ∂q j

12

µi ṙ T i ṙ i (9)

We can denote the kinetic energy of r i as

T i = 12

µ ṙ T i ṙ i , (10)

and rewrite Equation (9) as


= ddt

∂T i∂ q̇ j

− ∂T i∂q j

(11)

Combining the denition of generalized force (Equation (3)), D’Alembert’s principle (Equa-tion (4)), and the generalized inertial force (Equation (11)), we arrive at the following equa-tion:

ddt ∂T i∂ q̇ j − ∂T i∂q j δq j = Q ij δq j (12)If the set of generalized coordinates q j is linearly independent, Equation (12) leads to La-grangian equation :

ddt

∂T i∂ q̇ j

− ∂T i∂q j

− Q ij = 0 (13)

Equations of Motion in Vector Form. Equation (13) is the equation of motion for onegeneralized coordinate in a multibody system. We can combine n j scalar equations into thefamiliar vector form

M (q )q̈ + C (q , q̇ ) = Q (14)

where M (q ) is the mass matrix, C (q , q̇ ) is the Coriolis and centrifugal term of the equationof motion, and Q is the vector of generalized forces for all the degrees of freedom (DOFs) inthe system. M only depends on q and C depends quadratically on q̇ .

4


6/25

How do we derive M and C from Equation (13)? Let us go back to the velocity of oneparticle r i :

ṙ i = j

∂ r i∂q j

q̇ j = J i (q )q̇ (15)

where J i denotes the Jacobian of r

i . By summing up all the particles in the system, thekinetic energy of the system can then be expressed as

T =i

T i =i

12

µ ṙ T i ṙ i =i

12

µ(J i q̇ )T (J i q̇ ) = 12

q̇ T (i

µJ T i J i )q̇ = 12

q̇ T M (q )q̇ (16)

where we dene the mass matrix, M (q ) = i µJ T i J i , and will shortly show it is indeed the

mass matrix in Equation (14).

From Equation (16), we can derive the derivative terms to construct the Lagrange’s equation(Equation (13)):

ddt

∂T ∂ q̇

− ∂T ∂ q

= M q̈ + Ṁ q̇ − 12

q̇ T ∂M ∂ q

T q̇ ≡ M q̈ + C (q , q̇ ) (17)

Comparing Equation (17) to Equation (14), we conrm that the mass matrix is identical inboth equations. C is the Coriolis and centrifugal term in Equation (14) and is dened as

C = Ṁ q̇ − 12∂M ∂ q q̇

T q̇ .

Note. In the second term of C , we introduce tensor notation ∂M ∂ q , which implies that the j th element of the tensor ∂M

∂ qis the matrix ∂M

∂q j. Note that, in general, the quantity with

notation ∂M ∂ q q̇ is not equal to Ṁ . This is because, the jth column of the matrix ∂M ∂ q q̇ is the

vector ∂M ∂q j q̇ or k∂ (M )k

∂q jq̇ k , where the notation ( A) j denotes the j th column of the matrix A .

In contrast, the j th column of the matrix Ṁ is k∂ (M ) j

∂q kq̇ k .

Once we know how to compute the mass matrix, Coriolis and centrifugal terms, and gen-eralized forces, we can compute the acceleration in generalized coordinates, q̈ , for forward dynamics . Conversely, if we are given q̈ from a motion sequence, we can use these equationsof motion to derive generalized forces for inverse dynamics .

The above formulation is convenient for a system consisting of nite number of mass points.

However, for a dynamic system that consists of rigid bodies, there are innitely many pointscontained in each rigid body making the above formulation intractable. In the following twosections, we view a rigid body as a continuum and derive compact equations of motions inboth Cartesian coordinates and generalized coordinates.

5


7/25

3 Review: Newton-Euler equations

This section reviews Newton-Euler equations for rigid body dynamics. The derivation of massmatrix M (q ) and Coriolis and centrifugal term C (q , q̇ ) for a rigid body will be presented inthe next section. If you are familiar with Newton-Euler equations, you can skip this sectionand continue to the next. However, many math notations used in Witkin and Baraff’s coursenotes are also reviewed in this section, such as linear momentum, angular momentum, skew-symmetric matrix and its properties.

math notations and denitions used in Witkin and Baraff’s course notes, such as linearmomentum, angular momentum, and skew-symmetric matrices and their properties, youcan safely skip this section.

To derive Newton-Euler equations, we begin with the momenta of the rigid body whosemass, position of the center of mass (COM), orientation, linear velocity of the COM, andangular velocity are m , x , R , v , and ω respectively (these denitions are the same as are

found in Witkin and Baraff’s course notes). The linear momentum P is computed as:P =

i

P i =i

µ ṙ i =i

µ(v + ω × r ′i )

= mv (18)

where r ′i = r i − x . Because i µr′i = 0 (property of the COM), the second term vanishes.

The angular momentum L about the COM is computed as:

L =i

L i =i

r ′i × P i

=i

µr′

i × (v + ω × r′

i )

= 0 +i

µ[r ′i ][ω ]r′

i =i

− µ[r ′i ][r′

i ] ω (19)

The notation [ a ]b denotes the cross product a × b with [a ] being the skew-symmetric matrixcorresponding to the vector a :

[a ] =0 − a3 a2a 3 0 − a1

− a2 a1 0 (20)

Therefore the following identities hold: [ a ]b = − [b ]a and [a ]T = − [a ].

Now recall the inertia tensor about the COM dened in Witkin and Baraff’s course notes:I c = i µ((r

′ T i r

′i )I 3 − r

′i r

′ T i ), where I3 is the 3 × 3 identity matrix. We can easily show that

I c = i − µ[r′i ][r

′i ] by verifying the identity − [a ][a ] = (a T a )I 3 − aa T . As a result, we write

the angular momentum of a rigid body as:

L = I c ω (21)

6


8/25

where the inertia tensor can be written as I c = RI 0R T . R is the rotation matrix corre-sponding to the orientation of the body and I 0 is the constant inertia tensor dened at zerorotation. From Witkin and Baraff’s course notes, we also learned that the angular velocityin the skew-symmetric form is related to the rotation matrix R as [ω ] = ṘR T .

With these denitions, we can derive the equations of motion for a rigid body. The equationscorresponding to the linear force can be evaluated as:

f = ṗ = m v̇ (22)

The equations corresponding to the torque can be evaluated as:

τ = L̇ = ˙(I c ω )= I c ω̇ + ˙(RI 0R T )ω = I c ω̇ + ṘI 0R T ω + RI 0 Ṙ T ω

= I c ω̇ + ṘR T I c ω + I c( ṘR T )T ω= I c ω̇ + [ω ]I c ω − I c[ω ]ω (Using the identity [ ω ]T = − [ω ])= I c ω̇ + ω × I c ω (23)

Combining Equation (22) and Equation (23), we arrive at the Newton-Euler equations:

mI 3 00 I c

v̇ω̇

+ 0ω × I c ω

= f τ

(24)

7


9/25


10/25

where j j is the j th column of the matrix J ω .

Using Equation (28) and Equation (30), we can write:

V = J vJ ω q̇ ≡ J (q )q̇ (31)

Substituting the above in Equation (27), we get:

T = 12

q̇ T J T M cJ q̇ (32)

Using the recipe for Lagrangian dynamics in Equation (13), we rst compute ∂T ∂ q̇j as:

∂T ∂ q̇ j

= 1

2 q̇ T J T M c(J ) j +

12

(J )T j M cJ q̇

= ( J )T j M cJ q̇ (33)

where the notation ( A) j denotes the j th column of the matrix A. The term ddt ∂T ∂ q̇j iscomputed as:

ddt

∂T ∂ q̇ j

= ( J )T j M cJ q̈ + ( J )T j M c ˙J q̇ + ( J )

T j Ṁ cJ q̇ + ˙(J )

T

j M cJ q̇ (34)

Now we evaluate the term ∂T ∂q j :

∂T ∂q j

= 1

2 q̇ T J T M c

∂J ∂q j

q̇ + 12

q̇ T J T ∂M c∂q j

J q̇ + 12

q̇ T ∂J T

∂q jM cJ q̇

= q̇ T ∂J T

∂q jM cJ q̇ +

1

2 q̇ T J T

∂M c

∂q jJ q̇ (35)

Using the above equations, we write:

ddt

∂T ∂ q̇ j

− ∂T ∂q j

= ( J )T j M cJ q̈ + ( J )T j M c ˙J q̇ + ( J )

T j Ṁ cJ q̇ −

12


J q̇

+ ˙(J )T

j M cJ q̇ −∂J ∂q j

q̇T

M cJ q̇ (36)

Comparing Equation (36) to Equation (17), it seems that we can view the rst term as themass matrix multiplying by q̈ and the rest terms as Coriolis and centrifugal forces. However,we will show that the third, fourth, and fth terms of this equation can be greatly reduced.

Third term:

(J )T j Ṁ cJ q̇ = ( J ω )T j İ cJ ω q̇ (The linear term in M c is constant: see Equation (27))

= jT j ˙(RI 0R T )ω ( j j represents the j th column of J ω : see Equation (29))term 3 = jT j [ω ]I c ω (From Equation (23)) (37)

9


11/25

Fourth term: The fourth term in Equation (36) can be simplied as:

12


J q̇ = 1

2(J ω q̇ )T

∂I c∂q j

J ω q̇

= 1

2ω

T ∂R

∂q jI

0R T + RI

0

∂R T

∂q jω = ω T

∂R

∂q jI

0R T ω

= ω T ∂R∂q j

R T I c ω

= ω T [ j j ]I c ω (From Equation (29))term 4 = − jT j [ω ]I c ω (Using the identity a · (b × c) = − b · (a × c)) (38)

Fifth term: To simplify the fth term in Equation (36), we explicitly express it using thelinear and angular components:

˙(J v ) j T ˙(J ω ) j T mI 3 00 I c J v q̇J ω q̇ −

∂J v∂q j q̇

T

∂J ω∂q j q̇

T

mI3 00 I c J

v q̇J ω q̇ (39)

The linear term can be extracted and simplied as:

m ˙(J v ) j −∂J v∂q j

q̇T

J v q̇ = mk

∂ (J v ) j∂q k

q̇ k −k

∂ (J v )k∂q j

q̇ k

T

J v q̇

= mk

∂ 2x∂q j ∂q k

q̇ k −k

∂ 2x∂q k ∂q j

q̇ k

T

J v q̇

term 5 (linear) = 0 (40)

The above derivation uses the property of the Jacobian of the linear velocity ( J v ) j = ∂ x∂q j ∀ j(See Equation (28)).

We now extract and simplify the angular term in Equation (39) as:

˙(J ω ) j −∂J ω∂q j

q̇T

I cJ ω q̇ =k

∂ j j∂q k

q̇ k −k

∂ jk∂q j

q̇ k

T

I c ω

=k

∂ j j∂q k

− ∂ jk∂q j

q̇ k

T

I c ω ≡k

z jk q̇ k

T

I c ω (41)

10


12/25

Now let us evaluate the term denoted by z jk . Consider the skew symmetric form:

[z jk ] =∂ j j∂q k

− ∂ jk∂q j

= ∂ [ j j ]

∂q k−

∂ [ jk ]∂q j

(Using linearity of the skew symmetric matrix)

= ∂ 2R

∂q j ∂q kR T +

∂R

∂q j

∂R T

∂q k−

∂ 2R

∂q k ∂q jR T +

∂R

∂q k

∂R T

∂q j (From Equation (29))

= ∂R

∂q jR T

∂R∂q k

R T T

− ∂R∂q k

R T ∂R∂q j

R T T

= − [ j j ][ jk ] + [ jk ][ j j ] (Using the identity [ a ]T = − [a ])= [ jk × j j ] (Using the identity [ a × b ] = [a ][b ] − [b ][a ])

⇒ z jk = jk × j j = [ jk ] j j (42)

Substituting the above in Equation (41), we get:

k

z jk q̇ k

T

I c ω =k

[ jk ] j j q̇ k

T

I c ω

=k

[ jk ]q̇ k j j

T

I c ω

=k

jk q̇ k j j

T

I c ω

= ([ J ω q̇ ] j j )T I c ω = ([ω ] j j )T I c ωterm 5 (angular) = − jT j [ω ]I c ω (43)

Put it together: Finally, we substitute the terms computed in Equation (37), Equa-tion (38), Equation (40) and Equation (43) into Equation (36) and rewrite it as:

ddt

∂T ∂ q̇ j

− ∂T ∂q j

= ( J )T j M cJ q̈ + ( J )T j M c ˙J q̇ + j

T j [ω ]I c ω

= (J )T j M cJ q̈ + (J )T j M c ˙J + ( J )

T j [ω̃ ]M cJ q̇

where [ω̃ ] = 0 00 [J ω q̇ ] (44)

Writing the equations for all the q j in the vector form, we get:ddt

∂T ∂ q̇

− ∂T ∂ q

= J T M cJ q̈ + J T M c ˙J + J T [ω̃ ]M cJ q̇ (45)

Note that the second term in the above equation involves the computation of ˙J that can becomputed as k

∂J ∂q k

q̇ k . In other words, we will need to compute the rst and the secondderivatives of a rotation matrix (i.e. ∂R∂q j and

∂ 2 R∂q i ∂q k

) in order to compose Jacobian J and itstime derivative ˙J in Equation (45).

11


13/25

Derivation using Newton-Euler equations. We can alternatively derive the result inEquation (45) from the Newton-Euler equations in Equation (24). Using Equation (31), wesubstitute the Cartesian velocities v , ω in terms of the generalized velocities q̇ into Equa-tion (24) and get:

M c( ˙J q̇ ) + 0(J ω q̇ ) × I cJ ω q̇ = f

τ

⇒ M cJ q̈ + M c ˙J q̇ + [ ω̃ ]M cJ q̇ = f

τ (46)

From the principle of virtual work in Equation (3), we convert the Cartesian-space forcesto the Generalized space by pre-multiplying the above equation with the transpose of theJacobian J :

J T M cJ q̈ + J T M c ˙J + J T [ω̃ ]M cJ q̇ = J T v f + J T ω τ (47)

The LHS of Equation (47) is identical to the RHS of Equation (45) and they are of the formM (q )q̈ + C (q , q̇ ) = Q , where the Mass matrix, the Coriolis term and the generalized forcesare dened as:

M (q ) = J T M cJ C (q , q̇ ) = ( J T M c ˙J + J T [ω̃ ]M cJ )q̇

Q = J T v f + J T ω τ (48)

12


14/25

5 Articulated Rigid Body Dynamics

We now derive the equations of motion for an articulated rigid body structure. We followthe derivation of rigid body dynamics in generalized coordinates from Section 4.

An articulated rigid body system is represented as a set of rigid bodies connected through joints in a tree structure. Every rigid link has exactly one parent joint. The joint corre-sponding to the root of the tree is special; the root link does not link to any other rigidlink. The generalized coordinates are therefore the DOFs of the root link of the tree (thatmay represent the global translation and rotation) and the joint angles corresponding to theadmissible joint rotations for all the other joints.

5.1 Denitions

The state of an articulated rigid body system can be expressed as ( x k , R k , v k , ω k ), wherek = 1 , · · · , m and m is the number of rigid links. Here x k and Rk are the position of theCOM and the orientation of the rigid link k , and (v k , ω k ) are the linear and angular velocityof the rigid link k viewed in the world frame. Similarly, we dene the Cartesian force andtorque applied on rigid link k as (f k , τ k ), both of which are expressed in the world frame.

The same articulated rigid body system can be represented in generalized coordinates. Wedene the generalized state as ( q , q̇ ), where q = ( q 1, . . . , q k , . . . , q m ) and each q k is the setof DOFs of the joint that connects the link k to its parent link (see Figure 1).

k =1

k =2

k =3

k =4

q 1={q11,q12}

universal joint

hinge joint

ball joint

q2={q

21,q

22,q

23}

q4={q

41}

q3={q

31}

Figure 1: An articulated system.

We list a few notations and denitions for an articu-lated rigid body system with m rigid links.

• p(k) returns the index of the parent link of linkk. This is illustrated in Figure 1, p(4) = 2. p(1, k ) returns the indices of all the links in thechain from the root to the link k (including k),e.g. p(1, 4) = {1, 2, 4}

• n(k) returns the number of DOFs in the jointthat connects the link k to the parent link p(k).For example in Figure 1, n(2) = 3, n(3) = 1etc. We denote the total number of DOFs inthe system by n . e.g. n = 7 in Figure 1.

• Rk is the local rotation matrix for the link k anddepends only on the DOFs q k . R0k is the chain of rotational transformations from the world frameto the local frame of the link k. Therefore, R0k = R0 p(k )R k . Since the link 1 does nothave a parent link, R 0 p(1) = I 3.

13


15/25

5.2 Cartesian and generalized velocities

For a single rigid body, Equation (28) and Equation (30) describe the relation between theCartesian velocities and the generalized velocities. For an articulated rigid body system,we use the same recipe as rigid body dynamics in Section 4 and dene the Jacobians for

each rigid link that relate its respective Cartesian velocities to the generalized velocity of theentire system.

We start with deriving the relation for the angular velocity. The angular velocity (in skew-symmetric matrix form) of link k viewed in the world frame is:

[ω k ] = Ṙ 0k R 0kT = ˙(R 0 p(k )R k )(R

0 p(k )R k )

T

= ( Ṙ 0 p(k )R k + R0 p(k )Ṙ k )R

T k R

0 p(k )

T

= Ṙ 0 p(k )R0 p(k )

T + R0 p(k ) Ṙ k RT k R

0 p(k )

T ≡ [ω p(k )] + R0 p(k )[ω̂ k ]R0 p(k )

T (49)

In the above equation, we dene [ ω̂ k ] = Ṙ k R T k that denotes the angular velocity of the linkk in the frame of its parent link p(k) since the rotation matrix Rk is the rotation of therigid link k with respect to p(k). We can further write ω̂ k = Ĵ ωk q̇ k where Ĵ ωk is the local Jacobian matrix that relates the joint velocity of link k to its angular velocity in the frameof the parent link p(k). The dimension of Ĵ ωk is 3× n (k).

Using a property of skew symmetric matrices, [ R ω ] = R [ω ]R T , we can express Equation (49)in vector form as:

ω k = ω p(k ) + R 0 p(k ) Ĵ ωk q̇ k

=l∈ p(1 ,k )

R 0 p( l) Ĵ ωl q̇ l (By unrolling the recursive denition)

≡ J ωk q̇ (50)

where the Jacobian J ωk is:

J ωk = Ĵ ω1 . . . R 0 p(l) Ĵ ωl . . . 0 . . . (51)

Note that the zero matrices 0 of size 3× n (l) in J ωk correspond to joint DOFs q l that arenot in the chain of transformations from the root to the link k. Let us look at a couple of examples using the articulated rigid body system in Figure 1:

ω 1 = ( Ĵ ω1 0 0 0 )q̇ω

4 = ( Ĵ

ω1 R0

1 Ĵ

ω2 0 R0

2 Ĵ

ω4) ˙q

where Ĵ ω1 ∈ ℜ3× 2, Ĵ ω2 ∈ ℜ3× 3 and Ĵ ω4 ∈ ℜ3× 1. Depending on the representation of therotation qk , Ĵ ωk can assume different values and dimensions. For example, if the jointbetween link 1 and link 2 in Figure 1 is represented as three Euler rotations, R (x ) , R (y ) , andR (z ) such that R 2(q 2) = R (x )(q 21)R (y )(q 22)R (z )(q 23), we have:

Ĵ ω2 =100

R (x ) 010

R (x )R (y) 001

(52)

14


16/25

If the joint is represented as a quaternion or an exponential map, Ĵ ωk does not have a simpleform. As an example, the relation between the rotation matrix R k and the exponential maprepresentation qk = ( q k1, q k2, q k3) can be written as:

R k (q k ) = e[q k ] = I 3 + sinθ

θ [q k ] +

1 − cosθ

θ2 [q k ]

2 (53)

where θ = || q k || . The Jacobian Ĵ ωk can be derived by equating the result of Ṙ k R T k to [ Ĵ ωk q̇ k ]:

Ĵ ωk = Rk I 3 − 1 − cosθ

θ2 [q k ] +

θ − sinθθ3

[q k ]2

= I3 + 1 − cosθ

θ2 [q k ] +

θ − sinθθ3

[q k ]2 (54)

For the case when θ → 0, Rk and Ĵ ωk can be approximated as follows:

R k = I3 + [q k ] + 12[q k ]

2

(55)Ĵ ωk = I3 +

12

[q k ] + 16

[q k ]2 (56)

Similar to the angular velocity, the linear velocity of the center of mass of the link k can beexpressed in terms of the generalized velocity:

v k = J vk q̇ , where J vk = ∂ x k

∂ q =

∂W 0k c k∂ q

. (57)

where the chain of homogeneous transformations from the world frame to the local frame

of link k is denoted as W 0k . Note that W

0k is different from R

0k in that W

0k includes thetranslational transformations. ck is a constant vector that denotes the center of mass of link

k in its local frame.

We can concatenate the Cartesian velocities into a single vector V k and denote the relationas:

V k = J k q̇

where V k = vk

ω kand J k =

J vkJ ωk

(58)

5.3 Equations of motion

We now derive the equations of motion of an articulated rigid body system in generalizedcoordinates. The kinetic energy T of the entire system can be expressed as the sum of kineticenergies of all the rigid links as T = k T k . Therefore the equations of motion of the system

15


17/25

can be computed as:

ddt

∂T ∂ q̇

− ∂T ∂ q

= d

dt∂ k T k

∂ q̇−

∂ k T k∂ q

=k

d

dt

∂T k

∂ q̇−

∂T k

∂ q

=k

J T k M ck J k q̈ + J T k M ck ˙J k + J

T k [ω̃ k ]M ck J k q̇

=k

J T k M ck J k q̈ +k

J T k M ck ˙J k + J T k [ω̃ k ]M ck J k q̇ (59)

In deriving the above equation, we use the equations of motion in generalized coordinatesfor a single rigid body dened in Equation (45) subscripted by k for the dynamics of k th linkin the multibody system. The Jacobian J k for the k th link is dened in Equation (58).

16


18/25

6 Conversion between Cartesian and Generalized Co-ordinates

In practice, we often want to use third-party rigid body simulators rather than develop

our own. There are a few widely used physics engines that provide efficient, robust, andfairly accurate rigid body simulation and collision handling. Open Dynamic Engine (ODE),PhysX, and Bullet are perhaps the most popular free choices among game developers andacademic researchers. These commercial simulators use the maximal representation ratherthan generalized coordinates described above. That is, these simulators represent each link inthe articulated rigid body system as six DOFs, leading to a redundant system with additionalconstraints between links. A common practice is to develop control algorithms in generalizedcoordinates and do forward simulation using a commercial physics engine, such as ODE. Thisrequires some conversion between Cartesian and generalized coordinates.

6.1 Velocity conversion

We can concatenate all 2 m Jacobian matrices corresponding to each link into a single Jaco-bian that relates the generalized velocity to the Cartesian velocity of each link:

V ≡

v 1...

v mω 1...

ω m

=

J v1...

J vmJ ω1

...J ωm

q̇ 1...

q̇ m

≡ J vJ ω q̇ ≡ J q̇ (60)

Typically, the Jacobian J is full column rank because the number of DOFs in the maximalrepresentation is more than that in the generalized representation, i.e 6 m > n . To computeq̇ from V , we will end up solving an over-constrained linear system. We can use the pseudoinverse of J to compute q̇ :

q̇ = J + V (61)

where the pseudo-inverse notation J + = ( J T J )− 1J T . If this least-squares solution does notexactly solve the linear system (i.e. J q̇ = V ), it indicates that V cannot be achieved in thegeneralized coordinates without violating constraints of the system (e.g. constraints thatkeep links connected).Computing J + may be expensive for a system with many rigid links. Alternatively, we canrewrite the equation using the relative velocity between a child and a parent link expressedin the local frame of the parent, instead of using velocities of each link expressed in the worldframe. As an example, we write the simplied expression for the angular velocity of link k

17


19/25

using Equation (50) as:

ω k − ω p(k ) = R0 p(k ) ω̂ k = R0 p(k ) Ĵ ωk q̇ k

⇒ (− I 3 I3) ω p(k )

ω k = R0 p(k ) Ĵ ωk q̇ k (62)

Combining these equations for all the links, we get:

D ω = DJ ω q̇ = blockdiag( Ĵ ω1, . . . , R 0 p(m ) Ĵ ωm )q̇

= blockdiag( I 3, . . . , R 0 p(m ))blockdiag( Ĵ ω1, . . . , Ĵ ωm )q̇

≡ R Ĵ ω q̇ (63)

where D is a constant matrix that encodes the connectivity between links. For example,matrix D for the system in Figure 1 looks like:

D =

I 3 0 0 0

− I 3 I3 0 00 − I 3 I3 00 − I 3 0 I 3

(64)

The relations between ω̂ and ω , and Ĵ ω and J ω follow from Equation (63):

ω̂ = RT D ωand Ĵ ω = RT DJ ω (65)

The matrix Ĵ ω being block diagonal is much sparser as compared to J ω .

If q̇ satises the over-constrained system of equations V = J q̇ , using any n independentconstraints out of 6 m to solve this linear system will result in the same q̇ . This can beexplained by the problem of tting an unknown plane to 6 m 3D points as Ax = b , whereA ∈ ℜ6m × 3. If all 6m points happen to lie on a plane, i.e. there exists an x that exactlysatises the over-constrained system, any three distinctive points we pick as the constraintswill result in the same plane. Therefore, if we know V can be achieved in the generalizedcoordinates, we can pick a subset of rows from J to form a J ′ such that the rank of J ′ is n ,and compute q̇ = J ′ + V ′ , where V ′ are the velocity components corresponding to the rowsin J ′ . The solution q̇ to this system will be the same for any J ′ .

For a system with only rotational DOFs, it is sufficient to invert only J ω which is also a fullcolumn rank matrix ( J ω ∈ ℜ3m × n and n ≤ 3m ). This is because each rotational joint canhave at most three independent DOFs. We then can compute the velocities of the rotationalDOFs q̇ as:

q̇ = J +ω ω

= Ĵ +ω RT D ω = Ĵ +ω ω̂

= blockdiag Ĵ +ω1, . . . , Ĵ +ωm ω̂

or q̇ k = Ĵ +ωk ω̂ k , k ∈ 1 . . . m (66)

18


20/25

From this formulation, we see that the problem of computing the pseudo-inverse of a matrixJ ω is reduced to computing m pseudo-inverses of much smaller constant-sized matrices Ĵ ωk .Note that if q̇ computed in Equation (66) exactly satises the linear system ω = J ω q̇ , it alsosatises the linear velocity relation v = J v q̇ .

For systems that include translational DOFs as well, we can separately solve for the rotationalDOFs as in Equation (66) and solve for the translational DOFs for any link k as q̇ k = J +vk v k .In most of the cases, only the root joint has translational DOFs making the computation of generalized translational velocities extremely simple as J v1 becomes an identity matrix.

6.2 Force conversion

The relation between the Cartesian force and the generalized force can be found in Equa-tion (3):

Q =k

J ′

vkT

f k + J T ωk τ ′

k = J ′

v T J T ω f τ ′ , where J ′

vk = ∂ r k∂ q (67)

where r k is the point of application of the Cartesian force f k and τ ′

k is the body torque appliedto link k expressed in the world frame.

Note. Body torque τ ′

k is different from body torque τ k in Equation (48):

Q = J T v f + J T ω τ

Here, τ

′

k is the torque applied on link k in the world frame and does not

include the torqueinduced by the linear forces f k . However, τ k in Equation (48) includes the torque [ r k − x k ]f kdue to each force f k (x k is the COM of the link k). As a result, the linear Jacobian J v inEquation (48) is computed at the COM of the respective rigid link and J

′

v in Equation (67)is dened for the point of application of the force. It is easy to verify that J

′

vkT

f k = J T vk f k +J T ωk [r k − x k ]f k . i.e. τ k = τ

′

k + [r k − x k ]f k .

Often many controllers (such as a tracking controller) nd it convenient to compute theCartesian-space joint torques in the local frame of the parent link rather than body torquesin the world frame. Joint torque ˆτ k in the frame of a parent link p(k) is dened suchthat positive torque in the world frame R0 p(k ) τ̂ k is applied to the link k and negative torque

− R0 p(k ) ˆτ k is applied to the parent link p(k). Therefore, the body torque τ

′

k applied to thelink k can be written in terms of the joint torques as τ

′

k = R 0 p(k ) τ̂ k − l R0k τ̂ l , ∀l : k = p(l).

Collecting the body torques for all the rigid links in the vector τ , the relation between thebody torques and the joint torques can be dened as:

τ ′

= D T R τ̂ = ( R T D )T τ̂ (68)

19


21/25

where R, D are dened in Equation (65). We now substitute Equation (68) in Equation (67)and get:

Q = J ′

vT

J T ω f

(R T D )T τ̂

= J ′

vT (R T DJ ω )T f τ̂

= J ′

vT

Ĵ T ω f

τ̂ (Using Equation (65)) (69)

Equation (69) gives the relation to convert the given cartesian forces f and joint torques ˆτ to the generalized forces Q .

We now describe the process to convert the given generalized forces Q to cartesian forcesand torques. In general, the transposed Jacobian in Equation (67) can be inverted usinga pseudo-inverse to get the Cartesian forces and torques. Note that the relation representsan under-constrained system when solving for f and τ

′

. This is because the size of theunknowns is 6m and the number of constraints are n with n ≤ 6m . Therefore, we getparticular solutions for the Cartesian forces and torques out of many possible solutions.

Based on the information about the form of Q , we can solve for the Cartesian forces andtorques in different ways. We describe the solutions to the following cases:

1. General case. In the most general case, the points of application of the Cartesianforces are not known. Therefore, we cannot use the Jacobian J

′

v in Equation (67). Thisforces us to assume the points of application to be the COM of each link and computethe torques τ instead of body torques τ

′

. i.e. , we can invert the transposed Jacobian

by computing its pseudo-inverse that results in a particular least-squares solution forf and τ as:

f τ = J v T J T ω + Q (70)If the points of the force application are known, the Jacobian in Equation (69) can beinverted to obtain the forces f and the joint torques ˆτ .

2. No linear forces. The more common case for many controllers involves the conver-sion of only the joint torques from generalized to the Cartesian coordinates. Therefore,the linear forces f are zero and Equation (69) can be simplied further to result in thefollowing conversion relation:

τ̂ = ( Ĵ T ω )+ Qor ˆτ k = ( Ĵ T ωk )+ Q k ∀k ∈ 1 . . . m (71)

where Q k denotes the components of the generalized forces corresponding to the rota-tional DOFs q k . Note that the size of the matrix Ĵ T ωk is n(k) × 3 and n(k) ≤ 3. Thisimplies that we get a particular least-squares solution for each ˆτ k out of possibly manysolutions that would give rise to the same Q k using the relation Q k = Ĵ T ωk τ̂ k .

20


22/25

7 Recursive Inverse Dynamics

As Featherstone pointed out, inverse dynamics can be computed efficiently and much fasterby exploiting the recursive structure of an articulated rigid body system. A recursive algo-rithm allows computation of inverse dynamics in linear time proportional to the number of links in the articulated system. In this chapter, we will use our formulation to construct arecursive inverse dynamics algorithm.

7.1 Dynamics in the local frame

For each body link k , we dene ck as the center of mass in the local frame and d c(k )[i ] as thevector between the joint connecting to the parent link and the joint connecting to the i-thchild of link k, where c(k) returns the indices of child links of the link k . As a shorthand, wedene ĩ = c(k)[i] and use the notation d ĩ hereafter. Figure 2 illustrates the notations using

the same structure from the previous example.

k =1

k =2

k =3

k =4

q1={q

11,q

12}

q2={q

21,q

22,q

23}

q4={q

41}

q3={q

31}

c2

d 3

d 4k =2

f 2, τ 2 -f 3, -τ 3

-f 4, -τ 4

Figure 2: An articulated system.

The goal of the inverse dynam-ics algorithm is to compute theforce and the torque transmittedbetween links. For each link k,we dene the force and torque re-ceived from the parent link f k andτ k . Similarly, the force and thetorque from the i-th child are de-noted as − f ̃i and − τ ĩ . Please see

Figure 2 for illustration.To compute these forces andtorques, let us rst write downNewton-Euler equations for link kin its local frame. We will use thenotation a ℓk to denote a vector a expressed in the local frame of link k.

m k (v̇ k )ℓ = f ℓk −ĩ∈ c(k )

R ĩ f ℓĩ (72)

Ick ( ˙

ω

k )ℓ

+ ω

ℓ

k ×I

ckω

ℓ

k = τ

ℓ

k −c

k ×f ℓ

k − ĩ∈ c(k )(R

ĩτ

ℓ

ĩ + (d

ĩ −c

k ) × (R

ĩf ℓ

ĩ )) (73)

The inverse dynamics algorithm visits each link twice in two recursive passes. In the rst pass,the velocity and acceleration of each link is computed and expressed in the local frame. Inthe second pass, these terms are plugged into the above Newton-Euler equations to computeforces and torques transmitted between the links.

21


23/25

7.2 Pass 1: Compute velocity and acceleration

The articulated rigid body system can be represented as a tree structure, where every linkfrom the root to the leaves will be visited once in Pass 1. The algorithm is recursive becausethe computation at each link depends on the computation of its parent link. Let us rst

discuss the computation for a general link and take care of the special case of the root laterin this section.

Assuming the velocity and the acceleration of the parent link, v ℓ p(k ) , ωℓ

p(k ) , (v̇ p(k ))ℓ, ( ˙ω p(k ))ℓ ,

are already computed from the previous iteration, the COM of the link k in the world frameis W 0k c k . The linear velocity of the link k in Homogeneous coordinates is then expressed as:

v k = Ẇ 0k c k = Ẇ 0 p(k )W k c k + W0 p(k )Ẇ k c k

= Ẇ 0 p(k )(c p(k ) + W k c k − c p(k )) + W0 p(k )Ẇ k c k

= v p(k ) + Ẇ 0 p(k )(W k c k − c p(k )) + W0 p(k )Ẇ k c k (74)

where the fourth element of the vector W k c k − c p(k ) and that of the vector Ẇ k c k are bothzero. This will result in elimination of the translation part of the transformation. We cantherefore express v k in Cartesian space as:

v k = v p(k ) + Ṙ 0 p(k )(R k c k + d k − c p(k )) + R0 p(k ) Ṙ k c k

= v p(k ) + [ω p(k )]R 0 p(k )(R k c k + d k − c p(k )) + R0 p(k )[ω̂ k ]R k c k (Using [ω ] = ṘR T )(75)

So far vk is computed in the world frame and we would like to transform it to the localframe.

v ℓk = R 0k T v k = R T k R 0 p(k ) T v k= R T k (v

ℓ p(k ) + [ω

ℓ p(k )](R k c k + d k − c p(k )) + [ ω̂ k ]R k c k ) (Using R [ω ]R T = [R ω ])

= R T k (vℓ p(k ) + [ω

ℓ p(k )](d k − c p(k ))) + R

T k (([ω

ℓ p(k )] + [ω̂ k ])R k c k )

= R T k (vℓ p(k ) + ω

ℓ p(k ) × (d k − c p(k ))) + ω

ℓk × c k (Using R [ω ]R T = [ R ω ]) (76)

Similarly, we can compute the angular velocity of the link k in the world frame and thentransform it to the local frame.

[ω k ] = Ṙ 0k R 0kT = ( ˙R 0 p(k )R k )(R

T k R

0 p(k )

T ) = Ṙ 0 p(k )R k RT k R

0 p(k )

T + R 0 p(k ) Ṙ k RT k R

0 p(k )

T

= [ω p(k )] + R 0 p(k )[ω̂ k ]R 0 p(k ) T (77)

[ω ℓk ] = [R 0kT

ω k ] = R 0kT [ω k ]R 0k

= R T k (R 0 p(k )T [ω p(k )]R 0 p(k ) + [ ω̂ k ])R k = R

T k ([ω

ℓ p(k )] + [ω̂ k ])R k

ωℓk = R

T k (ω

ℓ p(k ) + ω̂ k ) (78)

22


24/25

Next, we compute the linear and angular acceleration for the link k. Note that the linearacceleration must be computed in the world frame rst and then transformed into the localframe. If we instead take the time derivative on v ℓk , i.e. v̇ ℓk , the result is different from thetrue linear acceleration ( v̇ k )ℓ, as the former does not take into account the Coriolis forcesdue to the moving frame.

v k = R 0k vℓk = R 0 p(k )(v

ℓ p(k ) + ω

ℓ p(k ) × (d k − c p(k ))) + R

0k ω

ℓk × c k

(v̇ k )ℓ = R 0kT

Ṙ 0 p(k )(vℓ p(k ) + ω

ℓ p(k ) × (d k − c p(k ))) + R

T k (v̇

ℓ p(k ) + ˙ω

ℓ p(k ) × (d k − c p(k )))

+ R 0kT

Ṙ 0k (ωℓk × c k ) + ˙ω

ℓk × c k

= R T k (v̇ℓ p(k ) + ω

ℓ p(k ) × v

ℓ p(k ) + ˙ω

ℓ p(k ) × (d k − c p(k )) + ω

ℓ p(k ) × (ω

ℓ p(k ) × (d k − c p(k ))))

+ ω ℓk × (ωℓk × c k ) + ˙ω

ℓk × c k (Using R 0k

T Ṙ 0k = [ω ℓk ]])= R T k (( v̇ p(k ))

ℓ + ˙ω ℓ p(k ) × (d k − c p(k )) + ωℓ

p(k ) × (ωℓ

p(k ) × (d k − c p(k ))))+ ω ℓk × (ω

ℓk × c k ) + ˙ω

ℓk × c k (Using (v̇ p(k ))ℓ = v̇ ℓ p(k ) + ω

ℓ p(k ) × v

ℓ p(k )) (79)

In contrast, the angular acceleration ( ˙ω k )ℓ is the same as ˙ω ℓk .

( ˙ω k )ℓ = R T k (( ˙ω p(k ))ℓ + ˙̂ω k + ω ℓ p(k ) × ω̂ k ) (80)

Base case: The base case of this recursive pass computes the velocity and the accelerationof the root link. We can simplied the computation as follows:

v ℓ0 = ωℓ0 × c0

ωℓ0 = R

T 0 ω̂ 0

(v̇ 0)ℓ = ω ℓ0 × (ωℓ0 × c0) + ( ˙ω 0)

ℓ × c0( ˙ω 0)ℓ = R T 0 ˙̂ω 0 (81)

Translational degrees of freedom: So far we assume that the links are connected byonly rotational DOFs, but the formulation can be easily generalized to translational DOFs.Let us rewrite Equation 74 with the assumption that W k has only translational DOFs qk ,i.e. R k = I 3 and d k = q k .

v k = v p(k ) + Ṙ 0 p(k )(c k + q k − c p(k )) + R0 p(k ) q̇ k (82)

Transforming into the local frame of the link k, we get

v ℓk = vℓ p(k ) + ω

ℓ p(k ) × (c k + q k − c p(k )) + q̇ k (83)

The angular velocity and accelerations can be derived in a similar way.

ωℓk = ω

ℓ p(k )

v̇ ℓk = v̇ℓ p(k ) + ˙ω

ℓ p(k ) × (c k + q k − c p(k )) + ω

ℓ p(k ) × q̇ k + q̈ k

ω̇ℓk = ˙ω

ℓ p(k ) (84)

23


25/25

7.3 Pass 2: Compute force and torque

The second pass computes forces and torques transmitted between body links. The algorithmvisits each link once from the leaf nodes to the root. At each iteration, we compute f ℓk andτ ℓk , given the velocity and the acceleration computed by Pass 1 and all the forces and torques

from the child links. Specically, we will plug v ℓk , ω ℓk , (v̇ k )ℓ, ( ˙ω k )ℓ, and all the f ℓ̃i and τ ℓĩ intoEquation 72 and Equation 73.

Base case: The leaf nodes have no child links connected to them. Therefore, f ̃i and τ ĩ arezero at each leaf node. Similarly for the root link, f 0 and τ 0 are zero.

Gravity: Instead of treating gravity as an external force, we can consider the effect of gravity by conveniently offsetting the linear acceleration of the root link by − g .

( ˙v

0)ℓ

= ω

ℓ

0 × (ω

ℓ

0 ×c

0) + ( ˙ω

0)ℓ

×c

0 −R T

0g

(85)

This is equivalent to adding a ctitious force, − m k g , to each link. The rest of the algorithmremains unchanged.

Acknowledgements

We thank Jeff Bingham, Stelian Coros, Marco da Silva, and Yuting Ye for proofreading thisdocument and providing valuable comments.

Documents

A Quick Tutorial on Multibodmy Dynamics