Jacobian Derivation

For the second term of ME 115 at Caltech (Introduction to Kinematics and Robotics), my final project was to develop a software package that would symbolically derive the forward kinematic equations and manipulator Jacobian matrix given the Denavit-Hartenberg parameters of a mechanicsm.

Introduction

While the relationship between the joint angles and the configuration of the tool frame of the manipulator is defined by a manipulator's forward kinematic equations, it is sometimes useful to have the relationship between joint angle velocities and the velocity of the tool frame. This relationship is given by the Jacobian of the manipulator. The Jacobian is important to the analysis of a manipulator as the joint velocities can be used to determine a manipulator's singularities.

A manipulator's singularities are configurations of the manipulator that result in a loss of motion in one or more directions. These configurations are important as singularities mean that the manipulator is restricted in its movements. This can be seen as a positive though as large forces can be applied in the singular direction because any force in that direction cannot result in a change in the joint angles. Additionally, near the singularity, large joint velocities affect the tool velocity only very slightly, so the mechanical advantage is very large near these configurations without the loss of motion.

Since the Jacobian is so important to the analysis of manipulators, it is beneficial to solve for this matrix quickly. However, for manipulators with many joints, the derivation can quickly become very involved. Thus, this project attempts to provide the necessary functions to symbolically find the Jacobian (in the spatial, body, or hybrid frames) as well as the forward kinematics of a manipulator given its Denavit-Hartenberg parameters. In finding these matrices, several helper functions were written as well that help to generate the necessary vector and screw operations.

The Jacobian is also useful when dealing with closed-loop mechanisms because it defines the twist vectors to be used when calculating the Grammian of the screw system. This Grammian is used when determining the special configurations of a closed-loop mechanism (by setting the determinant of the k-k cofactor equal to zero when determining the special configuration for joint k). Because this is useful to the calculation and is related to the Jacobian, a method for calculating the Grammian (given the Jacobian) is also provided.

Code


(* g: Create homogeneous transformation matrix from D-H parameters *)
g[a_, \[Alpha]_, d_, \[Theta]_] := ( {
    {Cos[\[Theta]], -Sin[\[Theta]], 0, a},
    {Sin[\[Theta]]*Cos[\[Alpha]], 
     Cos[\[Theta]]*Cos[\[Alpha]], -Sin[\[Alpha]], -d*Sin[\[Alpha]]},
    {Sin[\[Theta]]*Sin[\[Alpha]], Cos[\[Theta]]*Sin[\[Alpha]], 
     Cos[\[Alpha]], d*Cos[\[Alpha]]},
    {0, 0, 0, 1}
   } );

(* VectorHat: create a skew-symmetric matrix from a 3x1 vector *)
VectorHat[w_] := If[MatrixQ[w] && Dimensions[w] == {3, 1}, ( {
     {0, -w[[3, 1]], w[[2, 1]]},
     {w[[3, 1]], 0, -w[[1, 1]]},
     {-w[[2, 1]], w[[1, 1]], 0}
    } ),
    StringForm[
     "Vector cannot have hat operation performed. Vector has invalid \
dimensions: ``", Dimensions[w]]];

(* ScrewHat: create a 4x4 matrix from a 6x1 screw *)
ScrewHat[S_] := If[MatrixQ[S] && Dimensions[S] == {6, 1},
   (a = ConstantArray[0, {4, 4}];
    a[[1 ;; 3, 1 ;; 3]] = VectorHat[S[[4 ;; 6]]];
    a[[1 ;; 3, 4]] = S[[1 ;; 3, 1]];
    a),
   StringForm[
     "Screw cannot have hat operation performed. Screw has invalid \
dimensions: ``", Dimensions[S]]];

(* VectorUnhat: create a 3x1 vector from a 3x3 matrix *)
VectorUnhat[mat_] := (If[MatrixQ[mat] && Dimensions[mat] == {3, 3},
    ( {
      {mat[[3, 2]]},
      {mat[[1, 3]]},
      {mat[[2, 1]]}
     } ),
    Throw[
     StringForm[
      "Matrix cannot have unhat operation performed. Matrix has \
invalid dimensions: ``", Dimensions[mat]]]]);

(* ScrewUnhat: create a 6x1 screw from a 4x4 matrix *)
ScrewUnhat[mat_] := (If[MatrixQ[mat] && Dimensions[mat] == {4, 4},
    (a = ConstantArray[0, {6, 1}];
     a[[1 ;; 3, 1]] = mat[[1 ;; 3, 4]];
     a[[4 ;; 6]] = VectorUnhat[mat[[1 ;; 3, 1 ;; 3]]];
     ArrayFlatten[a]),
     StringForm[
      "Matrix cannot have unhat operation performed. Matrix has \
invalid dimensions: ``", Dimensions[mat]]]);

(* HomogenousRV: Create homogenous matrix from 3x3 rotation matrix and
3x1 translation vector *)
HomogenousRV[R_, p_] := 
  If[MatrixQ[R] && Dimensions[R] == {3, 3} && MatrixQ[p] && 
    Dimensions[p] == {3, 1},
   (a = IdentityMatrix[4];
    a[[1 ;; 3, 1 ;; 3]] = R[[1 ;; 3, 1 ;; 3]];
    a[[1 ;; 3, 4]] = p[[1 ;; 3, 1]];
    a),
   StringForm[
     "Cannot create homogeneous matrix. Dimensions given are R: ``, \
p: ``", Dimensions[R], Dimensions[p]]];

(* Adjoint: Create adjoint matrix from a 4x4 homogeneous matrix *)
Adjoint[mat_] := If[MatrixQ[mat] && Dimensions[mat] == {4, 4},
   (a = ConstantArray[0, {6, 6}];
    a[[1 ;; 3, 1 ;; 3]] = mat[[1 ;; 3, 1 ;; 3]];
    a[[4 ;; 6, 4 ;; 6]] = mat[[1 ;; 3, 1 ;; 3]];
    a[[1 ;; 3, 4 ;; 6]] = 
     VectorHat[Partition[mat[[1 ;; 3, 4]], 1]].mat[[1 ;; 3, 1 ;; 3]];
    a),
   StringForm[
     "Cannot create adjoint matrix. Matrix has invalid dimensions: \
``", Dimensions[mat]]];

(* ForwardKinematics: Derive forward kinematics given D-H parameters *)
ForwardKinematics[DHparams__] := (params = {DHparams};
   n = Length[params];
   gst = IdentityMatrix[4];
   For[i = 1, i <= n, i++, gst = gst.g[Sequence @@ params[[i]]]];
   Simplify[gst]);

(* SpatialJacobian: Derive the spatial Jacobian given D-H parameters *)
SpatialJacobian[DHparams__] := (gst = ForwardKinematics[DHparams];
   theta = {DHparams}[[All, 4]];
   n = Length[theta] - 1;
   J = ConstantArray[0, {6, n}];
   For[i = 1, i <= n, i++,
    (J[[All, i]] = ScrewUnhat[D[gst, theta[[i]]].Inverse[gst]];
     J = ArrayFlatten[J];)];
   Partition[Flatten[Simplify[J]], n]);

(* BodyJacobian: Derive the body Jacobian given D-H parameters *)
BodyJacobian[DHparams__] := (gst = ForwardKinematics[DHparams];
   theta = {DHparams}[[All, 4]];
   n = Length[theta] - 1;
   J = ConstantArray[0, {6, n}];
   For[i = 1, i <= n, i++,
    (J[[All, i]] = ScrewUnhat[Inverse[gst].D[gst, theta[[i]]]];
     J = ArrayFlatten[J];)];
   Partition[Flatten[Simplify[J]], n]);

(* HybridJacobian: Derive the hybrid Jacobian given D-H parameters *)
HybridJacobian[DHparams__] := (gst = ForwardKinematics[DHparams];
   J = BodyJacobian[DHparams];
   R = ConstantArray[0, {6, 6}];
   R[[1 ;; 3, 1 ;; 3]] = gst[[1 ;; 3, 1 ;; 3]];
   R[[4 ;; 6, 4 ;; 6]] = gst[[1 ;; 3, 1 ;; 3]];
   J = R.J;
   Simplify[J]);

(* Cofactor: Take ith cofactor of matrix *)
Cofactor[mat_, i_] := If[MatrixQ[mat] && i <= Min[Dimensions[mat]],
   (m = mat;
    m = Delete[m, i];
    m = Transpose[Delete[Transpose[m], i]];
    m),
   StringForm[
     "Not given a matrix or i is greater than the matrix dimension"]];

(* Grammian: Get Grammian of a Jacobian *)
Grammian[mat_] := If[MatrixQ[mat],
   (numcols = Dimensions[mat][[2]];
    G = ConstantArray[0, {numcols, numcols}];
    For[row = 1, row <= numcols, row++,
     For[col = 1, col <= numcols, col++,
      G[[row, col]] = mat[[All, row]].mat[[All, col]]]];
    G),
   StringForm["Not given a matrix to make the Grammian"]];
        

Conclusion

The project was successful in that it was able to capture the nature of the Jacobian using the Denavit-Hartenberg parameters. However, though these parameters are widely used, twists and screws are sometimes used to represent the joints of a mechanism. In this class, the majority of the joint descriptions were through the screw parameters. Because of this, the use of the Denavit-Hartenberg paramters is actually a shortcoming. One possible improvement to this would be to overload the functions so that the joints can be specified using either method and regardless of the specification, the software would be able to produce the required Jacobian.

The software also begins to extend into the analysis of the Jacobian with functions to find cofactors as well as the Grammian of a matrix. This could be further extended to provide more insight into a mechanism through the Jacobian such as whether the matrix is full rank or not.

One possible thing for future work is to combine the derivation of the Jacobian with simulation software (such as the one done for my final project last term). By doing this, the simulation’s overall quality can be improved by more accurately displaying the joint and tool frame velocities.

Home