Inverse Kinematics using Pseudoinverse Jacobian

Question

I'm having problems solving IK with Jacobian Pseudoinverse method.

What I do is:

1. At each local joint frame q_i of the chain q calculate the cross product between rotation axis (taken from local transform) and joint-to-end-effector vector (end-effector is transformed to q_i coordinates):

   // for each joint q_i -> to parent iteration
   
   // Get Local Joint Position and Rotation Axis
   lTransform = ... // local transform of i-th joint
   if (lTransform.Decompose(s, q, t)) {
       q.AxisAngle(lRotationAxis, angle); // quaternion, XMQuaternionToAxisAngle
   }
   
   // Transform End-Effector To Local Joint Coordinates
   lEndPosition = Vector3::Transform(lEndPosition, lTransform); // each iteration is updated to-parent
   lJointToEnd = lEndPosition - lTransform.Translation();
   lResultAxis = lRotationAxis.Cross(lJointToEnd);
   
   jacobian(0, i) = lResultAxis.x;
   jacobian(1, i) = lResultAxis.y;
   jacobian(2, i) = lResultAxis.z;
   jacobian(3, i) = lRotationAxis.x;
   jacobian(4, i) = lRotationAxis.y;
   jacobian(5, i) = lRotationAxis.z;
   

2. Set up desired change in position dX = globalObjective - globalEndPosition (fill the rest with zeros [orientations]).

3. Calculate configuration dQ with jacobian pseudoinverse:

   jT = jacobian.transpose();
   inv = (jacobian * jT).inverse();
   jpinv = jT * inv;
   
   dQ = jpinv * dX;
   

4. So what exactly is dQ? Each value should represents a local change in i-th joint.

   • Is it the angle of rotation around rotation axis in local joint frame?
   • Or maybe translation value along cross product?

I tried applying both variations above updating local joint transform with simple matrix multiplication and none of this works. The end-effector position seems to be moving very randomly - at each iteration it's very different.

Answer 1

The answer is this:

1. Jacobian needs to be calculated globally. For each joint in global coordinates rotation axis needs to be calculated as a cross product between joint-to-end and joint-to-target. The vector that goes into the Jacobian position field is a cross product between joint-to-end and rotation axis - it is a vector along which movement needs to be made (wikipedia under "angular velocity" names it: cross-radial component).

   // For each joint (except end-effector)
   JointPosition = ToWorld(jointChainID[j]).Translation();
   JointToEnd = EndPosition - JointPosition;
   JointToTarget = TargetPosition - JointPosition;
   
   // Calculate Rotation Axis
   RotationAxis = JointToEnd.Cross(JointToTarget);
   RotationAxis.Normalize();
   
   // Cross-Radial Vector
   Result = JointToEnd.Cross(RotationAxis);
   
   // Jacobian column entry
   jacobian(0, j) = Result.x;
   jacobian(1, j) = Result.y;
   jacobian(2, j) = Result.z;
   jacobian(3, j) = RotationAxis.x;
   jacobian(4, j) = RotationAxis.y;
   jacobian(5, j) = RotationAxis.z;
   

2. The result dQ contains rotation angles for each joint along RotationAxis (in global coordinates). Then I apply changes locally (this is weird, as I apply global changes to local joint coordinates, but somehow this works):

   // Apply changes to local skinning matrices
   // For each joint (except end-effector)
        jointID = JointChainID[j];
        gTransform = Matrix::CreateFromAxisAngle(cache.RotationAxes[j], ToRadians(dQ(j)));
        SkinningLocal[jointID] = gTransform * SkinningLocal[jointID];