The Jacobian

Introduction

Forward kinematic equations are functions used to relate joint variables to end-effector positions and orientations with respect to a fixed coordinate system. The Jacobian matrix linearly relates joint velocities to end-effector velocities and can be useful in several aspects of robotics: planning and execution of smooth paths, determination of singular configurations, derivation of dynamic equations of motion, and transformation of forces and torques from the end effector to the manipulator joints.

Jacobian Derivation

The relationship between the end-effector velocities and joint velocities can be described with a linear equation:

(1)
\begin{align} $\xi = J\dot q$ \end{align}

Where, $\xi$ is the body velocity vector; $\dot q$ is the joint velocity vector; and $J$ is the Jacobian matrix. For an n-link serial manipulator, the following expressions apply:

(2)
\begin{align} $\nu_{n}^{0} = J_{\nu}\dot q$ \end{align}
(3)
\begin{align} $\omega_{n}^{0} = J_{\omega}\dot q$ \end{align}

Where, $\nu_{n}^{0}$ is the linear velocity of the end effector, and $\omega_{n}^{0}$ is the angular velocity of the end effector. By separating the Jacobian matrix, and body velocity vector, $\xi$ and $J$ are now defined as:

(4)
\begin{align} \xi = \left[ \begin{array}{c}\nu_{n}^{0}\\\omega_{n}^{0}\end{array}\right] \end{align}

and

(5)
\begin{align} J = \left[ \begin{array}{c} J_{\nu}\\ J_{\omega}\end{array}\right] \end{align}

Now the $i^{th}$ column of the Jacobian will be derived for a prismatic and revolute joint. Note that the derivation is not detailed, but the end result will be very useful.

Prismatic Joint

If the $i^{th}$ joint is a prismatic joint, the motion of frame $i$ relative to frame $i-1$ is in pure translation, therefore,

(6)
\begin{align} \omega_{i}^{i-1} = 0 \end{align}

This makes the $i^{th}$ column of $J_{\omega}$ equal to zero.

(7)
\begin{align} J_{\omega_i} = \left[ \begin{array}{ccc} 0 & 0 & 0 \end{array}\right]^{T} \end{align}

The linear velocity of the end effector ($\nu_{n}^{0}$) is defined as the time derivative of position with respect to a base frame ($\dot o_{n}^0$). Using the chain rule, it can be shown that:

(8)
\begin{align} \dot o_{n}^0 = \displaystyle\sum_{i=0}^n \frac{\partial o_{n}^0}{\partial q_{i}}\dot q_{i} \end{align}

From the previous equation, it can be concluded that:

(9)
\begin{align} J_{\omega_i} = \frac{\partial o_{n}^0}{\partial q_{i}} \end{align}

If the DH convention is followed, and the $i^{th}$ joint is prismatic, pure translation parallel to the $z_{i-1}$ axis with magnitude $d_i$ can occur. Thus, for a prismatic joint, the following holds true.

(10)
\begin{align} \dot o_{n}^0 = \dot d_{i}z_{i-1}^{0} = \dot q_{i} z_{i-1}^{0} \end{align}

Finally, if the $i^{th}$ joint is a prismatic, the following can be concluded:

(11)
\begin{align} J_{i} = \left[\begin{array}{c} z_{i-1} \\ 0 \end{array} \right] \end{align}

Revolute Joint

If the $i^{th}$ joint is a revolute, the angular velocity of frame $i$ relative to frame $i-1$ can be defined as

(12)
\begin{align} \omega_{i}^{i-1} = \dot q_{i} z_{i-1} \end{align}

Therefore,

(13)
\begin{align} J_{\omega_i} = z_{i-1} \end{align}

The linear velocity of the end effector caused by angular rotation at joint $i$ can be described by

(14)
\begin{align} \nu_i = \omega_i \times r_i \end{align}

in which $r$ is the vector from the point of rotation to the end effector. Reffering to Figure 1,

(15)
\begin{equation} r_i = o_{n} - o_{i-1} \end{equation}
(16)
\begin{align} \omega_i = \dot\theta_i z_{i-1} \end{align}

For joint $i$, it can now be concluded that

(17)
\begin{align} \nu_i = \dot\theta_i (z_{i-1} \times (o_{n} - o_{i-1})) \end{align}

and

(18)
\begin{align} J_{\nu_i} = z_{i-1} \times (o_{n} - o_{i-1}) \end{align}
Jacobian_fig1.bmp

Figure 1: Arbitrary $n$-link serial robot

Finally, for revolute joint $i$, equation 13 and 18 can be put into equation 5 to get

(19)
\begin{align} J_i = \left[ \begin{array}{c} z_{i-1} \times (o_{n} - o_{i-1}) \\ z_{i-1} \end{array} \right] \end{align}

Total Jacobian

For a $n$-link serial manipulator, the Jacobian is the combination of $J_1$ to $J_n$ vectors as shown

(20)
\begin{align} J = \left[J_1 ... J_n \right] \end{align}

Where $J_i$ is defined in equations 11 and 19 according to joint type.

Singularities

The Jacobian can be used to find singular configurations of a manipulator. When a manipulator is in singular configuration, certain directions of motion are unattainable, or a degree or degrees of freedom are lost. Knowing that the $6 \times n$ Jacobian defines a mapping between the joint velocity vector and the end-effector velocity vector (Equation 1), all possible end effector velocities are linear combinations of the columns of the Jacobian matrix.

(21)
\begin{align} \xi = J_{1}\dot q_{1} + J_{2}\dot q_{2} ... J_{n}\dot q_{n} \end{align}

In order for the end effector to achieve arbitrary velocity ($\xi \in \mathbb{R}^{6}$), the Jacobian must have 6 linearly independent columns (or rows); meaning, the rank of the Jacobian must equal 6. Note that rank $J \leq min(6,n)$, and for a 6 DOF serial robot, rank $J \leq 6$. Therefore, if the Jacobian of a 6 degree of freedom robot has a rank of less than 6, a degree or degrees of freedom are lost; this is a singular configuration.

Decoupling of Singularities

The Jacobian matrix for a 6 DOF is square; therefore, if the determinant of the Jacobian is zero, the manipulator is in a singular configuration. Now, consider the case with 3-DOF arm and a 3-DOF spherical wrist. The Jacobian matrix of such a robot can be decoupled to find arm singularities and wrist singularities. The proof begins by partitioning the Jacobian into $3 \times 3$ blocks.

(22)
\begin{align} J = \left[\begin{array}{cc} J_P\ & J_O \end{array} \right] = \left[\begin{array}{cc} J_{11}& J_{12} \\ J_{21}& J_{22} \end{array} \right] \end{align}

Since the final three joints are always revolute

(23)
\begin{align} J_{12} = \left[ \begin{array}{ccc} z_{3} \times (o_{6} - o_{3}) & z_{4} \times (o_{6} - o_{4}) & z_{5} \times (o_{6} - o_{5}) \end{array} \right] \end{align}

Because the three wrist axes intersect at a common point, a common coordinate frame ($o$) can be chosen such that $o_3= o_4= o_5= o_6=o$. This means

(24)
\begin{align} J_{12} = 0_{3 \times 3} \end{align}

In this special case, the Jacobian matrix becomes:

(25)
\begin{align} J = \left[\begin{array}{cc} J_P\ & J_O \end{array} \right] = \left[\begin{array}{cc} J_{11}& 0 \\ J_{21}& J_{22} \end{array} \right] \end{align}

With determinate

(26)
\begin{equation} det J = det J_{11} det J_{22} \end{equation}

Now, it can be concluded that robot is in a singular configuration if $det J_{11}= 0$ or $det J_{22}= 0$ . Through this decoupling process, arm singularities are found if $det J_{11}= 0$ and wrist singularities are found if $det J_{22}= 0$ .

Inverse Velocity

Referring to Equation 1, the inverse velocity problem involves finding the joint velocities ($\dot q$) that produce a desired end-effector velocity ($\xi$). When the Jacobian is square and nonsingular, the inverse problem can be solved as follows:

(27)
\begin{align} \dot q = J^{-1}\xi \end{align}

The inverse velocity solution for a robot not having exactly 6 DOF involves a more complicated approach and will not be shown on this page.

Example

In this example, it is assumed that the forward kinematic equations have been found. Refer to "Forward Kinematic Problem" for the derivation of the following equations.

(28)
\begin{align} T _{\rm 1}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & 0\\ s _{\rm 1} & 0 & -c _{\rm 1} & 0\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(29)
\begin{align} T _{\rm 2}^{0}= \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & s _{\rm 1} d _{\rm 2}\\ s _{\rm 1} & 0 & -c _{\rm 1} & -c _{\rm 1} d _{\rm 2}\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(30)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} c _{\rm 3} & -c _{\rm 1} s _{\rm 3} & s _{\rm 1} & s _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ s _{\rm 1} c _{\rm 3} & -s _{\rm 1} s _{\rm 3} & -c _{\rm 1} & -c _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ s _{\rm 3} & c _{\rm 3} & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}

Joint 1 is a revolute. Therefore, $J_1$ is defined by Equation 19. With $i=1$, $z_0$ is defined as the projection of the $z$ axis of frame 0 onto itself, and $o_0$ is the distance from the origin of frame 0 to origin of the base frame.

(31)
\begin{align} z_0 = \left[ \begin{array}{c} 0\\0\\1 \end{array} \right] \end{align}
(32)
\begin{align} o_0 = \left[ \begin{array}{c} 0\\0\\0 \end{array} \right] \end{align}

The distance from the base frame to the end effector ($o_n$) can be found in the last column of the transformation matrix $T_n^0$. In this example, $n=3$, and $o_n$ can be directly taken from the last column of $T_3^0$.

(33)
\begin{align} o_3 = \left[ \begin{array}{c} s_1(d_2+d_3) \\ -c_1(d_2+d_3) \\ d_1 \end{array} \right] \end{align}

Now, directly plug Equations 31-33 into Equation 19.

(34)
\begin{align} J_1 = \left[ \begin{array}{c} z_{0} \times (o_{3} - o_{0}) \\ z_{0} \end{array} \right] = \left[ \begin{array}{c} \left[ \begin{array}{c} 0\\0\\1 \end{array} \right] \times \left[ \begin{array}{c} s_1(d_2+d_3) \\ -c_1(d_2+d_3) \\ d_1 \end{array} \right] \\ \left[ \begin{array}{c} 0\\0\\1 \end{array} \right] \end{array} \right] = \left[ \begin{array}{c} c_1(d_2+d_3) \\ s_1(d_2+d_3) \\ 0 \\ 0 \\ 0 \\ 1 \end{array} \right] \end{align}

Joint 2 is prismatic, and $J_2$ is determined by Equation 11. Now $i=2$, and $z_1$ represents the projection of $z_1$ onto $z_0$ which can be found in the third column of $T_1^0$.

(35)
\begin{align} z_1 = \left[ \begin{array}{c} s_1\\-c_1\\0 \end{array} \right] \end{align}

Thus,

(36)
\begin{align} J_2 = \left[ \begin{array}{c} z_{0} \\ 0 \end{array} \right] = \left[ \begin{array}{c} \left[ \begin{array}{c} s_1\\-c_1\\0 \end{array} \right] \\ \left[ \begin{array}{c} 0\\0\\0 \end{array} \right] \end{array} \right] = \left[ \begin{array}{c} s_1\\-c_1\\0 \\0\\0\\0\end{array} \right] \end{align}

Joint 3 is a prismatic. In a similar manner as $J_1$, $J_3$ can be calculated.

(37)
\begin{align} J_3 = \left[ \begin{array}{c} z_{2} \times (o_{3} - o_{2}) \\ z_{2} \end{array} \right] = \left[ \begin{array}{c} \left[ \begin{array}{c} s_1\\-c_1\\0 \end{array} \right] \times \left[ \begin{array}{c} s_1(d_3) \\ -c_1(d_3) \\ d_1 \end{array} \right] \\ \left[ \begin{array}{c} s_1\\-c_1\\0 \end{array} \right] \end{array} \right] = \left[ \begin{array}{c} 0 \\ 0 \\ 2c_1s_1(d_3) \\ s_1 \\ -c_1 \\ 0 \end{array} \right] \end{align}

Using the Equation 20, the total Jacobian can be determined.

(38)
\begin{align} J = \left[J_1 ... J_n \right]= \left[ \begin{array}{ccc} c_1(d_2+d_3) & s_1 & 0 \\ s_1(d_2+d_3) & -c_1 & 0 \\ 0 & 0 & 2c_1s_1(d_3) \\ 0 & 0 & s_1 \\ 0 & 0 & -c_1 \\ 1 & 0 & 0 \end{array} \right] \end{align}
Bibliography
1. Spong, Hutchinson, and Vidyasagar, Robot Modeling and Control
Add a New Comment
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License