Computing constraint Jacobians

Claude Lacoursie`reHPC2N/UMIT

andDepartment of Computing Science

Umea UniversitySE-901 87, Umea, Swedenclaude@hpc2n.umu.se

December 1, 2011

1 Introduction

These notes should explain more about how to compute Jacobians for simulating simplemultibody systems which are only subject to distance and contact constraints.

2 Whats a Jacobian?

Properly speaking, a Jacobian is a gradient and for a given scalar function g(x) of areal vector x Rn, thats defined as the row vector

g

x= g = G =

[gx1

gx2

. . . , gxn

]. (1)

In my convention here, lower case letters are either scalars or vectors, and upper caseletters refer to matrices. I will use g(x) for any function, but in particular, the functionsthat define constraints when g(x) = 0.

Consider the simple example where g(x) = x, i.e., the norm of the vector x whichcan also be written as

xTx =

x x, where xT is the transpose of vector x. I write

the dot-product of two vectors of compatible dimensions as either xT y or x y. Theconvention here is that vectors are always column vectors, and so xT is a row vector.Using standard matrix-matrix multiplication rules, xTx =

ni=1 x

2i , and this is a scalar

as it should.

1

Now, think of x as a scalar and now, by chain rule, we have

dx2

dx=

1

2x2

dx2

dx=

xx2. (2)

Were almost there. Now, consider that x is an n-dimensional vector and use the chainrule again so that

g = x = xTx =

1

2xTx

(xTx)

=1xTx

xT =1

xxT .

(3)

This is just the transpose of the unit vector pointing in the direction of x. The reasonto keep the fractions in front is to separate the vectors and the scalars. The transposehas to be there because of the convention for g being a row vector.

Now, if the function g(x) is also a vector of m-dimensions, meaning that

g(x) =

g1(x)g2(x)

...gm(x)

(4)we can still compute gi for each components. Since that gives us a row vector, thecomplete Jacobian g = G is now an m n matrix, namely

g = G =

g1x1

g1x2

g1xng2x1

g2x2

g2xn...

.... . .

...gmx1

gmx2

gmxn .

(5)That looks daunting at first but its going to be very simple. Recall that in our cases,the function g(x) is a constraint, and that usually involves only two bodies. So, if x isthe vector of coordinates of the entire system, well have gi/xj = 0 unless j is thecoordinate of one body constrained by gi.

Also, dont worry too much about matrices and storage, since you only need thenonzero components. In the case of particles or rigid body constraints, youll need 2msmall matrices to store a Jacobian and you can put that in your data structure for aconstraint, or keep pointers to a storage area if you like. Thats what we do in theAgX code to keep everything nice and tight, i.e., we use structs of arrays instead ofarrays of structs.

3 Point particles

First consider a particle at position x and the constraint g(x) = x l. Thats asimple pendulum hooked to the origin by a string of length l. The Jacobian for that

2

x(1)

x(2)

x(1) x(2)

G(1)

G(2)

Figure 1: A distance constraint between two particles. The Jacobians here are simplynormals aligned along the center line between the particles. What else couldit be!

is given by Eqn. (3). As the Jacobian points in the direction of fastest increase, wecan move along (1/x)xT to move radially toward the origin. If you remember thedefinition of a constraint force as GT, you can guess that < 0 is the force neededto keep the particle from flying away.

Now consider two particles x (1), x (2) and the length constraint g(x) = x (1) x (2)l = 0, where l is a constant. Here, I am writing x as the set of generalized coordinatesso that

x =

[x (1)

x (2)

]=

x(1)1

x(1)2

x(1)3

x(2)1

x(2)2

x(2)3

(6)

where x(1)j , j = 1, 2, 3 are the coordinates of the first particle, and same for the second.

For the case of particles, we have three dimensions for positions and three dimensionsfor velocities. Youll see later that this does not work exactly the same way for rigidbodies because of the quaternion representation of the rotations. But dont worry yet.

So, going back to the definition in Eqn. (1), we need to get all partial derivatives ofg(x) with respect to all coordinates. But you might already guess that there is a lot ofsymmetry between whats going on with x (1) and x (2). To make the chain rule clear

3

and easy to use here, write r = x (1) x (2). Thats just a 3D vector now and we canget the partials easily

r

x (1)=x (1) x (2)

x (1)= I

r

x (2)=x (1) x (2)

x (2)= I

(7)

where I is the 3 3 identity matrix. Recall now that r is a 3D vector so thatr/x (i), i = 1, 2 must be a 3 3 matrix as shown already. So now, using the chainrule, we have

g

x (1)=

1

2rrT r

x (1)=

1

rrT r

x (1)

=1

rrT I =

1

rrT .

(8)

Clearly, for particle 2, we get the opposite sign

g

x (1)= 1rr

T . (9)

Recall now that the gradient of a function is the direction of fastest increase and clearlynow, this shows that moving the particles in the central direction 1/rr changesthe distance as fast as possible. Which is obvious.

What happens if we have a long chain of n particles each linked to its neighbor witha distance constraint? That gives us m = n 1 constraint equations and so we needto compute the beast of a matrix described in Eqn. (5). If you look at that matrix,the particles identify the columns, and the constraints are on the rows. Lets label theconstraints from 1 to n 1 so that constraint gi is the one that keeps particles x (i)and x (i+1) at the given distance lii. So now, for a given constraint gi(x), we will havegi/x

(j) = 0 unless j = i or j = i1. Given what was computed for the two particlecase, well then have

gix (i)

=1

ri,i+1 (x(i) x (i+1)) = 1rii+1ri,i+1, where ri,j = rij = x

(i) x (j). (10)

I use a coma in ri,i+1 to avoid confusion but rij in general. Similarly,

gix (i+1)

= 1ri,i+1ri,i+1. (11)

Lets introduce the unit vectors uij = (1/rij)rij and so when you put all theseconstraints together in a matrix form, you have

G =

u12 u12 0 0 . . . 00 u23 u23 0 . . . 00 0 u34 u34 . . . 0...

.... . .

. . ....

...0 0 0 un2,n1 un1,n 00 0 0 0 un1,n un1,n

. (12)

4

And thats very, very sparse.This is all there is to distance constraints and as it turns out, except for a small

issue regarding quaternions, thats the same for rigid bodies.

4 Rigid body constraints

For rigid bodies, the generalized coordinates are labeled q because they are not onlyCartesian, but contain also the rotational degrees of freedom. Though there are onlythree of these, and though the angular velocity is a simple 3D vector, the rotationof the body is best represented as a quaternion. Well, lets write the velocity vector ofa rigid body with coordinate x as

v =

[x

]. (13)

Thats not the same as q because if we use the position x and the quaternion e asgeneralized coordinates, thats a 7 dimensional vector. What happened? Well, thevelocity of the quaternion is given by the well known formula

e =1

2 ? e, (14)

where is here a pure imaginary quaternion. I did not change the letter for it becausethere is a straight forward way to promote 3D vectors to quaternions. If we separatequaternions in terms of real (scalar) and imaginary components (a 3D vector), we canwrite

e =

[esev

](15)

and so for the 3D vector , or any other vector for that matter, we have

=

[0

](16)

with a flagrant abuse of notation. The quaternion product in Eqn. (14) can be writtenin matrix form

e =1

2 ? e =

1

2GT (e)

G(e) = [ev esI3 ev] , (17)and the convention used there is that the cross product of two 3D vectors x y canbe represented as

x y = xy = 0 x3 x2x3 0 x1x2 x1 0

y1y2y3

. (18)If you look at Eqn. (17), you can now see that it is possible to write

q = K(q)v, (19)

5

A contact constraint between two bodies

A distance constraint between two bodies

x(1)

x(2)

r(12) l0 = 0

p(1)

p(2)

A ball joint constraint between two bodies

x(1)

x(2)

r(12) = 0

p(1)p(2)

x(1)p(1)

x(2)

p(2)

n(1)

n(2)

Figure 2: Various constraints between two rigid bodies which all rely on the same basicBall Joint Jacobian. Note that normals at contacts are not unambiguouslydefined.

6

where v only contains x and , and K(q)) is a configuration dependent matrix thatdepends on the quaternions of your system of rigid bodies. Annoying as it may seem,that turns out to be a minor problem only.

Now, generally, if you have a constant angular velocity and want to integrate thedifferential equation in Eqn. (14) from t = 0 to t = h, you get the result

e(h) = exp(h

2)e(0), (20)

and the exponential of a pure imaginary quaternion is itself a quaternion, thoughnot pure imaginary, and it is defined as

exp(h

2) =

[cos(h2 )sin(h2 ) .

](21)

So when you integrate your multibody system, you can do all your computations for thevelocity v as defined in Eqn. (13), and then update your quaternions as in Eqn. (21),which will keep unit quaternions as you integrate along. Good quaternion librarieswill generate a quaternion which produces a rotation of (h/2) in the direction of without you having to do much calculation, and you can then just do a quaternionproduct to update.

People use different formulae for the update but thats probably the simplest one tocode and understand, though not the most clever or mathematically elegant. But itsfar better than doing

ek+1 ek + h2GT (ek)k+1

ek+1 1ek+1 ek+1DONT DO THIS!

(22)

Whats the point of all this? Well, remember the stepping formula for constrainedsystems [

M GTG T

] [vk+1

]=

[Mvk + hfkhgk + Gvk

]qk+1 = qk + hvk+1

, (23)

where is some parameter, and T is a block diagonal positive definite matrix. Spookis only one specific choice of these two. Except for the last line, everything can bedone using only the velocities. But now you know how to get your new rigid bodyconfiguration including the quaternions given velocities, so all you need to change isthe last line in Eqn. (23).

But something else slipped in here. When we assume that q = v, we have

g(qk+1) = g(qk + hvk+1) = g(qk) + hg

qvk+1. (24)

7

But thats not the case anymore. Instead, we need to look back at the definition

g(q) =g

qq =

g

qK(q)v, (25)

and thats the formula we want. Again, by abuse of notation, for the case of rigidbodies we still write

g = Gv (26)

and we call G the Jacobian. For this case, this is no longer just the gradient of g buta linear mapping between the velocity space of the rigid bodies and the constraintsthemselves.

What we want to do now is to avoid completely the computation of quaternionderivatives to get our precious Jacobian. How can that ever work? Consider a simplecase now of a body fixed vector p which has world coordinates p = R(e)p, and R(e) isthe orthogonal transform matrix that maps body-fixed vectors to world coordinates.The thing to remember here is that

dR

dt= R = wR, (27)

and since were assuming that p is constant, then

p = (R)p+R( p)

= (R)p+R 0 = p= p = p = p.

(28)

So, for instance, if the constraint was g(q) = x+ p where x is the center of mass of abody, this would pin the point p on the body at the origin. The Jacobian of that cannow be computed easily from Eqn. (26) since

g = x p = [I p] [x

], and so

G = G (BJ) =[I p] . (29)

I call that the ball-joint constraint for the good reason that if g(q) = 0, we have locked3 degrees of freedom on the body and thats really just that: a ball joint. Had we usedEuler angles to define our body kinematics, this ball joint would suffer gimbal lockfor some rotation, and thats just annoying, and unnecessary. The Jacobian G (BJ) inEqn. (29) never goes bad, i.e., it is never row-rank deficient because the first blockis the identity matrix.

We almost there for defining contact constraints. Consider two bodies and pointsp (1), p (2) fixed on them. The same superscript convention is used for velocities andcoordinates. Now, introduce the vector

r = x (1) + p (1) x (2) p (2) (30)

8

which goes from point p (1) to p (2) in world frame. The full Jacobian for this now,considering both bodies, is just as easy to derive, namely

r =[I p (1) I +p (2)]

x (1)

(1)

x (2)

(2)

. (31)So, lets now constrain the distance with g(q) = r l. Thats almost too easy bynow

g = r = 1rrT r

1

rrTG (BJ)v.

(32)

Ive abused notation again by writing G (BJ) for the Jacobian of the ball joint involvingtwo bodies, and removed all indices on v and r.

Lets assume now that we have a contact and maybe some penetration so that r 0.Now, someone needs to give us a normal vector n along which to separate, since r = 0,which means that it may or may not be difficult to normalize. What wed like is thatthe velocities separate at least so we want

nTG (BJ)v = G (contact)v 0. (33)

But what about tangential directions and frictional contact forces? Thats easy too.Consider s, t as two orthogonal vectors, and orthogonal to n as well. This is yet morenotational abuse because I already used t for time and n for the number of bodies, butdont worry. The point here is that we want to restrict the relative motion r alongboth direction so we want in fact tT r = sT r = 0 if we have stiction mode. You canjust replace n in Eqn. (33) to get what you need.

That leaves the annoying issue of the mass matrices because when you do Gauss-Seidel iterations, you basically work with the Schur complement matrix GM1GT +T .For rigid bodies, contrary to particles, this matrix is not just a multiple of the identity.Instead, for a single rigid body, we have

M =

[mI 00 J

]=

[mI 00 RJ0RT

](34)

where J0 is the inertia tensor in body frame, which is represented as a symmetric,positive definite 3 3 matrix, and R is the rotation matrix of the rigid bodies. Thematrix I in the north-west corner is the 3 3 identity matrix. So this is now a 6 6block matrix which needs to be updated at each step. The fact is though that forspheres and boxes, the inertia tensor is a multiple of the identity which is to say thatJ = J0 = I, where > 0 is a scalar. Thats not true for cylinders or long boxesor other shapes. But dont worry about this too much. In fact, if you do use such

9

big shapes, something will go very wrong with your simulations because you need toinclude gyroscopic forces.

If you think of continuous time and go back to Newton-Eulers laws of motion, youcan write the angular momentum L = M and then, the second law reads

dL

dt= L+M = , (35)

where is an external force. This is derived exactly in the same way as I did inEqn. (28), except that now, L = M is not constant in body frame. In principle,you could add this with other forces on the RHS in Eqn. (23) but that wont work.Do this and you can watch your bodies cram up on the north pole of the Riemannsphere, that is to say, all go to occupy infinity. The reason is complicated. You willfind all sorts of hacks for this if you read the recommended text or the SIGGRAPHproceedings. Some people say you can just integrate the angular momentum L directlywith Lk+1 = Lk + h and then recover k+1 from that, along with a rotation matrix.Thats fallacy. The fixes are complicated to derive and not entirely trivial to implement.So you are better off leaving them out. Still, I will use the J in what follows withoutassuming that it is just a scalar multiple of the identity.

Lets get to work now, and go back to our ball and socket Jacobian. What we wanthere is GM1GT for one body first, so we just do it. First let me write G (BJ) = G tosimplify the rest, and note that pT = p so now

GM1GT =[I p] [m1I 0

0 J1] [Ip

]=[I p] [m1IJ1p

]= m1I pJ1p.

(36)

The last bit looks bad in general, but remember that for distance and contact con-straints, we use something like nTG instead of just G. The result of doing this withsome vector z R3 for instance is

zTGM1GTn = m1zT z + (p z)J1(p z). (37)And thats not too bad after all.

Lets move on now to the two body case. We can partition the Jacobian as

G =[G (1) G (2)

](38)

and so our computation now is

GM1GT =[G (1) G (2)

] [M (1) 00 M (2)

] [G (1)

G (2)

]= G (1)M (1)

1G (1)

T+G (2)M (2)

1G (2)

T.

(39)

In other words, we just get a simple sum of terms.Notice that the matrix GM1GT has units of inverse mass and so (GM1GT )T

is some kind of inertia of the composite body when you apply a force of the formf = GT to it, i.e., a force that attempts to violate the constraint. But dont stretchthe analogy too much because GM1GT 6= 0 for a body in contact with the groundbut nevertheless, it has infinite inertia against forces that push it to the ground.

10