Robotics Kinematics and Dynamics/Serial Manipulator Differential Kinematics

Twists

A popular, and minimal way to represent the time derivate of a pose is a twist. A twist consists of two 3 × 1 vectors: $\omega$ and $v$, where $\omega$ is the angular velocity of the moving rigid body (manipulator link), and $v$ is the linear velocity of the point on the moving rigid body that instantaneously coincides with the origin of the reference frame.

$t = \left( \begin{array}{c} \omega \\ v \end{array} \right)$

Propagation of Velocity

Rotational velocities that are written with respect to the same frame can simply be added. Hence:

$_{i + 1} \omega_{i + 1} = \, ^i _{i + 1} R \, _i \omega_i + \begin{pmatrix} 0 \\ 0 \\ \dot{\theta}_{i + 1} \end{pmatrix}$

The linear velocity of the origin of a link is that of the origin of the preceeding link plus a rotational component:

$_{i + 1} v_{i + 1} = \, ^i _{i + 1} R \left( _i v_i + _i \omega_i \times \, _i P_{i + 1} \right)$

Note: In the above equations the resulting velocities are expressed relative to the frame of the current link. $_i P_{i + 1}$ is the vector pointing from the origin of frame ${i}$ to that of frame ${i + 1}$.

Example: The Two-Link Planar Manipulator

The two-link planar manipulator.

The link transformations that come with this manipulator can be easily derived graphically from looking at the figure on the right:

$^1 _0 T = \begin{pmatrix} c_1 & -s_1 & 0 & 0 \\ s_1 & c_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

$^2 _1 T = \begin{pmatrix} c_2 & -s_2 & 0 & l_1 \\ s_2 & c_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

$^3 _2 T = \begin{pmatrix} 1 & 0 & 0 & l_2 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}$

Here, $l_1$ and $l_2$ are the lengths link 1 and 2, respectively.

Note: Notice again the use of $s_i$ and $c_i$ as shorthand notations for $sin(\theta_i)$ and $cos(\theta_i)$, respectively.

Hence, from the above equations:

$_2 \omega_2 = \begin{pmatrix} 0 \\ 0 \\ \dot{\theta_1} + \dot{\theta_2} \end{pmatrix}$

and:

$_3 \omega_3 = _2 \omega_2$

Also:

$_2 v_2 = \begin{pmatrix} c_2 & -s_2 & 0 \\ s_2 & c_2 & 0 \\ 0 & 0 & 1 \end{pmatrix} \left[ \begin{pmatrix} 0 \\ 0 \\ \dot{\theta_1} \end{pmatrix} \times \begin{pmatrix} l_1 \\ 0 \\ 0 \end{pmatrix} \right] = \begin{pmatrix} l_1 s_2 \dot{\theta_1} \\ l_1 c_2 \dot{\theta_1} \\ 0 \end{pmatrix}$

and:

$_3 v_3 = \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} \left[ \begin{pmatrix} l_1 s_2 \dot{\theta_1} \\ l_1 c_2 \dot{\theta_2} \\ 0 \end{pmatrix} + \begin{pmatrix} 0 \\ 0 \\ \dot{\theta_1} + \dot{\theta_2} \end{pmatrix} \times \begin{pmatrix} l_2 \\ 0 \\ 0 \end{pmatrix} \right] = \begin{pmatrix} l_1 s_2 \dot{\theta_1} \\ l_1 c_2 \dot{\theta_1} + l_2 ( \dot{\theta_1} + \dot{\theta_1} ) \\ 0 \end{pmatrix}$

With respect to the base frame, the end-effector velocity becomes:

$_0 v_3 = \, _0 ^3 R \, _3 v_3 = \, _0 ^1 R \, _1 ^2 R \, _2 ^3 R \, _3 v_3 = \begin{pmatrix} -l_1 s_1 \dot{\theta_1} - l_2 s_{12} (\dot{\theta_1} + \dot{\theta_2}) \\ l_1 c_1 \dot{\theta_1} - l_2 c_{12} (\dot{\theta_1} + \dot{\theta_2}) \\ 0 \end{pmatrix}$

Jacobian

Suppose $m$ kinematic (end-effector) equations. Each is a function of $n$ degrees of freedom:

$\begin{array}{ccc} x_1 & = & x_1(\theta_1, \ldots, \theta_n) \\ \vdots & & \vdots \\ x_m & = & x_1(\theta_1, \ldots, \theta_n) \\ \end{array}$

Note that in the case of a non-redundant manipulator, $m = n$.

The time derivate of the above equations is found as follows:

$\begin{array}{ccc} \dfrac{dx_1}{dt} & = & \dfrac{\partial x_1}{\partial \theta_1} \dfrac{d \theta_1}{dt} + \cdots + \dfrac{\partial x_1}{\partial \theta_n} \dfrac{d \theta_n}{dt} \\ \vdots & & \vdots \\ \dfrac{dx_m}{dt} & = & \dfrac{\partial x_m}{\partial \theta_1} \dfrac{d \theta_1}{dt} + \cdots + \dfrac{\partial x_m}{\partial \theta_n} \dfrac{d \theta_n}{dt} \end{array}$

This can be written in vector form:

$v = J \frac{d \theta}{dt}$

The matrix $J$ is called the Jacobian, the matrix of which the elements are the partial derivatives of the kinematics equations. The relationship between the end-effector velocity and the (known) joint velocities is thus fully described by the Jacobian. The end-effector velocity is a linear function of the joint velocities.

$J = \begin{pmatrix} \frac{\partial x_1}{\partial \theta_1} & \cdots & \frac{\partial x_1}{\partial \theta_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial x_m}{\partial \theta_1} & \cdots & \frac{\partial x_m}{\partial \theta_n} \end{pmatrix}$

Example: The Two-Link Planar Manipulator

This example presents a slightly other way of solving the above problem. The position kinematics equations of this manipulator can be easily derived using the technique presented in the Serial Manipulator Position Kinematics section, or graphically from looking at the figure above:

$\begin{array}{l} x = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2) \\ y = l_1 \sin(\theta_1) + l_2 \sin(\theta_1 + \theta_2) \end{array}$

Here, the Jacobian is:

$J = \begin{pmatrix} -l_1 \sin(\theta_1) - l_2 \sin(\theta_1 + \theta_2) & -l_2 \sin(\theta_1 + \theta_2) \\ l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2) & l_2 \cos(\theta_1 + \theta_2) \\ \end{pmatrix}$

Hence, the end-effector velocity is:

$\begin{array}{ccc} \dfrac{dx}{dt} & = & \left[ -l_1 \sin(\theta_1) - l_2 \sin(\theta_1 + \theta_2) \right] \dfrac{d \theta_1}{dt} - l_2 \sin(\theta_1 + \theta_2) \dfrac{d \theta_2}{dt} \\ \dfrac{dy}{dt} & = & \left[ l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2) \right] \dfrac{d \theta_1}{dt} + l_2 \cos(\theta_1 + \theta_2) \dfrac{d \theta_2}{dt} \\ \end{array}$

Note that the end-effector's angular velocity is, of course, the sum of the joint velocities. Another possibility that yields exactly the same result would be to explicitly include the equation for angular velocity and then calculate the Jacobian accordingly. This result is, as could already be expected, the same as the one in the above example, where the link transformation matrices were used to achieve the result.

Inverse Velocity Kinematics

This problem can easily be solved by inverting the Jacobian...

Singularities

If the Jacobian $J$ is invertible, inverting it can be used to easily calculate the joint velocities if the (Cartesian) end-effector velocity is given. Locations (combinations of $\theta_i$ where the Jacobian is not invertible are called singularities. Setting the determinant of $J$ equal to zero and solving for $\theta$ allows for finding these singularities. These positions correspond to the loss of a degree of freedom.