When combining a revolute joint and an angle constraint to form the 2x2 block K matrix consisting of four 3x3 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 3x1 vectors as follows:
A*lambda = b
Code: Select all
// A = JM^-1JT
//
// A = [ Ma^-1 + [~ra]TIa^-1[~ra] + Mb^-1 + [~rb]TIb^-1[~rb] [~ra]TIa^-1 + [~rb]TIb^-1 ]
// [ Ia^-1[~ra] + Ib^-1[~rb] Ia^-1 + Ib^-1 ]
// b = -Jvi
// = [ -va - [~ra]Twa + vb + [~rb]Twb ]
// [ -wa + wb ]