# 3D Geometry for Robotics

In order to proceed with writing a simple robot simulator, there are a number of mathematical concepts that must be introduced before we can describe the motion of a manipulator.

In this post, I’ll be pulling sections from my PhD notes which, in turn, were largely taken from the excellent introductory text Modeling and Control of Robot Manipulators.

# Rigid body pose

The position and orientation (together, pose) of a rigid body in space can be described with respect to a fixed coordinate frame in terms of a translation from the fixed frame’s origin and rotation about the axes of the fixed frame. Here, we let $O$ denote the fixed coordinate frame and $O'$ the body coordinate frame, as shown below.

As shown, the fixed coordinate frame with origin $O$ is specified by unit axes $\mathbf{x}$, $\mathbf{y}$ and $\mathbf{z}$, while the body coordinate frame with origin $O'$ be specified by unit axes $\mathbf{x'}$, $\mathbf{y'}$ and $\mathbf{z'}$.

## Translation

The translation of frame $O'$ with respect to frame $O$ is

which can be written, in terms of the axes of the fixed frame, as the vector $\mathbf{o'} = [o'_{x}, o'_{y}, o'_{z}]^{T}$.

## Rotation

The rotation of the body frame with respect to the fixed frame is specified by expressing the axes of the body frame in terms of the axes of the fixed frame. This yields

These relations can be expressed compactly in the rotation matrix

This matrix simply describes the body frame’s axes in terms of the axes of the fixed frame. The rightmost expression can be verified by substituting in the definitions of $\mathbf{x'}$, $\mathbf{y'}$ and $\mathbf{z'}$ from above and using the fact that these axes vectors are mutually orthogonal (actually, mutually orthonormal since they are unit vectors). This means that dot products between different axes vectors are zero, eg if we denote $\mathbf{x'}$, $\mathbf{y'}$ and $\mathbf{z'}$ instead by $\mathbf{e_{1}}$, $\mathbf{e_{2}}$ and $\mathbf{e_{3}}$ respectively:

Also recall that the dot product can be written

where $\theta_{i,j}$ is the angle between the vectors. Since the axes of both body and fixed frames are unit vectors, we can rewrite the orientation of the body frame as

where $\theta_{i,j}$ again describes the angle between two axes unit vectors of the fixed and body frames. For this reason, the rotation matrix is sometimes referred to as the direction cosine matrix or DCM (where each element is referred to as a direction cosine).

Now, let’s take a quick look at propertis of the rotation matrix and how it’s used.

### Inverse rotations

If the rotation matrix $R$ represents the orientation of the body frame with respect to the fixed frame, then its inverse $R^{-1}$ represents the opposite rotation - the orientation of the fixed frame with respect to the body frame.

Recall from above that the columns of the rotation matrix were said to be mutually orthogonal - we call this an orthogonal matrix which has the special property that its inverse is equal to its transpose:

This makes transforming between frames easy and computationally-efficient. In addition, as was mentioned above, the columns of a rotation matrix are all unit vectors. This means its columns are mutually orthonormal, or in other words, $R$ is an orthonormal matrix.

### Representing a vector in different frames

Consider two frames with the same origin $O$ but different sets of unit axes, as shown below.

A vector $\mathbf{p}$ can be described in terms of the first frame as $\mathbf{p} = [p_{x}, p_{y}, p_{z}]^{T}$ and in terms of the second frame as $\mathbf{p'} = [p'_{x}, p'_{y}, p'_{z}]^{T}$. Since these vectors correspond to the same point in space, we must have $\mathbf{p} = \mathbf{p'}$ and thus

This is, from above, $\mathbf{p} = R\mathbf{p'}$; $R$ is the rotation matrix which transforms the vector from the second frame to the first. Since $R$ is orthogonal, the inverse transformation is $\mathbf{p'} = R^{T}\mathbf{p}$.

Note that since $R$ is an orthogonal transformation we have

The matrix $R$ therefore does not change the norm of the vector but only its direction, which is a property we expect from pure rotation.

### Composing multiple successive rotations

Consider a point $\mathbf{p}$ in space and its representations in the frames $O^{0}$, $O^{1}$ and $O^{2}$ having a common origin $O$. We introduce the noration $R_{i}^{j}$ to denote the rotation matrix describing the orientation of frame $i$ with respect to frame $j$; then we have

We also have, using the equation above,

Finally, we can write $\mathbf{p^{0}} = R_{2}^{0}\mathbf{p^{2}} = R_{1}^{0}R_{2}^{1}\mathbf{p^{2}}$

From the above equation it is obvious that we must have

Here it is interpreted that a frame aligned with $O^{0}$ is first rotated into alignment with $O^{1}$ with $R_{1}^{0}$ and then this frame is rotated into alignment with frame $O^{2}$ using $R_{2}^{1}$. That is, successive rotations can be applied to a vector via post-multiplication of the corresponding rotation matrices following the order of rotations.

The overall rotation is therefore expressed as the composition of partial rotations; each rotation is defined with respect to the axes of the current frame in the sequence. When each rotation is made instead with respect to the axes of a fixed frame, the elementary rotation matrices are premultiplied in the order in which the rotations are applied.

There’s a LOT more to be said about representing rotations in three dimensions, but that will have to wait for a dedicated post. For now, the point is that we understand what a rotation matrix is and how it’s formed from the axes of the fixed frame and frame of interest (here, the frame attached to a rigid body).

## Homogeneous Transformations

We’ve seen what it means to express the translation and rotation of one frame with respect to another, but for the sake of convenience we’d like to be able to desribe the full pose in one mathematical expression.

Recall from above that a point $p_{1}$ on a rigid body in frame $O^{1}$ is fully specified with respect to a fixed reference frame $O^{0}$ by a vector $\mathbf{o}_{1}^{0}$ describing the origin of Frame 1 with respect to Frame 0 and a rotation matrix $R_{1}^{0}$ describing the orientation of Frame 1 with respect to Frame 0.

The position of point P in Frame 0 is then

This specifies a coordinate transformation of a vector between the two frames. The inverse transformation comes from premultiplying both sides of the above equation by ${R_{1}^{0}}^{T}=R_{0}^{1}$ and solving for $p^{1}$; this leads to

These transformations can be expressed in a more compact form by converting to homogeneous representations given by

and, for the inverse transform,

where $\tilde{p} = [p, 1]^{T}$ is a homogeneous vector and $A_{0}^{1}$ and $A_{1}^{0}$ are homogeneous transformation matrices. It is important to note that, in general, homogeneous transformation matrices are NOT orthogonal, so unlike for rotation matrices we do not have $A^{-1}=A^{T}$.

Successive homogeneous transformations are composed in the same manner as for rotation matrices:

As before, note that post-multiplication is used because these matrices are all defined with respect to the preceding frame in the chain - this will be useful in defining a systematic way to transform between links of a robot manipulator. When transformations are all defined relative to the same fixed frame, we pre-multiply them as was done for pure rotations above.

# Wrapping up

We introduced the basics for 3D geometry, including rotations, translations and homogeneous transforms. Next time, we’ll build on this foundation to derive the kinematics of arbitrary robot manipulators.