I have temporarily just applied the revolute joint and restricted angle joint separately to both bodies and it seems to be doing the trick.

For the elbow angle:

currAngleDiff = body1.orientation – body2.orientation
if(currAngleDiff.z>=0 && currAngleDiff.z<=radians(130) skip constraint, assuming the arm is straight at first at angle zero

Hope that helps anyone

This should yield a 6×6 matrix (each row has 2 3×3 columns). To solve this we need to have a b vector that is 6×1.

We can see that the first row will yield a 3×1 as will the bottom. So we should be able to solve this 6 variable system using whatever linear equation solver you would like to use. To solve this you must invert the 6×6. You can use , , , etc just be careful of singularities and the respective algorithm’s restrictions.

Another option is to use an application like Mathematica (or something similar) to simplify the matrices symbolically (since it isn’t really hard, just tedious).

Hope that helps,
William

When combining a revolute joint and an angle constraint to form the 2×2 block K matrix consisting of four 3×3 matrices below for an elbow joint with restricted angular movement, how do I invert this matrix to solve for lambda, which seems like another matrix of two 3×1 vectors as follows:

A*lambda = b

A = JM^-1JT

A11 = Ma^-1 + [~ra]TIa^-1[~ra] + Mb^-1 + [~rb]TIb^-1[~rb]
A12 = [~ra]TIa^-1 + [~rb]TIb^-1
A21 = Ia^-1[~ra] + Ib^-1[~rb]
A22 = Ia^-1 + Ib^-1

b = -Jvi

b11 = -va – [~ra]Twa + vb + [~rb]Twb
b12 = -wa + wb

A*lambda = b

I am working in 3D, so multiplying the matrices out gets really messy

Thanks for another great tutorial