Manipulator
Differential Kinematics
Part 1: Kinematics, Velocity, and Applications

In the following sections, we use the skew and inverse skew operation as defined in Figure 3. Now consider the end-effector pose, which is a function of the joint coordinates given by ( Forward Kinematics). Its derivative with respect to time is

where each $\frac{\partial{{\mathbf{T}}}}{\partial q_{i}}\in\mathbb{R}^{4\times 4}$.

The information in ${{\mathbf{T}}}$ is non-minimal, and redundant, as is the information in $\dot{{{\mathbf{T}}}}$. We can write these respectively as

where ${{\mathbf{R}}}\in\mathrm{{\bf SO}(3)}$, $\dot{{{\mathbf{R}}}}\in\mathbb{R}^{3\times 3}$, and ${\bm{t}},\dot{{\bm{t}}}\in\mathbb{R}^{3}$.

We will write the partial derivative in partitioned form as

where ${{\mathbf{J}_{\!R}}}_{j}\in\mathbb{R}^{3\times 3}$ and ${{\mathbf{J}_{\!t}}}_{j}\in\mathbb{R}^{3\times 1}$, and then rewrite (6) as

and write a matrix equation for each non-zero partition

where each term represents the contribution to end-effector velocity due to the motion of the corresponding joint.

Taking (10) first, we can simply write

where ${{\mathbf{J}}}_{v}({\bm{q}})\in\mathbb{R}^{3\times n}$ is the translational part of the manipulator Jacobian matrix.

Rotation rate is slightly more complex, but using the identity $\dot{{{\mathbf{R}}}}=\left[\bm{\omega}\right]_{\times}{{\mathbf{R}}}$ where ${\bm{\omega}}\in\mathbb{R}^{3}$ is the angular velocity, and $\left[\bm{\omega}\right]_{\times}\in\bf{so}(3)$ is a skew-symmetric matrix, we can rewrite (9) as

and rearrange to

This $3\times 3$ matrix equation therefore has only three unique equations so applying the inverse skew operator to both sides we have

where ${{\mathbf{J}_{\!\omega}}}({\bm{q}})\in\mathbb{R}^{3\times n}$ is the rotational part of the manipulator Jacobian.

Combining (11) and ( First Derivative of a Pose) we can write

which expresses end-effector spatial velocity ${{}^{0}{\bm{\nu}}}=(v_{x},\ v_{y},\ v_{z},\ \omega_{x},\ \omega_{y},\ \omega_{z})$ in the world frame in terms of joint velocity and

is the manipulator Jacobian matrix expressed in the world-coordinate frame. The Jacobian expressed in the end-effector frame is

where ${{}^{0}{{\mathbf{R}}}}_{e}$ is a rotation matrix representing the orientation of the end-effector in the world frame.

Combining (14) and (15) we write

which provides the derivative of the left side of ( Forward Kinematics). However, in order to compute (17), we need to first compute (8), that is $\frac{\partial{{\mathbf{T}}}}{\partial q_{j}}$.

Before differentiating the ETS to find the manipulator Jacobian, it is useful to consider the derivative of a single Elementary Transform.

Now we can calculate the derivative of an ETS. To find out how the $j^{th}$ joint affects the end-effector pose, apply the chain rule to ( Forward Kinematics)

The derivative of the elementary transform with respect to a joint coordinate in ( The Manipulator Jacobian) is obtained using one of (20), (21), or (22) for a revolute joint, or one of (23), (24), or (25) for a prismatic joint.

Using ( First Derivative of a Pose) with ( The Manipulator Jacobian) we can form the angular velocity component of the $j^{th}$ column of the manipulator Jacobian

where the $\rho(\cdot)$ operation is defined in Figure 4. Using (11) with ( The Manipulator Jacobian), the translational velocity component of the $j^{th}$ column of the manipulator Jacobian is

where the $\tau(\cdot)$ operation is defined in Figure 4.

Stacking the translational and angular velocity components, the $j^{th}$ column of the manipulator Jacobian becomes

where the full manipulator Jacobian is

and we display a visualisation of the general structure of the manipulator Jacobian in Figure 5.

Calculating the manipulator Jacobian using (27) and (28) is easy to understand, but has $\mathcal{O}(n^{2})$ time complexity – we can do better.

Expanding (27) using ( The Manipulator Jacobian) and simplifying using ${{\mathbf{R}}}{{\mathbf{R}}}^{\top}={{\mathbf{1}}}$ gives

where ${}^{0}{{\mathbf{T}}}_{j}$ represents the transform from the base frame to frame $\{j\}$ as described by (5), and $[{\hat{{\bm{G}}}}_{\mu(j)}]$ corresponds to one of the six elementary transform derivatives from equations (20)-(22) and (23)-(25).

In the case of a prismatic joint, $\rho({\hat{{\bm{G}}}}_{\mu(j)})$ will be a $3\times 3$ matrix of zeros which results in zero angular velocity. In the case of a revolute joint, the angular velocity is parallel to the axis of joint rotation.

where $\begin{pmatrix}{\hat{{\bm{n}}}}_{j}&{\hat{{\bm{o}}}}_{j}&{\hat{{\bm{a}}}}_{j}\end{pmatrix}=\rho(^{0}{{\mathbf{T}}}_{j})$ are the columns of a rotation matrix as shown in Figure 6.

Expanding (28) using ( The Manipulator Jacobian) provides

which reduces to

where $\begin{pmatrix}x_{e}&y_{e}&z_{e}\end{pmatrix}^{\top}=\tau({{}^{j}{{\mathbf{T}}}}_{e})$.

This simplification reduces the time complexity of computation of the manipulator Jacobian to $\mathcal{O}(n)$.

Resolved-rate motion control (RRMC), a direct application of the first-order differential equation we generated in (17), is a simple and elegant method to generate joint velocities which create a desired Cartesian end effector motion [6]. The ability to instantaneously change the end-effector velocity is at the heart of sensor-based reactive control. By “closing the loop” on sensory information a robot can adapt to changes in its environment as new knowledge is acquired by sensors. A well-known example is visual servoing, a technique that pairs robotic vision with RRMC to guide the manipulator to an image-based goal [7]. RRMC is also foundational for numerical inverse kinematics which we will visit in the next section [8, 9]. Of course, this introductory technique, while still highly relevant and powerful, can be extended and enhanced to improve robustness and consider constraints such as joint limits and environmental collisions [10, 11, 12, 13, 14, 15].

We first re-arrange (17)

which can only be solved when ${{{{\mathbf{J}}}}\left({\bm{q}}\right)}$ is square (and non-singular), which is when the robot has six degrees-of-freedom.

For redundant robots, there is no unique solution for (35). Consequently, the most common approach is to use the Moore-Penrose pseudoinverse

The pseudoinverse will return joint velocities with the minimum velocity norm of the possible solutions, although note an important caveat on mixed units described in Excurse 1.

Immediately from this, we can construct a primitive open-loop velocity controller. At each time step, we must calculate the manipulator Jacobian ${{{{\mathbf{J}}}}\left({\bm{q}}\right)}$ which is a function of the robot’s current configuration ${\bm{q}}$. Then we set ${{}^{0}{\bm{\nu}}}$ to the desired spatial velocity of the end-effector.

A more useful application of RRMC is to employ it in a closed-loop pose controller which we denote as position-based servoing (PBS). Using this method we can make the end-effector travel in a straight line, in the robot’s task space, towards some desired end-effector pose as displayed in Figure 7. The PBS scheme relies on an error vector which represents the translation and rotation from the end-effector’s current pose to the desired pose

where the top three rows correspond to the translational error in the world frame, the bottom three rows correspond to the rotational error in the world frame, ${{}^{0}{{\mathbf{T}}}_{e}}={\cal K}({\bm{q}})$ is the forward kinematics of the robot which represents the end-effector pose in the base frame of the robot, $\,{}^{\scriptscriptstyle 0}\!\,{{\mathbf{T}}}_{e^{*}}$ is the desired end-effector pose in the base frame of the robot ($\cdot^{*}$ denotes desired), and $\alpha(\cdot):\mathrm{{\bf SO}(3)}\mapsto\mathbb{R}^{3}$ transforms a rotation matrix to its Euler vector equivalent [9]. The Euler vector is a form of angle-axis representation and specifies an axis of rotation and the angle of rotation about that axis.

If ${{\mathbf{R}}}$ is not a diagonal matrix then the Euler vector equivalent of ${{\mathbf{R}}}$ is calculated as

where using the convention from Figure 4 we define $\rho({{\mathbf{T}}})={{\mathbf{R}}}=\{r_{ij}\}$ and

If ${{\mathbf{R}}}$ is a diagonal matrix then we use different formulas. For the case where $\begin{pmatrix}r_{11}&r_{22}&r_{33}\end{pmatrix}=\begin{pmatrix}1&1&1\end{pmatrix}$ then $\alpha({{\mathbf{R}}})=\begin{pmatrix}0&0&0\end{pmatrix}^{\top}$ otherwise

To construct the PBS scheme we take the error term from (37) to set ${\bm{\nu}}$ in (35) (or (36) for robots with 7+ degrees of freedom) at each time step

where ${{\mathbf{k}}}$ is a proportional gain with units $s^{-1}$ that controls the rate of convergence to the goal and is typically a diagonal matrix to set gains for each task-space DoF

where $k_{t}$ is the translational motion gain and $k_{r}$ is the rotational motion gain. The inverse of the gain, ${{\mathbf{k}}}^{-1}$, corresponds to the time constant of the scheme. Note that since the error ${\bm{e}}$ is in the base frame of the robot we must use the base-frame manipulator Jacobian ${}^{0}{{\mathbf{J}}}({\bm{q}})$ in (35) or (36).

The control scheme we have just described will cause the error to asymptotically decrease to zero. For real applications, this is slow and impractical. We can improve this by increasing $k_{t}$ and $k_{r}$ and capping the end-effector velocity norm ${\lVert{\bm{\nu}}\rVert}$ at some value $\nu_{m}$, before stopping when the error norm ${\lVert{\bm{e}}\rVert}$ drops below some value $e_{m}$

This will cause ${\lVert{\bm{\nu}}\rVert}$ to be consistent until $e_{m}$ is reached and subsequently ${\lVert{\bm{\nu}}\rVert}$ asymptotically decreases to safely stop the robot. The effect of this is displayed in Figure 8 where $\nu_{m}$ has been set to 2.0. This approach involves combining mixed units, see Excurse 1.

This particular approach may cause unintuitive results as both ${\lVert{\bm{e}}\rVert}$ and ${\lVert{\bm{\nu}}\rVert}$ are non-homogeneous – they combine translational and rotational units. Therefore, a more intuitive approach would be to modify (43) to adjust the translational and angular velocity profiles separately. Alternate velocity profiles, such as a linearly decreasing velocity norm, are possible through modification of (43).

Although we have the safety mechanism of capping ${\lVert{\bm{\nu}}\rVert}$ at $\nu_{m}$, care must still be taken to not increase $k_{t}$ and $k_{r}$ without due consideration. Well-known problems caused by an excessive gain value, such as overshoot and system instability, may become apparent, especially on a real-world robot (which has non-modelled dynamics including flexibility, communication delay, and non-linearities).

For a reactive task, where a dynamic environment is causing the goal pose to change, simply update the desired end-effector pose $\,{}^{\scriptscriptstyle 0}\!\,{{\mathbf{T}}}_{e^{*}}$ in (37) at each time step, based on sensory information. A technique such as position-based visual servoing could be used to achieve this [7]. Refer to Notebook 3 where we use the Swift simulator to show this in action.

Inverse kinematics is the problem of determining the corresponding joint coordinates, given some end-effector pose. There are two approaches to solving inverse kinematics: analytical and numerical.

Analytical formulas must be pre-generated for a given manipulator and in some cases may not exist. The IKFast program provided as a part of OpenRAVE, pre-compiles analytic solutions for a given manipulator in optimised C++ code [17]. After this initial step, inverse kinematics can be computed rapidly, taking as little as 5 microseconds. However, analytic solutions generally can not optimise for additional criteria such as joint limits.

Numerical inverse kinematics use an iterative technique and can additionally consider extra constraints such as collision avoidance, joint limit avoidance, or manipulability [8, 9, 18]. In this section, we first construct a primitive numerical inverse kinematics solver, before showing how it can be improved using advanced optimisation techniques.

The Newton-Raphson (NR) method for inverse kinematics is remarkably similar to RRMC in a position-based servoing scheme. However, instead of sending joint velocities to the manipulator, the joint velocities update the configuration of a virtual manipulator until the goal pose is reached. To find the joint coordinates which correspond to some end-effector pose ${}^{0}{{\mathbf{T}}}_{e^{*}}$, the NR method seeks to minimise an error function

where ${\bm{e}}$ is defined in (37), and ${{\mathbf{W}}}_{e}=\mbox{diag}({\bm{w}_{e}})\big{(}{\bm{w}_{e}}\in(\mathbb{R}^{+})^{n}\big{)}$ is a diagonal weighting matrix which prioritises the corresponding error term. To achieve this, we iterate upon the following

With this approach, ${}^{0}{{\mathbf{J}}}({\bm{q}})$ must be square (and non-singular) to be invertible and is therefore limited to manipulators with 6 joints.

When using the NR method, the initial joint coordinates $q_{0}$ should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the problem is solvable, it converges very quickly. However, this method frequently fails to converge on the goal. We can improve the solvability of the problem and add compatibility with manipulators with greater than six joints, by using the Gauss-Newton (GN) method

where ${{\mathbf{J}}}={{}^{0}{{\mathbf{J}}}}$ is the base-frame manipulator Jacobian. If ${{\mathbf{J}}}({\bm{q}}_{k})$ is non-singular, and ${{\mathbf{W}}}_{e}={{\mathbf{1}}}_{n}$, then (46) provides the pseudoinverse solution to (45). However, if ${{\mathbf{J}}}({\bm{q}}_{k})$ is singular, (46) cannot be computed and the GN solution is infeasible.

Most linear algebra libraries (including the Python numpy library) implement the pseudoinverse using singular value decomposition, which is robust to singular matrices. Therefore, we can make both solvers more robust by using the pseudoinverse instead of the normal inverse in (45) and (46).

However, the computation is still unstable near singular points. We can further improve the solvability through the Levenberg-Marquardt (LM) method

where ${{\mathbf{W}}}_{n}=\mbox{diag}({\bm{w}_{n}})\big{(}{\bm{w}_{n}}\in(\mathbb{R}^{+})^{n}\big{)}$ is a diagonal damping matrix. The damping matrix ensures that ${{\mathbf{A}}}_{k}$ is non-singular and positive definite. The performance of the LM method largely depends on the choice of ${{\mathbf{W}}}_{n}$. Wampler [19] proposed ${\bm{w}_{n}}$ to be a constant, Chan and Lawrence [20] proposed a damped least-squares method with

where $\lambda$ is a constant which does not have much influence on performance, and $E_{k}$ is the error value from (44) at time step $k$. Sugihara [9] proposed

where ${\tilde{{\bm{w}}}}_{n}\in\mathbb{R}^{n}$, $\hat{w}_{n_{i}}=l^{2}\sim 0.01l^{2}$, and $l$ is the length of a typical link within the manipulator.

An important point to note is that the above methods are subject to local minima and in some cases will fail to converge on the solution. The choice of the initial joint configuration ${\bm{q}}_{0}$ is important. An alternative approach is to re-start an IK problem with a new random ${\bm{q}}_{0}$ after a few $20\sim 50$ iterations rather than persist with a single attempt with $500\sim 5\,000$ iterations. This is a simple but effective method of performing a global search for the IK solution.

We display a comparison of IK methods presented in this tutorial in Table 1. Table 1 shows results for several IK algorithms trying to solve for $10\,000$ randomly generated reachable end-effector poses using a six degree-of-freedom UR5 manipulator. We show each method initialised with a random valid ${\bm{q}}_{0}$ and a maximum of 500 iterations to reach the goal before being declared infeasible. We also show each of the methods with 100 searches to reach the goal where a new search is initialised with a new random valid ${\bm{q}}_{0}$ after 30 iterations. The iterations recorded for this method include the iterations from failed searches.

We have presented some useful IK methods but it is by no means an exhaustive list. Sugihara [9] provides a good comparison of many different numerical IK methods. In Part 2 of this tutorial, we explore advanced IK techniques which incorporate additional constraints into the optimisation.

Manipulator performance metrics seek to quantify the performance of a manipulator in a given configuration. In this section, we explore two common manipulator performance metrics based on the manipulator Jacobian. A full survey of performance metrics can be found in [21]. It is important to note several considerations on how manipulator-based performance metrics should be used in practice. Firstly, the metrics are unitless, and the upper bound of a metric depends on the manipulator kinematic model (i.e. joint types and link lengths) and the units chosen to represent translation and orientation. Consequently, metrics computed for different manipulators are not directly comparable. Secondly, the manipulator Jacobian contains three rows corresponding to translational rates, and three rows corresponding to angular rates. Therefore, any metric using the whole Jacobian will produce a non-homogeneous result due to the mixed units. Depending on the manipulator scale, this can cause either the translational or rotational component to dominate the result. This problem also arises in manipulators with mixed prismatic and revolute joints. In general, the most intuitive use of performance metrics comes from using only the translational or rotational rows of the manipulator Jacobian (where the choice of which depends on the use case), and only using the metric on a manipulator comprising a single joint type [21].