Numerically determinate the manipulator Jacobian

In classic kinematics, the Jacobian is used to solve the inverse velocity kinematics.

The forward kinematics equation is given by \( \quad x = f(\theta) \)and the Jacobian matrix is a linear approximation to \(f\).

$$J(\theta) = \begin{bmatrix} \frac{\partial p_x}{\partial \theta_1} & \frac{\partial p_x}{\partial \theta_2} & … & \frac{\partial p_x}{\partial \theta_n} \\[0.3em] \frac{\partial p_y}{\partial \theta_1} & \frac{\partial p_y}{\partial \theta_2} & … & \frac{\partial p_y}{\partial \theta_n} \\[0.3em] … & … & … & … \\[0.3em] \frac{\partial a_z}{\partial \theta_1} & \frac{\partial a_z}{\partial \theta_2} & … & \frac{\partial a_z}{\partial \theta_n} \end{bmatrix}$$

The below figure shows the relationship between the actual end-effector movement versus the linear approximation given by the Jacobian matrix.

jacobian_approx
Actual vs. linear approx. of end-effector movement.

The number of matrix columns equals the manipulators DOFs, and the number of matrix rows equals three or six, depending on whether it’s the half or full Jacobian respectably.

The full (simplified) Jacobian (6xn matrix) includes both velocity and orientation, while the half Jacobian (3xn matrix) only includes velocity.

Both the 3xn and 6xn Jacobian matrix may be calculated numerically. Wikipedia has an article showing the equation for determining the (i,k)-th entry of the Jacobian matrix.

It looks like this:

$$\frac{\partial p_i}{\partial \theta_k} \approx \frac{p_i (\hat{\theta}_k + h) – p_i(\hat{\theta})}{h} $$

Where $ p_i(\hat{\theta}) $ gives the i-th component of the position function, $ \hat{\theta}_k + h $ is simply $ \hat{\theta} $ with a small delta added to its k-th component and $ h $ is a reasonably small positive value. Just to be clear: $\hat{\theta} $ is the joint angles.

By implementing this equation in your preferred programming language, you should be able to determine the manipulator Jacobian for your current $\hat{\theta}$ configuration. Note that the above equation is only valid for the 3xn Jacobian matrix.

The last 3 rows of the 6xn Jacobian matrix may be obtained by replacing the position function $ p $ with a euler angle function $ e $.

Javascript implementation:

function fullJacobian (values) { //values are the joint angles
var h = 0.0001; //some low value

var rows = 6;
var cols = numDOF; //the number of DOFs -> variable needs to be instantiated
var jacobian = new Array(rows);
for (var i = 0; i < rows; i++) {
jacobian[i] = new Array(cols);
}

for (var i = 0; i < cols; i++) {
var vals = values.slice();
vals[i] = vals[i] + h;
var d1 = getPosition(vals); //getPosition must be implemented
var d2 = getPosition(values);
var d3 = getEuler(vals); //getEuler must be implemented
var d4 = getEuler(values);

jacobian[0][i] = (d1[0] - d2[0]) / h;
jacobian[1][i] = (d1[1] - d2[1]) / h;
jacobian[2][i] = (d1[2] - d2[2]) / h;
jacobian[3][i] = (d3[0] - d4[0]) / h;
jacobian[4][i] = (d3[1] - d4[1]) / h;
jacobian[5][i] = (d3[2] - d4[2]) / h;
}

return jacobian;
};

Java implementation:

public double[][] fullJacobian (double[] values) { //values are the joint angles
double h = 0.0001; //some low value

int rows = 6;
int cols = numDOF; //the number of DOFs -> variable needs to be instantiated
double[][] jacobian = new double[rows][cols];

for (int i = 0; i < cols; i++) {
double[] vals = values.clone();
vals[i] = vals[i] + h;
double[] d1 = getPosition(vals); //getPosition must be implemented
double[] d2 = getPosition(values);
double[] d3 = getEuler(vals); //getEuler must be implemented
double[] d4 = getEuler(values);

jacobian[0][i] = (d1[0] - d2[0]) / h;
jacobian[1][i] = (d1[1] - d2[1]) / h;
jacobian[2][i] = (d1[2] - d2[2]) / h;
jacobian[3][i] = (d3[0] - d4[0]) / h;
jacobian[4][i] = (d3[1] - d4[1]) / h;
jacobian[5][i] = (d3[2] - d4[2]) / h;
}

return jacobian;
}

Leave a Reply

Your email address will not be published. Required fields are marked *