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

- Study the forward kinematics of multi-joint, non-planar arms
- 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., to indicate that point is in coordinate system . Each point consists of three elements .

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 and be unit vectors that correspond to the principal axes of a coordinate system . When written in terms of coordinate system , they are denoted and .

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

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

. A derivation of this as well as an illustration of and 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

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

## 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 having the same orientation than frame and sitting at location in space. As the orientation of both frames is the same, we can express a point in frame as

Actually adding two vectors that are in different reference frames, i.e., , 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:

and therefore solve the mapping problem regardless of the orientation of to :

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:

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

and $latex^A_BT$ is a 4×4 matrix. Note that the added `1`s and 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 at the base, at its first joint, and at its end-effector. Given the transforms and , we can write

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 as

where and are the translations from to and from to , 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

While the X-Y-Z fixed angels approach expresses a coordinate frame using rotations with respect to the original coordinate frame, say , another possible description is to start with a coordinate frame that is coincident with frame , then rotate around the Z-axis with angle , then the Y-axis with angle and finally around the X-axis with angle . 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 and an angle , one can calculate the so-called **Euler parameters** or **unit quaternion**:

These four quantities are constrained by the relationship

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

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

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 of the common normal between the axis of two joints i and i-1 (
**link length)** - the angle between the axis of the two joints with respect to the common normal (
**link twist**) - the distance between the joint axes (
**link offset**) - the rotation around the common axis along which the link offset is measured (
**joint angle**)

# 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.

# Further Reading

The content of this lecture is inspired from and explained in more depth in Introduction to Robotics: Mechanics and Control (3rd Edition) by John J. Craig.

[…] In 6D (translation and rotation), an equivalent notation can be found for a transformation (see forward kinematics). An error function for the squared distance is then given […]