Forward kinematics allow us to express the pose of a mechanism given its actuator parameters such as the angle of a joint or the number of rotations of its wheel as well as their derivatives including speed and accleration, without taking into account the forces that cause the motion. So far, we have considered the forward kinematics of wheeled mechanisms and simple arms. The goals of this lecture are to

1. Study the forward kinematics of multi-joint, non-planar arms
2. Introduce the Denavit-Hartenberg schema to systematically derive the forward kinematic equations

# Manipulating arms

A manipulating arm consists of links that are connected by joints. Joints can be either rotational or prismatic, i.e., change their length and thus providing additional degrees of freedom. Knowing the length of all rigid links, the position of the manipulators end-effector is fully described by its joint angles and joint offset (for prismatic joints).

In industrial manipulators, the number of joints is usually equal to the degrees of freedom of the manipulator. As most manipulators are holonomic, the forward kinematics allow you – unlike on non-holonomic wheeled platforms – to directly relate absolute positions of joints with absolute positions in Cartesian space. It is also possible to derive equations that relate the speed of the joints to speed in Cartesian space. Like for wheeled platforms, this can be achieved via a Jacobian matrix.  At certain positions, the mapping provided by the Jacobian is not invertible, i.e., some velocities in Cartesian space are unachievable. These points are called singularities.

## Coordinate systems

Given some kind of fixed coordinate system, we can describe the position of a robot’s end-effector by a 3×1 position vector. As for mobile robots, there can be many coordinate systems defined on a robot and the environment. We identify the coordinate system a point relates to by a preceeding super-script, e.g., $^AP$ to indicate that point $P$ is in coordinate system ${A}$. Each point consists of three elements $^AP=[p_x p_y p_z]^T$.

When grasping an object, not only the position of the end-effector is important, but also its orientation. In order to describe the orientation of a point, we will attach a coordinate system to it. Let $\hat{X}_B, \hat{Y}_B$ and $\hat{Z}_B$ be unit vectors that correspond to the principal axes of a coordinate system ${B}$. When written in terms of coordinate system ${A}$, they are denoted $^A\hat{X}_B, ^A\hat{Y}_B$ and $^A\hat{Z}_B$.

We can now stack these three vectors together into a 3×3 matrix to obtain the rotation matrix

$^A_BR=[^A\hat{X}_B ^A\hat{Y}_B ^A\hat{Z}_B]$

which describes ${B}$ relative to ${A}$. It is important to note that all columns in $^A_BR$ are unit vectors, so that the rotation matrix is orthogonal. This is important as this allows us to easily obtain the inverse of $^A_BR$ as $^A_BR^T$ or

$^B_AR=^A_BR^T$. A derivation of this as well as an illustration of ${B}$ and ${A}$  are shown in the following screen cast:

Together, position and orientation is known as frame, which is a set of four vectors, one for the position and three for the orientation, and we can write

$\{B\}=\{^A_BR, ^AP\}$

to describe the coordinate frame ${B}$ with respect to ${A}$ using a vector $P$ and a rotation matrix $R$. Modern robots have many, many of such frames, here for example those defined for Willow Garage’s PR2:

Coordinate frames on the PR2 robot

## Mapping from one frame to another

Having introduced the concept of frames, we need the ability to map coordinates in one frame to coordinates in another frame. For example, lets consider frame ${B}$ having the same orientation than frame ${A}$ and sitting at location $^AP$ in space. As the orientation of both frames is the same, we can express a point $^BQ$ in frame ${A}$ as

$^AQ=^BQ+^AP$

Actually adding two vectors that are in different reference frames, i.e., $^BQ+^AP$, is only possible if both of them have the same orientation. We can, however, convert from one reference frame to the other using the rotation matrix:

$^AP=^A_BR^BP$

and therefore solve the mapping problem regardless of the orientation of ${A}$ to ${B}$:

$^AQ=^A_BR^BQ+^AP$

When sticking to the notation, we can see that leading subscripts cancel the leading superscripts of the following vector/rotation matrix. Whereas we have now a solution to transfer a point from one frame of reference to another by combining a rotation and a translation, it would be more appealing to write something like that:

$^AQ=^A_BT^BQ$

In order to do this, we need to introduce a 4×1 position vector such that

$\left[\begin{array}{c}^AP\\\end{array}\right]=\left[\begin{array}{ccc|c} & ^A_BR & & ^AP \\\hline 0 & 0 & 0 & 1\end{array}\right]\left[\begin{array}{c}^BQ\\1\end{array}\right]$

and $latex^A_BT$ is a 4×4 matrix.  Note that the added 1s and $[0 0 0 1]$ do not affect the other entries in the matrix during matrix multiplication. The 4×4 matrix is called a homogenous transform.

We have now established a notation to convert points from one coordinate system to another. There are many possible ways this can be done, in particular how rotation can be represented (see below), but all can be converted from one into the other.

### Transformation arithmetic

Transformations can be combined: consider for example an arm with two links, reference frame ${A}$ at the base, ${B}$ at its first joint, and ${C}$ at its end-effector. Given the transforms $^B_CT$ and $^A_BT$, we can write

$^AP=^A_BT^B_CT^CP=^A_CT$

to convert a point in the reference frame of the end-effector to that of its base. As this works for rotation and translation operators independently, we can construct $^A_CT$ as

$^A_CT=\left[\begin{array}{ccc|c} & ^A_BR^B_CR & & ^A_BR^BP_C +^AP_B \\\hline 0 & 0 & 0 & 1\end{array}\right]$

where $^AP_B$ and $^BP_C$ are the translations from ${A}$ to ${B}$ and from ${B}$ to ${C}$, respectively.

## Other representations for orientation

So far, we have represented orientation by a 3×3 matrix who’s column vectors are orthogononal  unit vectors describing the orientation of a coordinate system. Orientation is therefore represented with 9 different values. In fact, 3 values are sufficient. This becomes clear when considering that orthogonality (dot product of all mutual vectors is zero) and vector length (each vector must have length 1) impose 6 constraints. Indeed, an orientation can be represented about a rotation by certain angles around the x, the y, and the z-axis of the reference coordinate system. This is known as the X-Y-Z fixed angels notation. Mathematically, this can be represented by a rotation matrix of the form

$^A_BR_{XYZ}(\gamma,\beta,\alpha)=\left[\begin{array}{ccc}cos\alpha & -sin\alpha & 0\\sin\alpha & \cos\alpha & 0\\0 & 0 & 1\end{array}\right]\left[\begin{array}{ccc}cos\beta& 0 & sin\beta\\0 & 1 & 0\\0 & 0 & 1\end{array}\right]\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & cos\gamma & -sin\gamma\\0 & sin\gamma & cos\gamma\end{array}\right]$

While the X-Y-Z fixed angels approach expresses a coordinate frame using rotations with respect to the original coordinate frame, say ${A}$, another possible description is to start with a coordinate frame ${B}$ that is coincident with frame ${A}$, then rotate around the Z-axis with angle $\alpha$, then the Y-axis with angle $\beta$ and finally around the X-axis with angle $\gamma$. This representation is called Z-Y-X Euler angels.

This is important, as current tools such as the ROS TF package allow for using any of these conventions.  Among these, the preferred representation for computational and stability reasons are Quaternions. A quaternion is a 4-tuple that extends the complex numbers with very general application in mathematics and representing orientation and rotation in particular. The basic idea is that each rotation can be represented as a rotation around a single axis (a vector in space) by a specific angle. Given such an axis $\hat{K}=[k_x k_y k_z]^T$ and an angle $\theta$, one can calculate the so-called Euler parameters or unit quaternion:

$\epsilon_1=k_x sin \frac{\theta}{2}$

$\epsilon_2=k_y sin \frac{\theta}{2}$

$\epsilon_3=k_x sin \frac{\theta}{2}$

$\epsilon_4=cos\frac{\theta}{2}$

These four quantities are constrained by the relationship

$\epsilon_1^2+\epsilon_2^2+\epsilon_3^2+\epsilon_4^2=1$

which might be visualized by a point on a unit hyper-sphere.

# Deriving the coordinate transforms for standard joints: The Denavit-Hartenberg scheme

Illustration of the Denavit Hartenberg parameters

So far, we learned how to express one coordinate system in another one and how to convert between different representations. In practice we would be interested in calculating the Cartesian position of a point right between the grippers of a gripper. We would convert this position (probably [0 0 0] or similar) into the coordinate frame of the wrist, then in the elbow and finally in that of the base. While it is relatively straightforward to do this manually for simple kinematic mechanisms, Denavit and Hartenberg have developed a scheme that describes each joint-joint transformation by four parameters. These parameters are

• the length $r$ of the common normal between the axis of two joints i and i-1 (link length)
• the angle $\alpha$ between the axis of the two joints with respect to the common normal (link twist)
• the distance $d$ between the joint axes (link offset)
• the rotation $\theta$ around the common axis along which the link offset is measured (joint angle)
These quantities are illustrated well in this video:
Two of the D-H parameters describe the link between the joints, and the other two describe the link’s connection to a neighboring link. Depending on the link/joint type, these numbers are fixed or can be controlled. For example, in a revolute joint $\theta$ is the varying joint angle, while all other quantities are fixed.  Similarly, for a prismatic joint $d$ is the joint variable.
The coordinate transform from one link ($i-1$) to another ($i$) can now be constructed  using the following matrix:
In practice you will not need to do this, but will provide an XML description such as in URDF from which the transformation equations are generated.

# Take-home lessons

• The position of a point in space is given by a (3×1) vector. Its orientation is given by a 3×3 rotation matrix that consists of the unit vectors that span the local coordinate system at the point as their columns.
• Converting a point from one coordinate system to another requires a rotation and a translation. These can be combined in a single 4×4 matrix multiplication known as homogenous transforms.
• There are many different ways to represent rotations, including 3×3 rotation matrices, X-Y-Z fixed angles and quaternions.
• The Denavit-Hartenberg scheme is a notation system and algorithm to systematically generate the homogenous transforms that describe the coordinate system transformations of a manipulating arm.