HEJP's Blog

Now on to the algorithm that requires a little mathematical understanding, the Jacobian matrix method. This method approximates the IK problem as a linear problem, which it almost is if you only change the angles of the joints by small amounts.

Mathematics of the Method

Starting with the vector of angles,

We have the forward kinematic function f, i.e the function that return the position of the endpoint, x, given the angles of the joints.

Our goal is to find  this can be done be firstly introducing the Jacobian matrix as well as differentiating w.r.t. time resulting in this,

Since,

Replacing the δ operators with Δ operators to approximate the differentiation,

Sets us up for an iterative solution.

Outline of the Algorithm

1. Calculate Δx = Goal – f(q) if the size to Δx is small enough exit loop
2. Compute J^-1. J may not be square in which case use the pseudo inverse (J^TJ)^-1J^T
3. Compute Δθ = J^-1Δx
4. θ = θ + Δθ
5. Repeat

Example

Source

Source: here

Notes

J^TJ is very likely to be very ill-conditioned as it turns out here’s an example,

k, the condition number, is a measure of the relative error in the solution (of Ax=b say) to the error input (b).

So relating it to our IK problem the errors in Δθ are likely to be many orders of magnitudes larger than the errors in Δx. Remember that the cos and sin function use to compute Δx can only be so close to the real values. But there are other ways to compute the pseudo inverse…

References

http://www.eecs.tufts.edu/~mtabac0a/IK/Project.pdf