Real-Time Physics Simulation Forum

Re: Point-point constraint

2012-12-16T03:16:22+00:00

Thanks Dirk ! It works great

Statistics: Posted by c0der — Sun Dec 16, 2012 3:16 am

Re: Point-point constraint

2012-12-15T19:06:34+00:00

// Start with the position constraint ( 3 x 1 )
C = x2 + r2 - x1 - r1

// Build the time derivative (which is the relative velocity at the joint)
dC/dt = J*v = v2 + w2 x r2 - v1 - w1 x r1

// Identify Jacobian by inspection ( 3 x 12 ) - here I is the 3x3 identity matrix
J = ( -I | skew( r1 ) | I | -skew( r2 ) )

// Compute *inverse* effective mass matrix ( 3 x 12 * 12 x 12 * 12 x 3 = 3 x 3 )
J*M^-1*JT = ( InvM1 + InvM2 ) * I - skew( r1 ) * InvI1 * skew( r1 ) - skew( r2 ) * InvI2 * skew( r2 )

// Solve 3x3 linear system system for lambda
J*M^-1*JT * Lamba = -J*v - beta * C / dt

// Accumulate for warmstarting (if desired)
AccumulatedLambda += Lambda;

// Apply impulses
v1 -= InvM1 * lambda
w1 -= InvI1 * ( r1 x lambda )
v2 += InvM2 * lambda
w2 += InvI2 * ( r2 x lambda )

Statistics: Posted by Dirk Gregorius — Sat Dec 15, 2012 7:06 pm

Re: Point-point constraint

2012-12-15T09:52:27+00:00

Sorry, talkin to myself here ...

I have got the following equation for JM^-1JT(lambda) = -J*vi for the point-point constraint:

(Ma^-1 + skew(ra)*Ia^-1*skew(ra)T + Mb^-1 + skew(rb)*Ib^-1*skew(rb)T)*lambda = (-vai - skew(ra)wai + vb + skew(rb)*wbi)

Where vai, wai etc are the initial linear and angular velocities of each body and T denotes the transpose

To solve for lambda, there is a 4x4 matrix when summing up all the matrices on the LHS and a 3x1 vector for the RHS of the equation, which means I will end up with a 3x1 vector for lambda when multiplying the RHS with the inverse of the LHS. Therefore, wbf below gets a zero impulse applied to it always.

In the end I should have:

vaf = vai + Ma^-1*lambda
waf = wai + Ia^-1*skew(ra)T*lambda
vbf = vbi - Mb^-1*lambda
wbf = wbi - Ib^-1*skew(rb)T*lambda

What am I doing wrong?

Thanks for anyone's help !

Statistics: Posted by c0der — Sat Dec 15, 2012 9:52 am

Re: Point-point constraint

2012-12-14T08:18:11+00:00

Sorry my bad, I forgot to add the entries in JM^-1JT, so it will be a huge multiplication in 3D. None the less, a good way to confirm both methods.

Thanks anyway

Statistics: Posted by c0der — Fri Dec 14, 2012 8:18 am

Point-point constraint

2012-12-14T04:10:21+00:00

Hi,

I have followed this process in deriving the jacobian matrix for a point-point constraint:

C(x) = xa + ra - xb - rb = 0 where x is the position of each body and r is the distance vector to the anchor point from each body
dC/dt = va + cross(wa, ra) - vb - cross(wb, rb) = 0 where w is the angular velocity of each body

Now that we have this relative velocity, i get the jacobian by inspection as follows:

dC/dt = Jv = [ 1 skew(ra) -1 skew(rb) ] [ va wa vb wb ] where the velocity vector is a column vector

Given the jacobian [ 1 skew(ra) -1 skew(rb) ]

J*M^-1*JT*lambda = -Jva

Now [1 skew(ra) - 1 skew(rb) ] * M where M is:

[ Ma^-1 0 0 0 ]
[ 0 Ia^-1 0 0 ]
[ 0 0 Mb^-1 0 ]
[ 0 0 0 Ib^-1 ]

Gives:

[ Ma^-1 skew(ra)*Ia^-1 -Mb^-1 -skew(rb)*Ib^-1 ]

And multiplying this by the transpose of the jacobian [1 skew(ra) - 1 skew(rb) ] which is a column vector with the same entries gives:

J*M^-1*JT = [ Ma^-1 skew(ra)*skew(ra) *Ia^-1 Mb^-1 skew(rb)*skew(rb)*Ib^-1 ]

Since I now have a mass matrix(m^-1*Identity) and a tensor matrix inside a matrix, what is the next step? I multiplied the skew with the tensor in each case and this doesnt simplify the problem.

I can easily do this problem using Erin's derivation which uses impulse momentum and relative velocity but I want to use the lagrange multiplier to put the problem into a common form for the solver.

Thanks

Statistics: Posted by c0der — Fri Dec 14, 2012 4:10 am