I'm having problems solving IK with Jacobian Pseudoinverse method.
What I do is:
At each local joint frame
q_i
of the chainq
calculate the cross product between rotation axis (taken from local transform) and joint-to-end-effector vector (end-effector is transformed toq_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;
Set up desired change in position
dX = globalObjective - globalEndPosition
(fill the rest with zeros [orientations]).Calculate configuration
dQ
with jacobian pseudoinverse:jT = jacobian.transpose(); inv = (jacobian * jT).inverse(); jpinv = jT * inv; dQ = jpinv * dX;
So what exactly is
dQ
? Each value should represents a local change ini
-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.