Inverse Kinematics: Calculating the Jacobian

I am trying to do inverse kinematics for a serial chain of arbitrarily many links.

In the following paper, I have found an example for how to calculate the Jacobian matrix.

Entry (i, j) = v[j] * (s[i] - p[j])

where:

v[j] is the unit vector of the axis of rotation for joint j

s[i] is the position (int world coords?) of joint i

p[j] is the position (in world coords?) of joint j

The paper says that this works if j is a rotational joint with a single degree of freedom. But my rotational joints have no constraints on their rotation. What formula do I then want? (Or am I possibly misunderstanding the term "degree of freedom"?)

Solution

This question is old, but I'll answer anyway, as it is something I have thought about but never really gotten around to implement.

Rotational joints with no constraints are called ball joints or spherical joints; they have 3 degrees of freedom. You can use the formula in the tutorial for spherical joints also, if you parameterize each spherical joint in terms of 3 rotational (revolute) joints of one degree of freedom each.

For example: Let N be the number of spherical joints. Suppose each joint has a local transformation T_local[i] and a world transformation

T_world[i] = T_local[0] * ... * T_local[i]

Let R_world[i][k], k = 0, 1, 2, be the k-th column of the rotation matrix of T_world[i]. Define the 3 * N joint axes as

v[3 * j + 0] = R_world[i][0]
v[3 * j + 1] = R_world[i][1]
v[3 * j + 2] = R_world[i][2]

Compute the Jacobian J for some end-effector s[i], using the formula of the tutorial. All coordinates are in the world frame.

Using for example the pseudo-inverse method gives a displacement dq that moves the end-effector in a given direction dx.

The length of dq is 3 * N. Define

R_dq[j] = 
    R_x[dq[3 * j + 0]] *
    R_y[dq[3 * j + 1]] *
    R_z[dq[3 * j + 2]]

for j = 0, 1, ..., N-1, where R_x, R_y, R_z are the transformation matrices for rotation about the x-, y-, and z-axes.

Update the local transformations:

T_local[j] := T_local[j] * R_dq[j]

and repeat from the top to move the end-effector in other directions dx.