1
\$\begingroup\$

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.

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

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];
    
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy