# Inverse Kinematics using Pseudoinverse Jacobian

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.

• I need to calculate this locally, because the end-effector does not always represent a leaf of the skeleton. – user63753 Jan 18 '17 at 13:49

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();

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];