Chapter

2 Representing Position
and Orientation

Numbers are an important part of mathematics. We use numbers for counting: there are
2 apples. We use denominate numbers, a number plus a unit, to specify distance: the
object is 2 m away. We also call this single number a scalar. We use a vector, a de-
nominate number plus a direction, to specify a location: the object is 2 m due north.
We may also want to know the orientation of the object: the object is 2 m due north
and facing west. The combination of position and orientation we call pose.
A point in space is a familiar concept from mathematics and can be described
by a coordinate vector, as shown in Fig. 2.1a. The vector represents the dis-
placement of the point with respect to some reference coordinate frame – we
call this a bound vector since it cannot be freely moved. A coordinate frame, or
Cartesian coordinate system, is a set of orthogonal axes which intersect at a point
known as the origin. A vector can be described in terms of its components, a linear
combination of unit vectors which are parallel to the axes of the coordinate frame.
Note that points and vectors are different types of mathematical objects even though
each can be described by a tuple of numbers. We can add vectors but adding points
makes no sense. The difference of two points is a vector, and we can add a vector to a
point to obtain another point.
A point is an interesting mathematical abstraction, but a real object comprises
infinitely many points. An object, unlike a point, also has an orientation. If we
attach a coordinate frame to an object, as shown in Fig. 2.1b, we can describe every
We assume that the object is rigid, that point within the object as a constant vector with respect to that frame.
Now we can
is, the points do not move with respect describe the position and orientation – the pose – of that coordinate frame with re-
to each other.
spect to the reference coordinate frame. To distinguish the different frames we label
them and in this case the object coordinate frame is labeled {B} and its axes are labeled
xB and yB, adopting the frame’s label as their subscript.
To completely describe the pose of a rigid object in a 3-dimensional world we need
6 not 3 dimensions: 3 to describe its position and 3 to describe its orientation. These
dimensions behave quite differently. If we increase the value of one of the position
dimensions the object will move continuously in a straight line, but if we increase the
value of one of the orientation dimensions the object will rotate in some way and soon
get back to its original orientation – this dimension is curved. We clearly need to treat
the position and orientation dimensions quite differently.

Fig. 2.1.
a The point P is described by a
coordinate vector with respect
to an absolute coordinate frame.
b The points are described with
respect to the object’s coordi-
nate frame {B} which in turn is
described by a relative pose ξB.
Axes are denoted by thick lines
with an open arrow, vectors by
thin lines with a swept arrow
head and a pose by a thick line
with a solid head
18 Chapter 2 · Representing Position and Orientation

The pose of the coordinate frame is denoted by the symbol ξ – pronounced ksi.
Figure 2.2 shows two frames {A} and {B} and the relative pose AξB which describes {B}
with respect to {A}. The leading superscript denotes the reference coordinate frame
and the subscript denotes the frame being described. We could also think of AξB as
describing some motion – imagine picking up {A} and applying a displacement and
a rotation so that it is transformed to {B}. If the initial superscript is missing we as-
sume that the change in pose is relative to the world coordinate frame which is gen-
erally denoted {O}.
The point P in Fig. 2.2 can be described with respect to either coordinate frame by
the vectors Ap or Bp respectively. Formally they are related by

(2.1)

where the right-hand side expresses the motion from {A} to {B} and then to P. The op-
erator i transforms the vector, resulting in a new vector that describes the same point
but with respect to a different coordinate frame.
An important characteristic of relative poses is that they can be composed or com-
pounded. Consider the case shown in Fig. 2.3. If one frame can be described in terms
of another by a relative pose then they can be applied sequentially

Fig. 2.2.
The point P can be described by
coordinate vectors relative to ei-
ther frame {A} or {B}. The pose
of {B} relative to {A} is AξB

In relative pose composition we can check that we have our reference frames correct by ensur-
ing that the subscript and superscript on each side of the ⊕ operator are matched. We can then
cancel out the intermediate subscripts and superscripts

leaving just the end most subscript and superscript which are shown highlighted.

Euclid of Alexandria (ca. 325 BCE–265 BCE) was a Greek mathematician,

who was born and lived in Alexandria Egypt, and is considered
the “father of geometry”. His great work Elements comprising
13 books, captured and systematized much early knowledge about
geometry and numbers. It deduces the properties of planar and
solid geometric shapes from a set of 5 axioms and 5 postulates.
Elements is probably the most successful book in the histo-
ry of mathematics. It describes plane geometry and is the ba-
sis for most people’s first introduction to geometry and formal
proof, and is the basis of what we now call Euclidean geometry.
Euclidean distance is simply the distance between two points on
a plane. Euclid also wrote Optics which describes geometric vi-
sion and perspective.
 Chapter 2 · Representing Position and Orientation 19

which says, in words, that the pose of {C} relative to {A} can be obtained by compound-
ing the relative poses from {A} to {B} and {B} to {C}. We use the operator ⊕ to indicate
composition of relative poses.
For this case the point P can be described by

Later in this chapter we will convert these abstract notions of ξ , i and ⊕ into stan-
dard mathematical objects and operators that we can implement in MATLAB®.
In the examples so far we have shown 2-dimensional coordinate frames. This is ap-
propriate for a large class of robotics problems, particularly for mobile robots which
operate in a planar world. For other problems we require 3-dimensional coordinate
frames to describe objects in our 3-dimensional world such as the pose of a flying or
underwater robot or the end of a tool carried by a robot arm.

Fig. 2.3.
The point P can be described by
coordinate vectors relative to
either frame {A}, {B} or {C}. The
frames are described by rela-
tive poses

Euclidean versus Cartesian geometry. Euclidean geometry is concerned with points and lines in
the Euclidean plane (2D) or Euclidean space (3D). It is entirely based on a set of axioms and
makes no use of arithmetic. Descartes added a coordinate system (2D or 3D) and was then
able to describe points, lines and other curves in terms of algebraic equations. The study
of such equations is called analytic geometry and is the basis of all modern geometry. The
Cartesian plane (or space) is the
Euclidean plane (or space) with all
its axioms and postulates plus the
extra facilities afforded by the ad-
ded coordinate system. The term
Euclidean geometry is often used
to mean that Euclid’s fifth postu-
late (parallel lines never intersect)
holds, which is the case for a pla-
nar surface but not for a curved
surface.

René Descartes (1596–1650) was a French philosopher, mathematician and part-time mercenary. He
is famous for the philosophical statement “Cogito, ergo sum” or “I am thinking, therefore I exist” or
“I think, therefore I am”. He was a sickly child and developed a life-long habit of lying in bed and
thinking until late morning. A possibly apocryphal story is that during one such morning he was
watching a fly walk across the ceiling and realized that he could describe its position in terms of
its distance from the two edges of the ceiling. This is the basis of the Cartesian coordinate system
and modern (analytic) geometry, which he described in his 1637 book La Géométrie. For the first
time mathematics and geometry were connected, and modern calculus was built on this foun-
dation by Newton and Leibniz. In Sweden at the invitation of Queen Christina he was obliged to
rise at 5 a.m., breaking his lifetime habit – he caught pneumonia and died. His remains were later
moved to Paris, and are now lost apart from his skull which is in the Musée de l’Homme. After his
death, the Roman Catholic Church placed his works on the Index of Prohibited Books.
20 Chapter 2 · Representing Position and Orientation

Figure 2.4 shows a more complex 3-dimensional example in a graphical form where
we have attached 3D coordinate frames to the various entities and indicated some
relative poses. The fixed camera observes the object from its fixed viewpoint and es-
timates the object’s pose FξB relative to itself. The other camera is not fixed, it is at-
tached to the robot at some constant relative pose and estimates the object’s pose CξB
relative to itself.
An alternative representation of the spatial relationships is a directed graph (see
Appendix I) which is shown in Fig. 2.5. Each node in the graph represents a pose It is quite possible that a pose graph can
and each edge represents a relative pose. An arrow from X to Y is denoted XξY and de- be inconsistent, that is, two paths through
the graph give different results. In robot-
scribes the pose of Y relative to X. Recalling that we can compose relative poses using
ics these poses are only ever derived from
the ⊕ operator we can write some spatial relationships noisy sensor data.

and each equation represents a loop in the graph with each side of the equation starting
and ending at the same node. Each side of the first equation represents a path through
the network from {0} to {B}, a sequence of edges (arrows) written in order.

Fig. 2.4.
Multiple 3-dimensional coordi-
nate frames and relative poses

Fig. 2.5.
Spatial example of Fig. 2.4
expressed as a directed graph
 Chapter 2 · Representing Position and Orientation 21

In mathematical terms poses constitute

a group – a set of objects that supports There are just a few algebraic rules:

an associative binary operator (composi-
tion) whose result belongs to the group,
an inverse operation and an identity ele-
ment. In this case the group is the special
Euclidean group in either 2 or 3 dimen-
sions which are commonly referred to as where 0 represents a zero relative pose. A pose has an inverse
SE(2) or SE(3) respectively.

which is represented graphically by an arrow from {Y} to {X}. Relative poses can
also be composed or compounded

It is important to note that the algebraic rules for poses are different to nor-
mal algebra and that composition is not commutative

with the exception being the case where ξ1 ⊕ ξ 2 = 0. A relative pose can trans-
form a point expressed as a vector relative to one frame to a vector relative to
another

A very useful property of poses is the ability to perform algebra. The second loop
equation says, in words, that the pose of the robot is the same as composing two rela-
tive poses: from the world frame to the fixed camera and from the fixed camera to the
Order is important here, and we add
ξ F robot. We can subtract ξF from both sides of the equation
by adding the inverse of
to the left on each side of the equation. ξF which we denote as
ξF and this gives

which is the pose of the robot relative to the fixed camera, shown as a dashed line
in Fig. 2.5.
We can write these expressions quickly by inspection. To find the pose of node X
with respect to node Y:

find a path from Y to X and write down the relative poses on the edges in a left to
right order;

if you traverse the edge in the direction of its arrow precede it with the ⊕ operator,
otherwise use
.

So what is ξ ? It can be any mathematical object that supports the algebra de-
scribed above and is suited to the problem at hand. It will depend on whether we
are considering a 2- or 3-dimensional problem. Some of the objects that we will
discuss in the rest of this chapter will be familiar to us, for example vectors, but
others will be more exotic mathematical objects such as homogeneous transfor-
mations, orthonormal rotation matrices, twists and quaternions. Fortunately all
these mathematical objects are well suited to the mathematical programming en-
vironment of MATLAB.
22 Chapter 2 · Representing Position and Orientation

To recap:

1. A point is described by a bound coordinate vector that represents its displacement

from the origin of a reference coordinate system.
2. Points and vectors are different things even though they are each described by a
tuple of numbers. We can add vectors but not points. The difference between two
points is a vector.
3. A set of points that represent a rigid object can be described by a single coordinate
frame, and its constituent points are described by constant vectors relative to that
coordinate frame.
4. The position and orientation of an object’s coordinate frame is referred to as its
pose.
5. A relative pose describes the pose of one coordinate frame with respect to another
and is denoted by an algebraic variable ξ .
6. A coordinate vector describing a point can be represented with respect to a dif-
ferent coordinate frame by applying the relative pose to the vector using the i op-
erator.
7. We can perform algebraic manipulation of expressions written in terms of relative
poses and the operators ⊕ and
.

The remainder of this chapter discusses concrete representations of ξ for various

common cases that we will encounter in robotics and computer vision. We start by
considering the two-dimensional case which is comparatively straightforward and
then extend those concepts to three dimensions. In each case we consider rotation
first, and then add translation to create a description of pose.

l
2.1 Working in Two Dimensions (2D)
A 2-dimensional world, or plane, is familiar to us from high-school Euclidean geom-
etry. We use a right-handed Cartesian coordinate system or coordinate frame with The relative orientation of the x- and
orthogonal axes denoted x and y and typically drawn with the x-axis horizontal and y-axes obey the right-hand rule as shown
on page 31.
the y-axis vertical. The point of intersection is called the origin. Unit-vectors paral-
lel to the axes are denoted ' and (. A point is represented by its x- and y-coordinates
(x, y) or as a bound vector

(2.2)

Figure 2.6 shows a red coordinate frame {B} that we wish to describe with respect
to the blue reference frame {A}. We can see clearly that the origin of {B} has been
displaced by the vector t = (x, y) and then rotated counter-clockwise by an angle θ .

Fig. 2.6.
Two 2D coordinate frames {A}
and {B} and a world point P.
{B} is rotated and translated
with respect to {A}
 2.1 · Working in Two Dimensions (2D) 23

A concrete representation of pose is therefore the 3-vector AξB ∼ (x, y, θ ), and we use
the symbol ∼ to denote that the two representations are equivalent. Unfortunately
this representation is not convenient for compounding since

is a complex trigonometric function of both poses. Instead we will look for a different
way to represent rotation and pose. We will consider the problem in two parts: rota-
tion and then translation.

l
2.1.1 Orientation in 2-Dimensions
l
2.1.1.1 Orthonormal Rotation Matrix

Consider an arbitrary point P which we can express with respect to each of the coor-
dinate frames shown in Fig. 2.6. We create a new frame {V} whose axes are parallel
to those of {A} but whose origin is the same as {B}, see Fig. 2.7. According to Eq. 2.2
we can express the point P with respect to {V} in terms of the unit-vectors that define
the axes of the frame

(2.3)

which we have written as the product of a row and a column vector.

The coordinate frame {B} is completely described by its two orthogonal axes which
we represent by two unit vectors

which can be factorized into matrix form as

(2.4)

Using Eq. 2.2 we can represent the point P with respect to {B} as

Fig. 2.7.
Rotated coordinate frames
in 2D. The point P can be con-
sidered with respect to the red
or blue coordinate frame
24 Chapter 2 · Representing Position and Orientation

and substituting Eq. 2.4 we write

(2.5)

Now by equating the coefficients of the right-hand sides of Eq. 2.3 and Eq. 2.5 we write

which describes how points are transformed from frame {B} to frame {V} when the
frame is rotated. This type of matrix is known as a rotation matrix since it transforms
a point from frame {V} to {B} and is denoted VRB

(2.6)

is a 2-dimensional rotation matrix with some special properties:

it is orthonormal (also called orthogonal) since each of its columns is a unit

vector and the columns are orthogonal. See Appendix B which provides a re-

the columns are the unit vectors that define the axes of the rotated frame Y fresher on vectors, matrices and linear
algebra.
with respect to X and are by definition both unit-length and orthogonal.

it belongs to the special orthogonal group of dimension 2 or R ∈ SO(2) ⊂ R2×2.
This means that the product of any two matrices belongs to the group, as does
its inverse.

its determinant is +1, which means that the length of a vector is unchanged
after transformation, that is,
Yp
=
Xp
, ∀θ .

the inverse is the same as the transpose, that is, R−1 = RT.

We can rearrange Eq. 2.6 as

Note that inverting the matrix is the same as swapping the superscript and subscript,
which leads to the identity R(−θ ) = R(θ )T.
It is interesting to observe that instead of representing an angle, which is a scalar, we
have used a 2 × 2 matrix that comprises four elements, however these elements are not
independent. Each column has a unit magnitude which provides two constraints. The
columns are orthogonal which provides another constraint. Four elements and three
constraints are effectively one independent value. The rotation matrix is an example of
a nonminimum representation and the disadvantages such as the increased memory it
requires are outweighed, as we shall see, by its advantages such as composability.
The Toolbox allows easy creation of these rotation matrices
>> R = rot2(0.2)
R =
0.9801 -0.1987
0.1987 0.9801
 2.1 · Working in Two Dimensions (2D) 25

where the angle is specified in radians. We can observe some of the properties such as
>> det(R)
ans =
1
and the product of two rotation matrices is also a rotation matrix
>> det(R*R)
ans =
1
You will need to have the MATLAB Sym- The Toolbox also supports symbolic mathematics
for example
bolic Math Toolbox™ installed.
>> syms theta
>> R = rot2(theta)
R =
[ cos(theta), -sin(theta)]
[ sin(theta), cos(theta)]
>> simplify(R*R)
ans =
[ cos(2*theta), -sin(2*theta)]
[ sin(2*theta), cos(2*theta)]
>> simplify(det(R))
ans =
1

l
2.1.1.2 Matrix Exponential
Consider a pure rotation of 0.3 radians expressed as a rotation matrix
>> R = rot2(0.3)
ans =
0.9553 -0.2955
0.2955 0.9553
We can compute the logarithm of this matrix using the MATLAB builtin function
logm is different to the builtin function logm

log which computes the logarithm of
>> S = logm(R)
each element of the matrix. A logarithm
S =
can be computed using a power series,
0.0000 -0.3000
with a matrix rather than scalar argu-
0.3000 0.0000
ment. For a matrix the logarithm is not
unique and logm computes the prin- and the result is a simple matrix with two elements having a magnitude of 0.3, which
cipal logarithm of the matrix. intriguingly is the original rotation angle. There is something deep and interesting
going on here – we are on the fringes of Lie group theory which we will encounter
throughout this chapter.

In 2 dimensions the skew-symmetric matrix is

(2.7)

which has clear structure and only one unique element ω ∈ R. A simple example of Toolbox
support for skew-symmetric matrices is
>> skew(2)
ans =
0 -2
2 0
and the inverse operation is performed using the Toolbox function vex
>> vex(ans)
ans =
2
26 Chapter 2 · Representing Position and Orientation

This matrix has a zero diagonal and is an example of a 2 × 2 skew-symmetric ma-

trix. The matrix has only one unique element and we can unpack it using the Toolbox
function vex
>> vex(S)
ans =
0.3000
to recover the rotation angle.
The inverse of a logarithm is exponentiation and using the builtin MATLAB matrix
exponential function expm expm is different to the builtin function
exp which computes the exponential
>> expm(S)
of each element of the matrix.
ans =
expm(A) = I + A + A2 / 2! + A3 / 3! + 
0.9553 -0.2955
0.2955 0.9553
the result is, as expected, our original rotation matrix. In fact the command
>> R = rot2(0.3);

is equivalent to
>> R = expm( skew(0.3) );

Formally we can write

where θ is the rotation angle, and the notation [·]×: R  R2×2 indicates a mapping from
a scalar to a skew-symmetric matrix.

l
2.1.2 Pose in 2-Dimensions
l
2.1.2.1 Homogeneous Transformation Matrix

Now we need to account for the translation between the origins of the frames shown
in Fig. 2.6. Since the axes {V} and {A} are parallel, as shown in Figs. 2.6 and 2.7, this
is simply vectorial addition

(2.8)

(2.9)

(2.10)

or more compactly as

(2.11)

where t = (x, y) is the translation of the frame and the orientation is ARB. Note that
AR = VR since the axes of frames {A} and {V} are parallel. The coordinate vectors for
B B
point P are now expressed in homogeneous form and we write
 2.1 · Working in Two Dimensions (2D) 27

A vector p = (x, y) is written in homogeneous form as p ∈ P2, p = (x1, x2, x3) where x = x1 / x3,
y = x2 / x3 and x3 ≠ 0. The dimension has been increased by one and a point on a plane is now
represented by a 3-vector. To convert a point to homogeneous form we typically append an ele-
ment equal to one p = (x, y, 1). The tilde indicates the vector is homogeneous.
Homogeneous vectors have the important property that p is equivalent to λ p for all λ ≠ 0
which we write as p  λ p. That is p represents the same point in the plane irrespective of the
overall scaling factor. Homogeneous representation is important for computer vision that we
discuss in Part IV. Additional details are provided in Sect. C.2.

and ATB is referred to as a homogeneous transformation. The matrix has a very

specific structure and belongs to the special Euclidean group of dimension 2 or
T ∈ SE(2) ⊂ R3×3.
By comparison with Eq. 2.1 it is clear that ATB represents translation and orienta-
tion or relative pose. This is often referred to as a rigid-body motion.

A concrete representation of relative pose ξ is ξ ∼ T ∈ SE(2) and T1 ⊕ T2  T1T2

which is standard matrix multiplication

One of the algebraic rules from page 21 is ξ ⊕ 0 = ξ . For matrices we know

that TI = T, where I is the identify matrix, so for pose 0  I the identity matrix.
Another rule was that ξ
ξ = 0. We know for matrices that TT −1 = I which im-
plies that
T  T −1

For a point described by p ∈ P2 then T i p  Tp which is a standard matrix-

vector product.

To make this more tangible we will show some numerical examples using MATLAB
and the Toolbox. We create a homogeneous transformation which represents a trans-
lation of (1, 2) followed by a rotation of 30°
>> T1 = transl2(1, 2) * trot2(30, 'deg')
T1 =
0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000
The function transl2 creates a relative pose with a finite translation but zero rota-
Many Toolbox functions have variants tion, while trot2 creates a relative pose with a finite rotation but zero translation.

that return orthonormal rotation ma- We can plot this, relative to the world coordinate frame, by
trices or homogeneous transformations,
for example, rot2 and trot2.
>> plotvol([0 5 0 5]);
>> trplot2(T1, 'frame', '1', 'color', 'b')
28 Chapter 2 · Representing Position and Orientation

Fig. 2.8.
Coordinate frames drawn using
the Toolbox function trplot2

The options specify that the label for the frame is {1} and it is colored blue and this
is shown in Fig. 2.8. We create another relative pose which is a displacement of (2, 1)
and zero rotation
>> T2 = transl2(2, 1)
T2 =
1 0 2
0 1 1
0 0 1
which we plot in red
>> trplot2(T2, 'frame', '2', 'color', 'r');

Now we can compose the two relative poses

>> T3 = T1*T2
T3 =
0.8660 -0.5000 2.2321
0.5000 0.8660 3.8660
0 0 1.0000
and plot it, in green, as
>> trplot2(T3, 'frame', '3', 'color', 'g');

We see that the displacement of (2, 1) has been applied with respect to frame {1}. It is
important to note that our final displacement is not (3, 3) because the displacement
is with respect to the rotated coordinate frame. The noncommutativity of composi-
tion is clearly demonstrated by
>> T4 = T2*T1;
>> trplot2(T4, 'frame', '4', 'color', 'c');

and we see that frame {4} is different to frame {3}.

Now we define a point (3, 2) relative to the world frame
>> P = [3 ; 2 ];

which is a column vector and add it to the plot

>> plot_point(P, 'label', 'P', 'solid', 'ko');

To determine the coordinate of the point with respect to {1} we use Eq. 2.1 and
write down
 2.1 · Working in Two Dimensions (2D) 29

and then rearrange as

Substituting numerical values

>> P1 = inv(T1) * [P; 1]
P1 =
1.7321
-1.0000
1.0000
where we first converted the Euclidean point coordinates to homogeneous form by ap-
pending a one. The result is also in homogeneous form and has a negative y-coordinate
in frame {1}. Using the Toolbox we could also have expressed this as
>> h2e( inv(T1) * e2h(P) )
ans =
1.7321
-1.0000
where the result is in Euclidean coordinates. The helper function e2h converts Euclidean
coordinates to homogeneous and h2e performs the inverse conversion.

l
2.1.2.2 Centers of Rotation
We will explore the noncommutativity property in more depth and illustrate with the
example of a pure rotation. First we create and plot a reference coordinate frame {0}
and a target frame {X}
>> plotvol([-5 4 -1 5]);
>> T0 = eye(3,3);
>> trplot2(T0, 'frame', '0');
>> X = transl2(2, 3);
>> trplot2(X, 'frame', 'X');
and create a rotation of 2 radians (approximately 115°)
>> R = trot2(2);

and plot the effect of the two possible orders of composition

>> trplot2(R*X, 'framelabel', 'RX', 'color', 'r');
>> trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

The results are shown as red coordinate frames in Fig. 2.9. We see that the frame {RX}
has been rotated about the origin, while frame {XR} has been rotated about the ori-
gin of {X}.
What if we wished to rotate a coordinate frame about an arbitrary point? First of
all we will establish a new point C and display it
>> C = [1 2]';
>> plot_point(C, 'label', ' C', 'solid', 'ko')

and then compute a transform to rotate about point C

>> RC = transl2(C) * R * transl2(-C)
RC =
-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000
and applying this
>> trplot2(RC*X, 'framelabel', 'XC', 'color', 'r');
30 Chapter 2 · Representing Position and Orientation

Fig. 2.9.
The frame {X} is rotated by
2 radians about {0} to give
frame {RX}, about {X} to
give {XR}, and about point C
to give frame {XC}

we see that the frame has indeed been rotated about point C. Creating the required
transform was somewhat cumbersome and not immediately obvious. Reading from
right to left we first apply an origin shift, a translation from C to the origin of the RC left multiplies X, therefore the first
reference frame, apply the rotation about that origin, and then apply the inverse transform applied to X is transl(-C),
then R, then transl(C).
origin shift, a translation from the reference frame origin back to C. A more descrip-
tive way to achieve this is using twists.

l
2.1.2.3 Twists in 2D
The corollary to what we showed in the last section is that, given any two frames we
can find a rotational center that will rotate the first frame into the second. For the case
of pure translational motion the rotational center will be at infinity. This is the key
concept behind what is called a twist.
We can create a rotational twist about the point specified by the coordinate vec-
tor C
>> tw = Twist('R', C)
tw =
( 2 -1; 1 )
and the result is a Twist object that encodes a twist vector with two components: a
2-vector moment and a 1-vector rotation. The first argument 'R' indicates a rota-
tional twist is to be computed. This particular twist is a unit twist since the magnitude
of the rotation, the last element of the twist, is equal to one.
To create an SE(2) transformation for a rotation about this unit twist by 2 radians
we use the T method
>> tw.T(2)
ans =
-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000
which is the same as that computed in the previous section, but more concisely speci-
fied in terms of the center of rotation. The center is also called the pole of the trans-
formation and is encoded in the twist
>> tw.pole'
ans =
1 2
 2.2 · Working in Three Dimensions (3D) 31

If we wish to perform translational motion in the direction (1, 1) the relevant unit
For a unit-translational twist the rotation twist is

is zero and the moment is a unit vector. >> tw = Twist('T', [1 1])
tw =
( 0.70711 0.70711; 0 )
and for a displacement of √⎯2 in the direction defined by this twist the SE(2) trans-
formation is
>> tw.T(sqrt(2))
ans =
1 0 1
0 1 1
0 0 1
which we see has a null rotation and a translation of 1 in the x- and y-directions.
For an arbitrary planar transform such as
>> T = transl2(2, 3) * trot2(0.5)
T =
0.8776 -0.4794 2.0000
0.4794 0.8776 3.0000
0 0 1.0000
we can compute the twist vector
>> tw = Twist(T)
tw =
( 2.7082 2.4372; 0.5 )
and we note that the last element, the rotation, is not equal to one but is the required ro-
tation angle of 0.5 radians. This is a nonunit twist. Therefore when we convert this to an
SE(2) transform we don’t need to provide a second argument since it is implicit in the twist
>> tw.T
ans =
0.8776 -0.4794 2.0000
0.4794 0.8776 3.0000
0 0 1.0000
and we have regenerated our original homogeneous transformation.

l
2.2 Working in Three Dimensions (3D)
The 3-dimensional case is an extension of the 2-dimensional case discussed in the
previous section. We add an extra coordinate axis, typically denoted by z, that is or-
thogonal to both the x- and y-axes. The direction of the z-axis obeys the right-hand
rule and forms a right-handed coordinate frame. Unit vectors parallel to the axes are
In all these identities, the symbols from denoted ', ( and ) such that

left to right (across the equals sign) are a
cyclic rotation of the sequence xyz.
(2.12)

A point P is represented by its x-, y- and z-coordinates (x, y, z) or as a bound vector

Figure 2.10 shows a red coordinate frame {B} that we wish to describe with respect
to the blue reference frame {A}. We can see clearly that the origin of {B} has been

Right-hand rule. A right-handed coordinate frame is defined by the first three fingers of your right
hand which indicate the relative directions of the x-, y- and z-axes respectively.
32 Chapter 2 · Representing Position and Orientation

Fig. 2.10.
Two 3D coordinate frames {A}
and {B}. {B} is rotated and trans-
lated with respect to {A}

displaced by the vector t = (x, y, z) and then rotated in some complex fashion. Just
as for the 2-dimensional case the way we represent orientation is very important.
Our approach is to again consider an arbitrary point P with respect to each of the
coordinate frames and to determine the relationship between Ap and Bp. We will
again consider the problem in two parts: rotation and then translation. Rotation
is surprisingly complex for the 3-dimensional case and we devote all of the next
section to it.

l
2.2.1 Orientation in 3-Dimensions
Any two independent orthonormal coordinate frames
can be related by a sequence of rotations (not more than three)
about coordinate axes, where no two successive rotations may be about the same axis.
Euler’s rotation theorem (Kuipers 1999).

Figure 2.10 shows a pair of right-handed coordinate frames with very different orien-
tations, and we would like some way to describe the orientation of one with respect
to the other. We can imagine picking up frame {A} in our hand and rotating it until
it looked just like frame {B}. Euler’s rotation theorem states that any rotation can be
considered as a sequence of rotations about different coordinate axes.
We start by considering rotation about a single coordinate axis. Figure 2.11 shows a
right-handed coordinate frame, and that same frame after it has been rotated by vari-
ous angles about different coordinate axes.
The issue of rotation has some subtleties which are illustrated in Fig. 2.12. This
shows a sequence of two rotations applied in different orders. We see that the final
orientation depends on the order in which the rotations are applied. This is a deep
and confounding characteristic of the 3-dimensional world which has intrigued math-
ematicians for a long time. There are implication for the pose algebra we have used
in this chapter:

In 3-dimensions rotation is not commutative – the order in which rotations are

applied makes a difference to the result.

Mathematicians have developed many ways to represent rotation and we will discuss
several of them in the remainder of this section: orthonormal rotation matrices, Euler
and Cardan angles, rotation axis and angle, exponential coordinates, and unit quater-
nions. All can be represented as vectors or matrices, the natural datatypes of MATLAB
or as a Toolbox defined class. The Toolbox provides many function to convert between
these representations and these are shown in Tables 2.1 and 2.2 (pages 57, 58).
 2.2 · Working in Three Dimensions (3D) 33

Rotation about a vector. Wrap your right hand around the vector with your thumb (your x-finger) in
the direction of the arrow. The curl of your fingers indicates the direction of increasing angle.

Fig. 2.11.
Rotation of a 3D coordinate frame.
a The original coordinate frame,
b–f frame a after various rota-
tions as indicated

Fig. 2.12.
Example showing the noncom-
mutativity of rotation. In the top
row the coordinate frame is ro-
tated by ü about the x-axis and
then ü about the y-axis. In the
bottom row the order of rota-
tions has been reversed. The re-
sults are clearly different
34 Chapter 2 · Representing Position and Orientation

l
2.2.1.1 Orthonormal Rotation Matrix
Just as for the 2-dimensional case we can represent the orientation of a coordinate frame
by its unit vectors expressed in terms of the reference coordinate frame. Each unit vec-
tor has three elements and they form the columns of a 3 × 3 orthonormal matrix ARB

(2.13)

which transforms the description of a vector defined with respect to frame {B} to
a vector with respect to {A}.

A 3-dimensional rotation matrix XRY has some special properties:

it is orthonormal (also called orthogonal) since each of its columns is a unit

vector and the columns are orthogonal. See Appendix B which provides a re-

the columns are the unit vectors that define the axes of the rotated frame Y fresher on vectors, matrices and linear
algebra.
with respect to X and are by definition both unit-length and orthogonal.

it belongs to the special orthogonal group of dimension 3 or R ∈ SO(3) ⊂ R3×3.
This means that the product of any two matrices within the group also belongs
to the group, as does its inverse.

its determinant is +1, which means that the length of a vector is unchanged
after transformation, that is,
Yp
=
Xp
, ∀θ .

the inverse is the same as the transpose, that is, R−1 = RT.

The orthonormal rotation matrices for rotation of θ about the x-, y- and z-axes are

The Toolbox provides functions to compute these elementary rotation matrices,

for example Rx(θ ) is
>> R = rotx(pi/2)
R =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
and its effect on a reference coordinate frame is shown graphically in Fig. 2.11b. The
functions roty and rotz compute Ry(θ ) and Rz(θ ) respectively.
If we consider that the rotation matrix represents a pose then the corresponding
coordinate frame can be displayed graphically
>> trplot(R)

which is shown in Fig. 2.13a. We can visualize a rotation more powerfully using the
Toolbox function tranimate which animates a rotation
>> tranimate(R)
 2.2 · Working in Three Dimensions (3D) 35

Fig. 2.13.
Coordinate frames displayed us-
ing trplot. a Reference frame
rotated by ü about the x-axis,
b frame a rotated by ü about
the y-axis

showing the world frame rotating into the specified coordinate frame. If you have a
pair of anaglyph stereo glasses
you can see this in more realistic 3D by
>> tranimate(R, '3d')

To illustrate compounding of rotations we will rotate the frame of Fig. 2.13a again,
this time around its y-axis
>> R = rotx(pi/2) * roty(pi/2)
R =
0.0000 0 1.0000
1.0000 0.0000 -0.0000
-0.0000 1.0000 0.0000
>> trplot(R)

to give the frame shown in Fig. 2.13b. In this frame the x-axis now points in the direc-
tion of the world y-axis.
The noncommutativity of rotation can be shown by reversing the order of the ro-
tations above
>> roty(pi/2)*rotx(pi/2)
ans =
0.0000 1.0000 0.0000
0 0.0000 -1.0000
-1.0000 0.0000 0.0000
which has a very different value.
We recall that Euler’s rotation theorem states that any rotation can be represented
by not more than three rotations about coordinate axes. This means that in general an
arbitrary rotation between frames can be decomposed into a sequence of three rota-
tion angles and associated rotation axes – this is discussed in the next section.
The orthonormal rotation matrix has nine elements but they are not independent.
The columns have unit magnitude which provides three constraints. The columns are
If the column vectors are ci, i ∈ 1  3 orthogonal to each other which provides another three constraints.
Nine elements
then c1 i c2 = c2 i c3 = c3 i c1 = 0 and and six constraints is effectively three independent values.

ci
= 1.

Reading an orthonormal rotation matrix, the columns from left to right tell us the directions of the
new frame’s axes in terms of the current coordinate frame. For example if
R =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
the new frame has its x-axis in the old x-direction (1, 0, 0), its y-axis in the old z-direction (0, 0, 1),
and the new z-axis in the old negative y-direction (0, −1, 0). In this case the x-axis was unchanged
since this is the axis around which the rotation occurred. The rows are the converse – the current
frame axes in terms of the new frame axes.
36 Chapter 2 · Representing Position and Orientation

l
2.2.1.2 Three-Angle Representations
Euler’s rotation theorem requires successive rotation about three axes such that no
two successive rotations are about the same axis. There are two classes of rotation se-
quence: Eulerian and Cardanian, named after Euler and Cardano respectively.
The Eulerian type involves repetition, but not successive, of rotations about one
particular axis: XYX, XZX, YXY, YZY, ZXZ, or ZYZ. The Cardanian type is character-
ized by rotations about all three axes: XYZ, XZY, YZX, YXZ, ZXY, or ZYX.

It is common practice to refer to all 3-angle representations as Euler angles

but this is underspecified since there are twelve different types to choose
from. The particular angle sequence is often a convention within a particular
technological field.

The ZYZ sequence

(2.14)

is commonly used in aeronautics and mechanical dynamics, and is used in the Toolbox.
The Euler angles are the 3-vector ¡ = (φ, θ, ψ ).
For example, to compute the equivalent rotation matrix for ¡ = (0.1, 0.2, 0.3) we
write
>> R = rotz(0.1) * roty(0.2) * rotz(0.3);

or more conveniently
>> R = eul2r(0.1, 0.2, 0.3)
R =
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
The inverse problem is finding the Euler angles that correspond to a given rota-
tion matrix
>> gamma = tr2eul(R)
gamma =
0.1000 0.2000 0.3000
However if θ is negative
>> R = eul2r(0.1 , -0.2, 0.3)
R =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
the inverse function
>> tr2eul(R)
ans =
-3.0416 0.2000 -2.8416
returns a positive value for θ and quite different values for φ and ψ . However the cor-
responding rotation matrix

Leonhard Euler (1707–1783) was a Swiss mathematician and physicist who dominated eighteenth
century mathematics. He was a student of Johann Bernoulli and applied new mathematical
techniques such as calculus to many problems in mechanics and optics. He also developed the
functional notation, y = f(x), that we use today. In robotics we use his rotation theorem and his
equations of motion in rotational dynamics.
He was prolific and his collected works fill 75 volumes. Almost half of this was produced dur-
ing the last seventeen years of his life when he was completely blind.
 2.2 · Working in Three Dimensions (3D) 37

>> eul2r(ans)
ans =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801

is the same – the two different sets of Euler angles correspond to the one rotation ma-
trix. The mapping from a rotation matrix to Euler angles is not unique and the Toolbox
always returns a positive angle for θ .
For the case where θ = 0
>> R = eul2r(0.1, 0, 0.3)
R =
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
the inverse function returns
>> tr2eul(R)
ans =
0 0 0.4000
which is clearly quite different but the result is the same rotation matrix. The expla-
nation is that if θ = 0 then Ry = I and Eq. 2.14 becomes

which is a function of the sum φ + ψ . Therefore the inverse operation can do no more
than determine this sum, and by convention we choose φ = 0. The case θ = 0 is a sin-
gularity and will be discussed in more detail in the next section.
Another widely used convention are the Cardan angles: roll, pitch and yaw.
Confusingly there are two different versions in common use. Text books seem to de-
fine the roll-pitch-yaw sequence as ZYX or XYZ depending on whether they have a
Well known texts such as Siciliano mobile robot or robot arm focus.
When describing the attitude of vehicles such as
et al. (2008), Spong et al. (2006) and ships, aircraft and cars the convention is that the x-axis points in the forward direc-
Paul (1981) use the XYZ sequence. The
tion and the z-axis points either up or down. It is intuitive to apply the rotations in
Toolbox supports both formats by means
of the 'xyz' and 'zyx' options. the sequence: yaw (direction of travel), pitch (elevation of the front with respect to
The ZYX order is default for Release 10, horizontal) and then finally roll (rotation about the forward axis of the vehicle). This
but for Release 9 the default was XYZ. leads to the ZYX angle sequence

(2.15)

Named after Peter Tait a Scottish physi- Roll-pitch-yaw angles are also known as Tait-Bryan angles
or nautical angles, and
cist and quaternion supporter, and George for aeronautical applications they can be called bank, attitude and heading angles re-
Bryan an early Welsh aerodynamicist.
spectively.

Gerolamo Cardano (1501–1576) was an Italian Renaissance mathematician, physician, astrologer,

and gambler. He was born in Pavia, Italy, the illegitimate child of a mathematically gifted law-
yer. He studied medicine at the University of Padua and later was the first to describe typhoid
fever. He partly supported himself through gambling and his book about games of chance Liber
de ludo aleae contains the first systematic treatment of probability as well as effective cheating
methods. His family life was problematic: his eldest son was executed for poisoning his wife,
and his daughter was a prostitute who died from syphilis (about which he wrote a treatise). He
computed and published the horoscope of Jesus, was accused of heresy, and spent time in prison
until he abjured and gave up his professorship.
He published the solutions to the cubic and quartic equations in his book Ars magna in 1545,
and also invented the combination lock, the gimbal consisting of three concentric rings allow-
ing a compass or gyroscope to rotate freely (see Fig. 2.15), and the Cardan shaft with universal
joints – the drive shaft used in motor vehicles today.
38 Chapter 2 · Representing Position and Orientation

Fig. 2.14.
The Toolbox application
tripleangle allows you to
experiment with Euler angles
and roll-pitch-yaw angles and
see how the attitude of a body
changes

When describing the attitude of a robot gripper, as shown in Fig. 2.16, the conven-
tion is that the z-axis points forward and the x-axis is either up or down. This leads
to the XYZ angle sequence

(2.16)

The Toolbox defaults to the ZYX sequence but can be overridden using the 'xyz'
option. For example
>> R = rpy2r(0.1, 0.2, 0.3)
R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
and the inverse is
>> gamma = tr2rpy(R)
gamma =
0.1000 0.2000 0.3000
The roll-pitch-yaw sequence allows all angles to have arbitrary sign and it has a singularity
when θp = ±ü which is fortunately outside the range of feasible attitudes for most vehicles.
The Toolbox includes an interactive graphical tool
>> tripleangle

that allows you to experiment with Euler angles or roll-pitch-yaw angles and see their ef-
fect on the orientation of a body as shown in Fig. 2.14.

l
2.2.1.3 Singularities and Gimbal Lock
A fundamental problem with all the three-angle representations just described is singu-
larity. This is also known as gimbal lock, a term made famous in the movie Apollo 13.
This occurs when the rotational axis of the middle term in the sequence becomes paral-
lel to the rotation axis of the first or third term.
“The LM Body coordinate system is right-
A mechanical gyroscope used for spacecraft navigation is shown in Fig. 2.15. The handed, with the +X axis pointing up
innermost assembly is the stable member which has three orthogonal gyroscopes that through the thrust axis, the +Y axis
hold it at a constant orientation with respect to the universe. It is mechanically con- pointing right when facing forward
nected to the spacecraft via a gimbal mechanism which allows the spacecraft to move which is along the +Z axis. The rotation-
around the stable platform without exerting any torque on it. The attitude of the space- al transformation matrix is constructed
by a 2-3-1 Euler sequence, that is: Pitch
craft is determined directly by measuring the angles of the gimbal axes with respect to about Y, then Roll about Z and, finally,
the stable platform – giving a direct indication of roll-pitch-yaw angles which in this Yaw about X. Positive rotations are pitch
design are a Cardanian YZX sequence. up, roll right, yaw left.” (Hoag 1963).
 2.2 · Working in Three Dimensions (3D) 39

Fig. 2.15.
Schematic of Apollo Lunar
Module (LM) inertial measure-
ment unit (IMU). The vehicle’s
coordinate system has the x-axis
pointing up through the thrust
axis, the z-axis forward, and the
y-axis pointing right. Starting
at the stable platform {S} and
working outwards toward the
spacecraft’s body frame {B} the
rotation angle sequence is YZX.
The components labeled Xg, Yg
and Zg are the x-, y- and z-axis
gyroscopes and those labeled Xa,
Ya and Za are the x-, y- and z-axis
accelerometers (redrawn after
Apollo Operations Handbook,
LMA790-3-LM)

Consider the situation when the rotation angle of the middle gimbal (rotation about
the spacecraft’s z-axis) is 90° – the axes of the inner and outer gimbals are aligned and
they share the same rotation axis. Instead of the original three rotational axes, since
two are parallel, there are now only two effective rotational axes – we say that one de-
Operationally this was a significant limit- gree of freedom has been lost.

ing factor with this particular gyroscope In mathematical, rather than mechanical, terms this problem can be seen using the
(Hoag 1963) and could have been allevi-
definition of the Lunar module’s coordinate system where the rotation of the space-
ated by adding a fourth gimbal, as was
used on other spacecraft. It was omit- craft’s body-fixed frame {B} with respect to the stable platform frame {S} is
ted on the Lunar Module for reasons of
weight and space.

Rotations obey the cyclic rotation rules For the case when θr = ü we can apply the identity

Rx(ü) Ry(θ ) Rx(ü)T ≡ Rz(θ )
Ry(ü) Rz(θ ) Ry(ü)T ≡ Rx(θ )
Rz(ü) Rx(θ ) Rz(ü)T ≡ Ry(θ )
and anti-cyclic rotation rules
Ry(ü)T Rx(θ ) Ry(ü) ≡ Rz(θ ) leading to
Rz(ü)T Ry(θ ) Rz(ü) ≡ Rx(θ ).

which is unable to represent any rotation about the y-axis. This is not a good thing
because spacecraft rotation about the y-axis would rotate the stable element and thus
ruin its precise alignment with the stars: hence the anxiety on Apollo 13.
The loss of a degree of freedom means that mathematically we cannot invert the
transformation, we can only establish a linear relationship between two of the angles.
In this case the best we can do is determine the sum of the pitch and yaw angles. We
observed a similar phenomena with the Euler angle singularity earlier.
40 Chapter 2 · Representing Position and Orientation

Apollo 13 mission clock: 02 08 12 47

Flight: “Go, Guidance.”

Guido: “He’s getting close to gimbal lock there.”

Flight: “Roger. CapCom, recommend he bring up C3, C4, B3, B4, C1 and C2 thrusters, and ad-
vise he’s getting close to gimbal lock.”

CapCom: “Roger.”

Apollo 13, mission control communications loop (1970) (Lovell and Kluger 1994, p 131; NASA
1970).

All three-angle representations of attitude, whether Eulerian or Cardanian, suf-

fer this problem of gimbal lock when two consecutive axes become aligned. For ZYZ-
Euler angles this occurs when θ = kπ , k ∈ Z and for roll-pitch-yaw angles when pitch
θp = ±(2k + 1)ü. The best that can be hoped for is that the singularity occurs for an
attitude which does not occur during normal operation of the vehicle – it requires ju-
dicious choice of angle sequence and coordinate system.
Singularities are an unfortunate consequence of using a minimal representation.
To eliminate this problem we need to adopt different representations of orientation.
Many in the Apollo LM team would have preferred a four gimbal system and the clue
to success, as we shall see shortly in Sect. 2.2.1.7, is to introduce a fourth parameter.

l
2.2.1.4 Two Vector Representation
For arm-type robots it is useful to consider a coordinate frame {E} attached to the end-effec-
tor as shown in Fig. 2.16. By convention the axis of the tool is associated with the z-axis and
is called the approach vector and denoted $ = (ax, ay, az). For some applications it is more
convenient to specify the approach vector than to specify Euler or roll-pitch-yaw angles.
However specifying the direction of the z-axis is insufficient to describe the coordi-
nate frame – we also need to specify the direction of the x- and y-axes. An orthogonal
vector that provides orientation, perhaps between the two fingers of the robot’s gripper
is called the orientation vector, & = (ox, oy, oz). These two unit vectors are sufficient to
completely define the rotation matrix

(2.17)

since the remaining column, the normal vector, can be computed using Eq. 2.12 as
% = & × $. Consider an example where the gripper’s approach and orientation vec-
tors are parallel to the world x- and y-directions respectively. Using the Toolbox this
is implemented by
>> a = [1 0 0]';
>> o = [0 1 0]';
>> R = oa2r(o, a)
R =
0 0 1
0 1 0
-1 0 0

Any two nonparallel vectors are sufficient to define a coordinate frame. Even if the
two vectors $ and & are not orthogonal they still define a plane and the computed %
is normal to that plane. In this case we need to compute a new value for &′ = $ × %
which lies in the plane but is orthogonal to each of $ and %.
For a camera we might use the optical axis, by convention the z-axis, and the left
side of the camera which is by convention the x-axis. For a mobile robot we might use
 2.2 · Working in Three Dimensions (3D) 41

Fig. 2.16.
Robot end-effector coordinate
system defines the pose in terms
of an approach vector $ and an
orientation vector &, from which
% can be computed. %, & and
$ vectors correspond to the x-,
y- and z-axes respectively of the
end-effector coordinate frame.
(courtesy of Kinova Robotics)

the gravitational acceleration vector (measured with accelerometers) which is by con-

vention the z-axis and the heading direction (measured with an electronic compass)
which is by convention the x-axis.

l
2.2.1.5 Rotation about an Arbitrary Vector
Two coordinate frames of arbitrary orientation are related by a single rotation about
some axis in space. For the example rotation used earlier
>> R = rpy2r(0.1 , 0.2, 0.3);

we can determine such an angle and vector by

>> [theta, v] = tr2angvec(R)
th =
0.3655
v =
0.1886 0.5834 0.7900
This is not unique. A rotation of –theta where theta is the angle of rotation and v is the vector
around which the rotation occurs.
about the vector –v results in the same This information is encoded in the eigenvalues and eigenvectors of R. Using the built-
orientation.
in MATLAB function eig
>> [x,e] = eig(R)
x =
-0.6944 + 0.0000i -0.6944 + 0.0000i 0.1886 + 0.0000i
0.0792 + 0.5688i 0.0792 - 0.5688i 0.5834 + 0.0000i
0.1073 - 0.4200i 0.1073 + 0.4200i 0.7900 + 0.0000i
e =
0.9339 + 0.3574i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.9339 - 0.3574i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i
the eigenvalues are returned on the diagonal of the matrix e and the corresponding
Both matrices are complex, but some eigenvectors are the corresponding columns of x.

elements are real (zero imaginary part). From the definition of eigenvalues and eigenvectors we recall that

where v is the eigenvector corresponding to the eigenvalue λ . For the case λ = 1

which implies that the corresponding eigenvector v is unchanged by the rotation. There
is only one such vector and that is the one about which the rotation occurs. In the exam-
ple the third eigenvalue is equal to one, so the rotation axis is the third column of x.
42 Chapter 2 · Representing Position and Orientation

Olinde Rodrigues (1795–1850) was a French banker and mathematician who wrote extensively on poli-
tics, social reform and banking. He received his doctorate in mathematics in 1816 from the University
of Paris, for work on his first well known formula which is related to Legendre polynomials. His
eponymous rotation formula was published in 1840 and is perhaps the first time the representation
of a rotation as a scalar and a vector was articulated. His formula is sometimes, and inappropriate-
ly, referred to as the Euler-Rodrigues formula. He is buried in the Pere-Lachaise cemetery in Paris.

An orthonormal rotation matrix will always have one real eigenvalue at λ = 1 and
in general a complex pair λ = cos θ ±i sin θ where θ is the rotation angle. The angle
of rotation in this case is
>> theta = angle(e(1,1))
theta =
0.3655
The inverse problem, converting from angle and vector to a rotation matrix, is
achieved using Rodrigues’ rotation formula
It can also be shown that the trace of a
(2.18) rotation matrix tr(R) = 1 + 2cos θ from
which we can compute the magnitude
of θ but not its sign.
where [v̂]× is a skew-symmetric matrix. We can use this formula to determine the ro-
tation of ü about the x-axis
>> R = angvec2r(pi/2, [1 0 0])
R =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
It is interesting to note that this representation of an arbitrary rotation is parameterized
by four numbers: three for the rotation axis, and one for the angle of rotation. This is far
fewer than the nine numbers required by a rotation matrix. However the direction can
be represented by a unit vector which has only two parameters and the angle can be en- Imagine a unit-sphere. All possible unit
vectors from the center can be described
coded in the length to give a 3-parameter representation such as * θ , * sin (θ /2), * tan (θ ) by the latitude and longitude of the
or the Rodrigues’ vector * tan (θ /2). While these forms are minimal and efficient in terms point at which they touch the surface of
of data storage they are analytically problematic and ill-defined when θ = 0. the sphere.

l
2.2.1.6 Matrix Exponentials
Consider an x-axis rotation expressed as a rotation matrix
>> R = rotx(0.3)
R =
1.0000 0 0
0 0.9553 -0.2955
0 0.2955 0.9553
As we did for the 2-dimensional case we can compute the logarithm of this matrix
using the MATLAB builtin function logm logm is different to the builtin function
>> S = logm(R) log which computes the logarithm of
S = each element of the matrix. A logarithm
0 0 0 can be computed using a power series,
0 0.0000 -0.3000 with a matrix rather than scalar argument.
0 0.3000 0.0000 For a matrix the logarithm is not unique
and logm computes the principal loga-
and the result is a sparse matrix with two elements that have a magnitude of 0.3, which rithm of the matrix.
is the original rotation angle. This matrix has a zero diagonal and is another example
of a skew-symmetric matrix, in this case 3 × 3.
Applying vex to the skew-symmetric matrix gives
>> vex(S)'
ans =
0.3000 0 0
 2.2 · Working in Three Dimensions (3D) 43

and we find the original rotation angle is in the first element, corresponding to the
x-axis about which the rotation occurred. For the 3-dimensional case the Toolbox
trlog uses a more efficient closed- function trlog is equivalent

form solution as well as being able to
return the angle and axis information >> [th,w] = trlog(R)
separately. th =
0.3000
w =
1.0000
0
0

The inverse of a logarithm is exponentiation and applying the builtin MATLAB

expm is different to the builtin function matrix exponential function expm

exp which computes the exponential
>> expm(S)
of each element of the matrix:
ans =
expm(A) = I + A + A2 / 2! + A3 / 3! + 
1.0000 0 0
0 0.9553 -0.2955
0 0.2955 0.9553
we have regenerated our original rotation matrix. In fact the command
>> R = rotx(0.3);

is equivalent to
>> R = expm( skew([1 0 0]) * 0.3 );

where we have specified the rotation in terms of a rotation angle and a rotation axis
(as a unit-vector). This generalizes to rotation about any axis and formally we can
write

where θ is the rotation angle, ë is a unit-vector parallel to the rotation axis, and the
notation [·]×: R3  R3×3 indicates a mapping from a vector to a skew-symmetric
matrix. Since [ω]×θ = [ωθ ]× we can treat ωθ ∈ R3 as a rotational parameter called
exponential coordinates. For the 3-dimensional case, Rodrigues’ rotation formula
(Eq. 2.18) is a computationally efficient means of computing the matrix exponen-
tial for the special case where the argument is a skew-symmetric matrix, and this
is used by the Toolbox function trexp which is equivalent to expm.

In 3-dimensions the skew-symmetric matrix has the form

(2.19)

which has clear structure and only three unique elements ω ∈ R3. The matrix can be used to
implement the vector cross product v1 × v2 = [v1]× v2. A simple example of Toolbox support for
skew-symmetric matrices is
>> skew([1 2 3])
ans =
0 -3 2
3 0 -1
-2 1 0
and the inverse operation is performed using the Toolbox function vex
>> vex(ans)'
ans =
1 2 3
Both functions work for the 3D case, shown here, and the 2D case where the vector is a 1-vector.
44 Chapter 2 · Representing Position and Orientation

l
2.2.1.7 Unit Quaternions
Quaternions came from Hamilton after his really good work had been done;
and, though beautifully ingenious, have been an unmixed evil to those
who have touched them in any way, including Clark Maxwell.
Lord Kelvin, 1892

Quaternions were discovered by Sir William Hamilton over 150 years ago and, while
initially controversial, have great utility for robotics. The quaternion is an extension of
the complex number – a hypercomplex number – and is written as a scalar plus a vector

(2.20)

where s ∈ R, v ∈ R3 and the orthogonal complex numbers i, j and k are defined such that

(2.21)

and we denote a quaternion as

In the Toolbox quaternions are implemented by the Quaternion class. Quaternions

support addition and subtraction, performed element-wise, multiplication by a scalar
and multiplication

which is known as the quaternion or Hamilton product. If we write the quaternion as a 4-vector
One early objection to quaternions was that multiplication was not commutative (s, v1, v2, v2) then multiplication can be
expressed as a matrix-vector product
but as we have seen above this is exactly what we require for rotations. Despite the
where
initial controversy quaternions are elegant, powerful and computationally straight-
forward and they are widely used for robotics, computer vision, computer graphics
and aerospace navigation systems.
To represent rotations we use unit-quaternions denoted by f. These are quaterni-
ons of unit magnitude; that is, those for which
q
= s 2 + v 12 + v 22 + v 32 = 1. They can
be considered as a rotation of θ about the unit vector * which are related to the qua-
ternion components by As for the angle-vector representation
this is not unique. A rotation of θ about
the vector –ν results in the same orienta-
Sir William Rowan Hamilton (1805–1865) was an Irish mathematician, physicist, and astronomer. tion. This is referred to as a double map-
He was a child prodigy with a gift for languages and by age thirteen knew classical and mod- ping or double cover.
ern European languages as well as Persian, Arabic, Hindustani, Sanskrit, and Malay. Hamilton
taught himself mathematics at age 17, and discovered an error in Laplace’s Celestial Mechanics.
He spent his life at Trinity College, Dublin, and was appointed Professor of Astronomy and Royal
Astronomer of Ireland while still an undergraduate. In addition to quaternions he contributed to
the development of optics, dynamics, and algebra. He also wrote poetry and corresponded with
Wordsworth who advised him to devote his energy to mathematics.
According to legend the key quaternion equation, Eq. 2.21, occured to Hamilton in 1843 while
walking along the Royal Canal in Dublin with his wife, and this is commemorated by a plaque
on Broome bridge:

Here as he walked by on the 16th of October 1843 Sir William Rowan Hamilton in a flash of ge-
nius discovered the fundamental formula for quaternion multiplication i2 = j2 = k2 = i j k = −1
& cut it on a stone of this bridge.

His original carving is no longer visible, but the bridge is a pilgrimage site for mathemati-
cians and physicists.
 2.2 · Working in Three Dimensions (3D) 45

For the case of unit quaternions our generalized pose is a rotation ξ ∼ h ∈ S3 and

and

which is the quaternion conjugate. The zero rotation 0  1 <0, 0, 0> which is
the identity quaternion. A vector v ∈ R3 is rotated by

where È = 0 <v> is known as a pure quaternion.

(2.22)

and has similarities to the angle-axis representation of Sect. 2.2.1.5.

In the Toolbox these are implemented by the UnitQuaternion class and the
constructor converts a passed argument such as a rotation matrix to a unit quater-
nion, for example
>> q = UnitQuaternion( rpy2tr(0.1, 0.2, 0.3) )
q =
0.98335 < 0.034271, 0.10602, 0.14357 >
This class overloads a number of standard methods and functions. Quaternion mul-
Compounding two orthonormal rota- tiplication
is invoked through the overloaded multiplication operator
tion matrices requires 27 multiplications
and 18 additions. The quaternion form >> q = q * q;
requires 16 multiplications and 12 ad-
ditions. This saving can be particularly and inversion, the conjugate of a unit quaternion, is
important for embedded systems. >> inv(q)
ans =
0.93394 < -0.0674, -0.20851, -0.28236 >
Multiplying a quaternion by its inverse yields the identity quaternion
>> q*inv(q)
ans =
1 < 0, 0, 0 >
which represents a null rotation, or more succinctly
>> q/q
ans =
1 < 0, 0, 0 >
The quaternion can be converted to an orthonormal rotation matrix by
>> q.R
ans =
0.7536 -0.4993 0.4275
0.5555 0.8315 -0.0081
-0.3514 0.2436 0.9040
and we can also plot the orientation represented by a quaternion
>> q.plot()

which produces a result similar in style to that shown in Fig. 2.13. A vector is rotated
by a quaternion using the overloaded multiplication operator
>> q*[1 0 0]'
ans =
0.7536
0.5555
-0.3514
46 Chapter 2 · Representing Position and Orientation

The Toolbox implementation is quite complete and the UnitQuaternion class has
many methods and properties which are described fully in the online documentation.

l
2.2.2 Pose in 3-Dimensions
We return now to representing relative pose in three dimensions – the position and
orientation change between the two coordinate frames as shown in Fig. 2.10. This is
often referred to as a rigid-body displacement or rigid-body motion.
We have discussed several different representations of orientation, and we need to com-
bine one of these with translation, to create a tangible representation of relative pose.

l
2.2.2.1 Homogeneous Transformation Matrix
The derivation for the homogeneous transformation matrix is similar to the 2D case
of Eq. 2.11 but extended to account for the z-dimension. t ∈ R3 is a vector defining the
origin of frame {B} with respect to frame {A}, and R is the 3 × 3 orthonormal matrix
which describes the orientation of the axes of frame {B} with respect to frame {A}.

If points are represented by homogeneous coordinate vectors then

(2.23)

and ATB is a 4 × 4 homogeneous transformation matrix. This matrix has a very specific struc-
ture and belongs to the special Euclidean group of dimension 3 or T ∈ SE(3) ⊂ R4×4.

A concrete representation of relative pose is ξ ∼ T ∈ SE(3) and T1 ⊕ T2  T1T2

which is standard matrix multiplication.

(2.24)

One of the rules of pose algebra from page 21 is ξ ⊕ 0 = ξ. For matrices we

know that TI = T, where I is the identify matrix, so for pose 0  I the identity
matrix. Another rule of pose algebra was that ξ
ξ = 0. We know for matrices
that TT −1 = I which implies that
T  T −1

(2.25)

The 4 × 4 homogeneous transformation is very commonly used in robotics, com-

puter graphics and computer vision. It is supported by the Toolbox and will be used
throughout this book as a concrete representation of 3-dimensional pose.
The Toolbox has many functions to create homogeneous transformations. For ex-
ample we can demonstrate composition of transforms by
 2.2 · Working in Three Dimensions (3D) 47

>> T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)

T =
1.0000 0 0 1.0000
0 0.0000 -1.0000 0.0000
0 1.0000 0.0000 1.0000
0 0 0 1.0000
The function transl creates a relative pose with a finite translation but no rotation,
while trotx creates a relative pose corresponding to a rotation of ü about the x-axis
Many Toolbox functions have variants that with zero translation.
We can think of this expression as representing a walk along
return orthonormal rotation matrices or the x-axis for 1 unit, then a rotation by 90° about the x-axis and then a walk of 1 unit
homogeneous transformations, for exam- along the new y-axis which was the previous z-axis. The result, as shown in the last
ple, rotx and trotx, rpy2r and
rpy2tr etc. Some Toolbox functions
column of the resulting matrix is a translation of 1 unit along the original x-axis and
accept an orthonormal rotation matrix 1 unit along the original z-axis. The orientation of the final pose shows the effect of the
or a homogeneous transformation and rotation about the x-axis. We can plot the corresponding coordinate frame by
ignore the translational component, for >> trplot(T)
example, tr2rpy.
The rotation matrix component of T is
>> t2r(T)
ans =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
and the translation component is a column vector
>> transl(T)'
ans =
1.0000 0.0000 1.0000

l
2.2.2.2 Vector-Quaternion Pair
A compact and practical representation is the vector and unit quaternion pair. It rep-
This representation is not implemented resents pose using just 7 numbers, is easy to compound, and singularity free.

in the Toolbox.

For the vector-quaternion case ξ ∼ (t, h) where t ∈ R3 is a vector defining the

frame’s origin with respect to the reference coordinate frame, and h ∈ S3 is the
frame’s orientation with respect to the reference frame.
Composition is defined by

and negation is

and a point coordinate vector is transformed to a coordinate frame by

l
2.2.2.3 Twists
In Sect. 2.1.2.3 we introduced twists for the 2D case. Any rigid-body motion in 3D space
Pure translation can be considered as ro- is equivalent to a screw motion – motion about and along some line in space.
We rep-
tation about a point at infinity. resent a screw as a pair of 3-vectors s = (v, ω) ∈ R6.
The ω component of the twist vector is the direction of the screw axis. The v com-
ponent is called the moment and encodes the position of the line of the twist axis in
space and also the pitch of the screw. The pitch is the ratio of the distance along the
screw axis to the rotation about the screw axis.
48 Chapter 2 · Representing Position and Orientation

Consider the example of a rotation of 0.3 radians about the x-axis. We first specify
a unit twist with an axis that is parallel to the x-axis and passes through the origin A rotational unit twist has
ω
= 1.
>> tw = Twist('R', [1 0 0], [0 0 0])
tw =
( -0 -0 -0; 1 0 0 )
which we convert, for the required rotation angle, to an SE(3)-homogeneous trans-
formation
>> tw.T(0.3)
ans =
1.0000 0 0 0
0 0.9553 -0.2955 0
0 0.2955 0.9553 0
0 0 0 1.0000
and has the same value we would obtain using trotx(0.3).
For pure translation in the y-direction the unit twist would be A translational unit twist has
v
= 1
and ω = 0.
>> tw = Twist('T', [0 1 0])
tw =
( 0 1 0; 0 0 0 )
which we convert, for the required translation distance, to an SE(3)-homogeneous
transformation.
>> tw.T(2)
ans =
1 0 0 0
0 1 0 2
0 0 1 0
0 0 0 1
which is, as expected, an identity matrix rotational component (no rotation) and a
translational component of 2 in the y-direction.
To illustrate the underlying screw model we define a coordinate frame {X}
>> X = transl(3, 4, -4);
which we will rotate by a range of angles
>> angles = [0:0.3:15];
around a screw axis parallel to the z-axis, direction (0, 0, 1), through the point (2, 3, 2)
and with a pitch of 0.5
>> tw = Twist('R', [0 0 1], [2 3 2], 0.5);
The next line packs a lot of functionality. For values of θ drawn successively from the vector
angles we use an anonymous function to evaluate the twist for each value of θ and apply
it to the frame {X}. This sequence is animated and each frame in the sequence is retained
>> tranimate( @(theta) tw.T(theta) * X, angles, ...
'length', 0.5, 'retain', 'rgb', 'notext');
and the result is shown in Fig. 2.17. We can clearly see the screw motion in the successive
poses of the displaced reference frame as it is rotated about the screw axis.
The screw axis is the line
>> L = tw.line
L =
{ 3 -2 0; 0 0 1 }
which is described in terms of its Plücker coordinates which we can plot
>> L.plot('k:', 'LineWidth', 2)
Finally we can convert an arbitrary homogeneous transformation to a nonunit twist
>> T = transl(1, 2, 3) * eul2tr(0.3, 0.4, 0.5);
>> tw = Twist(T)
tw =
( 1.1204 1.6446 3.1778; 0.041006 0.4087 0.78907 )
 2.3 · Advanced Topics 49

Fig. 2.17.
A coordinate frame {X} dis-
played for different values of θ
about a screw parallel to the
z-axis and passing through the
point (2, 3, 2). The x-, y- and
z-axes are indicated by red,
green and blue lines respectively

which has a pitch of

>> tw.pitch
ans =
3.2256
and the rotation about the axis is
>> tw.theta
ans =
0.8896
and a point lying on the twist axis is
>> tw.pole'
ans =
0.0011 0.8473 -0.4389

l
2.3 Advanced Topics
l
2.3.1 Normalization

The IEEE standard for double precision Floating-point arithmetic has finite precision
and consecutive operations will accu-
floating point, the standard MATLAB mulate error. A rotation matrix has by definition, a determinant of one
numeric format, has around 16 decimal
digits of precision. >> R = eye(3,3);
>> det(R) - 1
ans =
0
but if we repeatedly multiply by a valid rotation matrix the result
>> for i=1:100
R = R * rpy2r(0.2, 0.3, 0.4);
end
>> det(R) - 1
ans =
4.4409e-15

indicates a small error – the determinant is no longer equal to one and the matrix is
no longer a proper orthonormal rotation matrix. To fix this we need to normalize the
matrix, a process which enforces the constraints on the columns ci of an orthonormal
matrix R = [c1, c2, c3]. We need to assume that one column has the correct direction

then the first column is made orthogonal to the last two

50 Chapter 2 · Representing Position and Orientation

However the last two columns may not have been orthogonal so

Finally the columns are all normalized to unit magnitude

In the Toolbox normalization is implemented by

>> R = trnorm(R);

and the determinant is now much closer to one This error is now at the limit of double pre-
>> det(R) - 1 cision arithmetic which is 2.2204 ×10−16
ans = and given by the MATLAB function eps.
-2.2204e-16
A similar issue arises for unit quaternions when the norm, or magnitude, of the unit
quaternion is no longer equal to one. However this is much easier to fix since normal-
izing the quaternion simply involves dividing all elements by the norm

which is implemented by the unit method

>> q = q.unit();

The UnitQuaternion class also supports a variant of multiplication

>> q = q .* q2;

which performs an explicit normalization after the multiplication.

Normalization does not need to be done after every multiplication since it is an
expensive operation. However for situations like the example above where one trans-
form is being repeatedly updated it is advisable.

l
2.3.2 Understanding the Exponential Mapping
In this chapter we have glimpsed some connection between rotation matrices, skew-
symmetric matrices and matrix exponentiation. The basis for this lies in the mathemat-
ics of Lie groups which are covered in text books on algebraic geometry and algebraic
topology. These require substantial knowledge of advanced mathematics and many
people starting out in robotics will find their content quite inaccessible. An introduc-
tion to the essentials of this topic is given in Appendix D. In this section we will use an
intuitive approach, based on undergraduate engineering mathematics, to shed some
light on these relationships.
Consider a point P, defined by a coordinate vector p, being rotated with an angular
velocity ω which is a vector whose direction defines the axis of rotation and whose
magnitude
ω
specifies the rate of rotation about the axis which we assume passes
through the origin. We wish to rotate the point by an angle θ about this axis and the Angular velocity will be properly intro-
velocity of the point is known from mechanics to be duced in the next chapter.

and we replace the cross product with a skew-symmetric matrix giving a matrix-vec-
tor product
 2.3 · Advanced Topics 51

(2.26)

We can find the solution to this first-order differential equation by analogy to the
simple scalar case

whose solution is

This implies that the solution to Eq. 2.26 is

If
ω
= 1 then after t seconds the vector will have rotated by t radians. We require
a rotation by θ so we can set t = θ to give

which describes the vector p(0) being rotated to p(θ ). A matrix that rotates a vec-
tor is a rotation matrix, and this implies that our matrix exponential is a rotation
matrix

Now consider the more general case of rotational and translational motion. We
can write

and rearranging into matrix form

and introducing homogeneous coordinates this becomes

where Σ is a 4 × 4 augmented skew-symmetric matrix. Again, by analogy with the

scalar case we can write the solution as

A matrix that rotates and translates a point in homogeneous coordinates is a ho-

mogeneous transformation matrix, and this implies that our matrix exponential is a
homogeneous transformation matrix

where [ë]× θ defines the magnitude and axis of rotation and v θ is the translation.
52 Chapter 2 · Representing Position and Orientation

The exponential of a scalar can be computed using a power series, and the matrix
case is analogous and relatively straightforward to compute. The MATLAB function
expm uses a polynomial approximation for the general matrix case. If A is skew-sym-
metric or augmented-skew-symmetric then an efficient closed-form solution for a
rotation matrix – the Rodrigues’ rotation formula (Eq. 2.18) – can be used and this is
implemented by the Toolbox function trexp.

l
2.3.3 More About Twists
In this chapter we introduced and applied twists and here we will more formally define
them. We also highlight the very close relationship between twists and homogeneous
transformation matrices via the exponential mapping.
The key concept comes from Chasle’s theorem: “any displacement of a body in space
can be accomplished by means of a rotation of the body about a unique line in space
accompanied by a translation of the body parallel to that line”. Such a line is called a
screw axis and is illustrated in Fig. 2.18. The mathematics of screw theory was devel-
oped by Sir Robert Ball in the late 19th century for the analysis of mechanisms. At the
core of screw theory are pairs of vectors: angular and linear velocity; forces and mo-
ments; and Plücker coordinates (see Sect. C.1.2.2).
The general displacement of a rigid body in 3D can be represented by a twist vector

where v ∈ R3 is referred to as the moment and encodes the position of the action line
in space and the pitch of the screw and ω ∈ R3 is the direction of the screw axis.
For rotational motion where the screw axis is parallel to the vector $, passes
through a point Q defined by its coordinate vector q, and the screw pitch p is the
ratio of the distance along the screw axis to the rotation about the axis, the twist
elements are

and the pitch can be recovered by

For the case of pure rotation the pitch of the screw is zero and the unit twist is

Fig. 2.18.
Conceptual depiction of a screw.
A coordinate frame is attached
to a nut by a rigid rod and rotat-
ed around the screw thread. The
pose changes from the red frame
to the blue frame. The corollary
is that given any two frames we
can determine a screw axis to
rotate one into the other
 2.3 · Advanced Topics 53

Michel Chasles (1793–1880) was a French mathematician born at Épernon. He studied at the
École Polytechnique in Paris under Poisson and in 1814 was drafted to defend Paris in the War
of the Sixth Coalition. In 1837 he published a work on the origin and development of methods in
geometry, which gained him considerable fame and he was appointed as professor at the École
Polytechnique in 1841, and at the Sorbonne in 1846.
He was an avid collector and purchased over 27 000 forged letters purporting to be from Newton,
Pascal and other historical figures – all written in French! One from Pascal claimed he had dis-
covered the laws of gravity before Newton, and in 1867 Chasles took this to the French Academy
of Science but scholars recognized the fraud. Eventually Chasles admitted he had been deceived
and revealed he had spent nearly 150 000 francs on the letters. He is buried in Cimetière du Père
Lachaise in Paris.

For purely translational motion in the direction parallel to the vector a, the pitch
is infinite which leads to a zero rotational component and the unit twist is

A twist is related to the rigid-body displacement in SE(3) by the exponential map-

ping already discussed.

where the augmented skew-symmetric matrix

belongs to the Lie algebra se(3) and is the generator of the rigid-body displacement.
The matrix exponential has an efficient closed-form

where R(θ , ë) is computed using Rodrigues’ rotation formula (Eq. 2.18). For a nonunit
rotational twist, that is
ω
≠ 1, then θ =
ω
.

For real numbers, if x = log X and y = log Y then

but for the matrix case this is only true if the matrices commute, and rotation
matrices do not, therefore

The bottom line is that there is no shortcut to compounding rotations, we

must compute z = log (e x e y ) not z = x + y.

The Toolbox provides many ways to create twists and to convert them to rigid-body
displacements expressed as homogeneous transformations. Now that we understand
more about the exponential mapping we will revisit the example from page 48
>> tw = Twist('R', [1 0 0], [0 0 0])
tw =
( -0 -0 -0; 1 0 0 )
54 Chapter 2 · Representing Position and Orientation

A unit twist describes a family of motions that have a single parameter, either
a rotation and translation about and along some screw axis, or a pure transla-
tion in some direction. We can visualize it as a mechanical screw in space, or
represent it as a 6-vector S = (v, ω ) where
ω
= 1 for a rotational twist and

v
= 1, ω = 0 for a translational twist.
A particular rigid-body motion is described by a unit-twist s and a motion
parameter θ which is a scalar specifying the amount of rotation or translation.
The motion is described by the twist Sθ which is in general not a unit-twist. The
exponential of this in 4 × 4 matrix format is the 4 × 4 homogeneous transforma-
tion matrix describing that particular rigid-body motion in SE(3).

which is a unit twist that describes rotation about the x-axis in SE(3). The Twist has
a number of properties
>> tw.S'
ans =
0 0 0 1 0 0
>> tw.v'
ans =
0 0 0
>> tw.w'
ans =
1 0 0
as well as various methods. We can create the se(3) Lie algebra using the se method
of this class
>> tw.se
ans =
0 0 0 0
0 0 -1 0
0 1 0 0
0 0 0 0

which is the augmented skew-symmetric version of S. The method T performs the

exponentiation of this to create an SE(3) homogeneous transformation for the speci- The expm method is synonomous and
fied rotation about the unit twist both invoke the Toolbox function trexp.

>> tw.T(0.3)
ans =
1.0000 0 0 0
0 0.9553 -0.2955 0
0 0.2955 0.9553 0
0 0 0 1.0000

The Toolbox functions trexp and trlog are respectively closed-form alternatives
to expm and logm when the arguments are in so(3)/se(3) or SO(3)/SE(3).
The line method returns a Plucker object that represents the line of the screw
in Plücker coordinates
>> tw.line
ans =
{ 0 0 0; 1 0 0 }
Finally, the overloaded multiplication operator for the Twist class will compound
two twists.
>> t2 = tw * tw
t2 =
( -0 -0 -0; 2 0 0 )
>> tr2angvec(t2.T)
Rotation: 2.000000 rad x [1.000000 0.000000 0.000000]
and the result in this case is a nonunit twist of two units, or 2 rad, about the x-axis.
 2.3 · Advanced Topics 55

l
2.3.4 Dual Quaternions
Quaternions were developed by William Hamilton in 1843 and we have already seen
their utility for representing orientation, but using them to represent pose proved
more difficult. One early approach was Hamilton’s bi-quaternion where the quaternion
1845–1879, an English mathematician coefficients were complex numbers. Somewhat later William Clifford
developed the
and geometer. dual number, defined as an ordered pair d = (x, y) which can be written as d = x + yε
where ε 2 = 0 and for which specific addition and multiplication rules exist. Clifford
created a quaternion dual number with x, y ∈ H which he also called a bi-quaternion
but is today called a dual quaternion

where ñ ∈ H is a unit quaternion representing the rotational part of the pose and
ò ∈ H is a pure quaternion representing translation. This type of mathematical ob-
ject has been largely eclipsed by modern matrix and vector approaches, but there
seems to be a recent resurgence of interest in alternative approaches. The dual qua-
ternion is quite compact, requiring just 8 numbers; it is easy to compound using a
special multiplication table; and it is easy to renormalize to eliminate the effect of
imprecise arithmetic. However it has no real useful computational advantage over
matrix methods.

l
2.3.5 Configuration Space
We have so far considered the pose of objects in terms of the position and orientation
of a coordinate frame affixed to them. For an arm-type robot we might affix a coor-
dinate frame to its end-effector, while for a mobile robot we might affix a frame to its
body – its body-fixed frame. This is sufficient to describe the state of the robot in the
familiar 2D or 3D Euclidean space which is referred to as the task space or operational
space since it is where the robot performs tasks or operates.
An alternative way of thinking about this comes from classical mechanics and is
referred to as the configuration of a system. The configuration is the smallest set of
parameters, called generalized coordinates, that are required to fully describe the po-
sition of every particle in the system. This is not as daunting as it may appear since in
general a robot comprises one or more rigid elements, and in each of these the par-
ticles maintain a constant relative offset to each other.
If the system is a train moving along a track then all the particles comprising the
train move together and we need only a single generalized coordinate q, the distance
along the track from some datum, to describe their location. A robot arm with a
fixed base and two rigid links, connected by two rotational joints has a configuration
that is completely described by two generalized coordinates – the two joint angles
(q1, q2). The generalized coordinates can, as their name implies, represent displace-
ments or rotations.

Sir Robert Ball (1840–1913) was an Irish astronomer born in Dublin. He became Professor of
Applied Mathematics at the Royal College of Science in Dublin in 1867, and in 1874 became Royal
Astronomer of Ireland and Andrews Professor of Astronomy at the University of Dublin. In 1892
he was appointed Lowndean Professor of Astronomy and Geometry at Cambridge University and
became director of the Cambridge Observatory. He was a Fellow of the Royal Society and in 1900
became the first president of the Quaternion Society.
He is best known for his contributions to the science of kinematics described in his treatise
“The Theory of Screws” (1876), but he also published “A Treatise on Spherical Astronomy” (1908)
and a number of popular articles on astronomy. He is buried at the Parish of the Ascension Burial
Ground in Cambridge.
56 Chapter 2 · Representing Position and Orientation

The number of independent generalized coordinates N is known as the number That is, there are no holonomic constraints
of degrees of freedom of the system. Any configuration of the system is represented on the system.
by a point in its N-dimensional configuration space, or C-space, denoted by C and
q ∈ C. We can also say that dim C = N. For the train example C ⊂ R which says that
the displacement is a bounded real number. For the 2-joint robot the generalized co-
ordinates are both angles so C ⊂ S1 × S1.
Any point in the configuration space can be mapped to a point in the task space
q ∈ C  τ ∈ T but the inverse is not necessarily true. This mapping depends on the
task space that we choose and this, as its name suggests, is task specific.
Consider again the train moving along its rail. We might be interested to describe the
train in terms of its position on a plane in which case the task space would be T ⊂ R2, or
in terms of its latitude and longitude, in which case the task space would be T ⊂ S1 × S1.
We might choose a 3-dimensional task space T ⊂ SE(3) to account for height changes
as the train moves up and down hills and its orientation changes as it moves around
curves. However in all these case the dimension of the task space exceeds the dimension
of the configuration space dim T > dim C and this means that the train cannot access
all points in the task space. While every point along the rail line can be mapped to the
task space, most points in the task space will not map to a point on the rail line. The
train is constrained by its fixed rails to move in a subset of the task space.
The simple 2-joint robot arm can access a subset of points in a plane so a useful
task space might be T ⊂ R2. The dimension of the task space equals the dimension of
the configuration space dim T = dim C and this means that the mapping between task
and configuration spaces is bi-directional but it is not necessarily unique – for this type
of robot, in general, two different configurations map to a single point in task space.
Points in the task space beyond the physical reach of the robot are not mapped to the
configuration space. If we chose a task space with more dimensions such as SE(2) or
SE(3) then dim T > dim C and the robot would only be able to access points within a
subset of that space.
Now consider a snake-robot arm, such as shown in Fig. 8.9, with 20 joints and
C ⊂ S1 ×  × S1 and dim T < dim C. In this case an infinite number of configurations
in a 20 − 6 = 14-dimensional subspace of the 20-dimensional configuration space will
map to the same point in task space. This means that in addition to the task of position-
ing the robot’s end-effector we can simultaneously perform motion in the configura-
tion subspace to control the shape of the arm to avoid obstacles in the environment.
Such a robot is referred to as over-actuated or redundant and this topic is covered in
Sect. 8.4.2.
The body of a quadrotor, such as shown in Fig. 4.19d, is a single rigid-body whose
configuration is completely described by six generalized coordinates, its position and
orientation in 3D space C ⊂ R3 × S1 × S1 × S1 where the orientation is expressed in
some three-angle representation. For such a robot the most logical task space would
be SE(3) which is equivalent to the configuration space and dim T = dim C. However
the quadrotor has only four actuators which means it cannot directly access all the
points in its configuration space and hence its task space. Such a robot is referred to
as under-actuated and we will revisit this in Sect. 4.2.

l
2.4 Using the Toolbox
The Toolbox supports all the different representations discussed in this chapter as
well as conversions between many of them. The representations and possible con-
versions are shown in tabular form in Tables 2.1 and 2.2 for the 2D and 3D cases
respectively.
Quaternions and twists are implemented
In this chapter we have mostly used native MATLAB matrices to represent rotations as classes not native types, but in very old
and homogeneous transformations and historically this has been what the Toolbox versions of the Toolbox quaternions were
supported – the Toolbox classic functions. From Toolbox release 10 there are classes that 1 × 4 vectors.
 2.4 · Using the Toolbox 57

represent rotations and homogeneous transformations, named respectively SO2 and

SE2 for 2 dimensions and SO3 and SE3 for 3 dimensions. These provide real advan-
tages in terms of code readability and type safety and can be used in an almost identical
fashion to the native matrix types. They are also polymorphic meaning they support
many of the same operations which makes it very easy to switch between using say ro-
tation matrices and quaternions or lifting a solution from 2- to 3-dimensions. A quick
illustration of the new functionality is the example from page 27 which becomes
>> T1 = SE2(1, 2, 30, 'deg');
>> about T1
The size of the object in bytes, shown in
T1 [SE2] : 1x1 (176 bytes)
parentheses, will vary between MATLAB.
versions and computer types. which results in an SE2 class object not a 3 × 3 matrix.
If we display it however it
does look like a 3 × 3 matrix
If you have the cprintf package from >> T1
MATLAB File Exchange installed then the T1 =
rotation submatrix will be colored red. 0.8660 -0.5000 1
0.5000 0.8660 2
0 0 1
The matrix is encapsulated within the object and we can extract it readily if required
>> T1.T
ans =
0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000
>> about ans
ans [double] : 3x3 (72 bytes)

Returning to that earlier example we can quite simply transform the vector
>> inv(T1) * P
ans =
1.7321
-1.0000
and the class handles the details of converting the vector between Euclidean and ho-
mogeneous forms.
Table 2.1. Toolbox supported data
This new functionality is also covered in Tables 2.1 and 2.2, and Table 2.3 is a map
types for representing 2D pose: between the classic and new functionality to assist you in using the Toolbox. From here
constructors and conversions on the book will use a mixture of classic functions and the newer classes.
58 Chapter 2 · Representing Position and Orientation

Table 2.2. Toolbox supported data

l types for representing 3D pose:
2.5 Wrapping Up constructors and conversions
In this chapter we learned how to represent points and poses in 2- and 3-dimensional
worlds. Points are represented by coordinate vectors relative to a coordinate frame.
A set of points that belong to a rigid object can be described by a coordinate frame,
and its constituent points are described by constant vectors in the object’s coordinate
frame. The position and orientation of any coordinate frame can be described relative
to another coordinate frame by its relative pose ξ . We can think of a relative pose as a
motion – a rigid-body motion – and these motions can be applied sequentially (com-
posed or compounded). It is important to remember that composition is noncommu-
tative – the order in which relative poses are applied is important.
We have shown how relative poses can be expressed as a pose graph or manipulated
algebraically. We can also use a relative pose to transform a vector from one coordinate
frame to another. A simple graphical summary of key concepts is given in Fig. 2.19.
We have discussed a variety of mathematical objects to tangibly represent pose. We
have used orthonormal rotation matrices for the 2- and 3-dimensional case to repre-
sent orientation and shown how it can rotate a points’ coordinate vector from one co-
ordinate frame to another. Its extension, the homogeneous transformation matrix, can
be used to represent both orientation and translation and we have shown how it can
rotate and translate a point expressed in homogeneous coordinates from one frame
 2.5 · Wrapping Up 59

Table 2.3. Table of subsitutions to another. Rotation in 3-dimensions has subtlety and complexity and we have looked
from classic Toolbox functions at various parameterizations such as Euler angles, roll-pitch-yaw angles and unit qua-
that operate on and return a ma-
ternions. Using Lie group theory we showed that rotation matrices, from the group
trix, to the corresponding new
classes and methods SO(2) or SO(3), are the result of exponentiating skew-symmetric generator matrices.
Similarly, homogeneous transformation matrices, from the group SE(2) or SE(3), are
the result of exponentiating augmented skew-symmetric generator matrices. We have
also introduced twists as a concise way of describing relative pose in terms of rotation
around a screw axis, a notion that comes to us from screw theory and these twists are
the unique elements of the generator matrices.
There are two important lessons from this chapter. The first is that there are
many mathematical objects that can be used to represent pose and these are sum-
marized in Table 2.4. There is no right or wrong – each has strengths and weak-
nesses and we typically choose the representation to suit the problem at hand.
Sometimes we wish for a vectorial representation, perhaps for interpolation, in
which case (x, y, θ ) or (x, y, z, ¡ ) might be appropriate, but this representation can-
not be easily compounded. Sometime we may only need to describe 3D rotation
in which case Γ or h is appropriate. Converting between representations is easy as
shown in Tables 2.1 and 2.2.
The second lesson is that coordinate frames are your friend. The essential first step
in many vision and robotics problems is to assign coordinate frames to all objects of
interest, indicate the relative poses as a directed graph, and write down equations for
the loops. Figure 2.20 shows you how to build a coordinate frame out of paper that
you can pick up and rotate – making these ideas more tangible. Don’t be shy, embrace
the coordinate frame.
We now have solid foundations for moving forward. The notation has been defined
and illustrated, and we have started our hands-on work with MATLAB. The next chap-
ter discusses motion and coordinate frames that change with time, and after that we
are ready to move on and discuss robots.
60 Chapter 2 · Representing Position and Orientation

Fig. 2.19.
Everything you need to know
about pose

Table 2.4. Summary of the various

concrete representations of pose ξ
Further Reading introduced in this chapter

The treatment in this chapter is a hybrid mathematical and graphical approach that
covers the 2D and 3D cases by means of abstract representations and operators which
are later made tangible. The standard robotics textbooks such as Kelly (2013), Siciliano
et al. (2009), Spong et al. (2006), Craig (2005), and Paul (1981) all introduce homoge-
neous transformation matrices for the 3-dimensional case but differ in their approach.
These books also provide good discussion of the other representations such as angle-
vector and 3-angle representations. Spong et al. (2006, sect. 2.5.1) have a good discus-
sion of singularities. The book Lynch and Park (2017) covers the standard matrix ap-
proaches but also introduces twists and screws. Siegwart et al. (2011) explicitly cover
the 2D case in the context of mobile robotics.
Quaternions are discussed in Kelly (2013) and briefly in Siciliano et al. (2009). The
book by Kuipers (1999) is a very readable and comprehensive introduction to quater-
nions. Quaternion interpolation is widely used in computer graphics and animation
and the classic paper by Shoemake (1985) is very readable introduction to this topic.
The first publication about quaternions for robotics is probably Taylor (1979), and
followed up in subsequent work by Funda (1990).
You will encounter a wide variety of different notation for rotations and transfor-
mations in textbooks and research articles. This book uses ATB to denote a transform
giving the pose of frame {B} with respect to frame {A}. A common alternative notation
is TBA or even BAT. To denote points this book uses ApB to denote a vector from the ori-
gin of frame {A} to the point B whereas others use pBA, or even CpBA to denote a vector
 2.5 · Wrapping Up 61

Fig. 2.20.
Build your own coordinate frame.
a Get the PDF file from http://
www.petercorke.com/axes.pdf;
b cut it out, fold along the dot-
ted lines and add a staple. Voila!

from the origin of frame {A} to the point B but with respect to coordinate frame {C}.
Twists can be written as either (v, ω ) as in this book, or as (ω , v).

Historical and general. Hamilton and his supporters, including Peter Tait, were vigor-
ous in defending Hamilton’s precedence in inventing quaternions, and for opposing the
concept of vectors which were then beginning to be understood and used. Rodrigues
developed his eponymous formula in 1840 although Gauss discovered it in 1819 but,
as usual, did not publish it. It was published in 1900. Quaternions had a tempestuous
beginning. The paper by Altmann (1989) is an interesting description on this tussle of
ideas, and quaternions have even been woven into fiction (Pynchon 2006).

Exercises
1. Explore the many options associated with trplot.
2. Animate a rotating cube
a) Write a function to plot the edges of a cube centered at the origin.
b) Modify the function to accept an argument which is a homogeneous transfor-
mation which is applied to the cube vertices before plotting.
c) Animate rotation about the x-axis.
d) Animate rotation about all axes.
3. Create a vector-quaternion class to describe pose and which supports composition,
inverse and point transformation.
4. Create a 2D rotation matrix. Visualize the rotation using trplot2. Use it to trans-
form a vector. Invert it and multiply it by the original matrix; what is the result?
Reverse the order of multiplication; what is the result? What is the determinant of
the matrix and its inverse?
5. Create a 3D rotation matrix. Visualize the rotation using trplot or tranimate.
Use it to transform a vector. Invert it and multiply it by the original matrix; what
is the result? Reverse the order of multiplication; what is the result? What is the
determinant of the matrix and its inverse?
6. Compute the matrix exponential using the power series. How many terms are re-
quired to match the result shown to standard MATLAB precision?
7. Generate the sequence of plots shown in Fig. 2.12.
8. For the 3-dimensional rotation about the vector [2, 3, 4] by 0.5 rad compute
an SO(3) rotation matrix using: the matrix exponential functions expm and
trexp, Rodrigues’ rotation formula (code this yourself), and the Toolbox function
angvec2tr. Compute the equivalent unit quaternion.
9. Create two different rotation matrices, in 2D or 3D, representing frames {A} and {B}.
Determine the rotation matrix ARB and BRA. Express these as a rotation axis and
angle, and compare the results. Express these as a twist.
62 Chapter 2 · Representing Position and Orientation

10. Create a 2D or 3D homogeneous transformation matrix. Visualize the rigid-body

displacement using tranimate. Use it to transform a vector. Invert it and multi-
ply it by the original matrix, what is the result? Reverse the order of multiplication;
what happens?
11. Create two different rotation matrices, in 2D or 3D, representing frames {A} and {B}.
Determine the rotation matrix ARB and BRA. Express these as a rotation axis and
angle and compare the results. Express these as a twist.
12. Create three symbolic variables to represent roll, pitch and yaw angles, then use these
to compute a rotation matrix using rpy2r. You may want to use the simplify
function on the result. Use this to transform a unit vector in the z-direction. Looking
at the elements of the rotation matrix devise an algorithm to determine the roll,
pitch and yaw angles. Hint – find the pitch angle first.
13. Experiment with the tripleangle application in the Toolbox. Explore roll, pitch
and yaw motions about the nominal attitude and at singularities.
14. If you have an iPhone or iPad download from the App Store the free “Euler Angles”
app by École de Technologie Supérieure and experiment with it.
15. Using Eq. 2.24 show that TT −1 = I.
16. Is the inverse of a homogeneous transformation matrix equal to its transpose?
17. In Sect. 2.1.2.2 we rotated a frame about an arbitrary point. Derive the expression
for computing RC that was given.
18. Explore the effect of negative roll, pitch or yaw angles. Does transforming from
RPY angles to a rotation matrix then back to RPY angles give a different result to
the starting value as it does for Euler angles?
19. From page 53 show that ex ey ≠ ex+y for the case of matrices. Hint – expand the first
few terms of the exponential series.
20. A camera has its z-axis parallel to the vector [0, 1, 0] in the world frame, and its
y-axis parallel to the vector [0, 0, −1]. What is the attitude of the camera with respect
to the world frame expressed as a rotation matrix and as a unit quaternion?