Geometry ⊥

Kinematics →

and Dynamics ×

for Robotics ⊥→×

A Summary

Philippe Nadeau

July 1, 2025

Contents

1 Introduction 7

1.1 Our Robotics Notation Convention . . . . . . . . . . . . . . . . 8

2 Geometry 13

2.1 Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.2 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . 14

2.2.1 Alternate Conventions . . . . . . . . . . . . . . . . . . . 15

2.3 Orientations And Rotations . . . . . . . . . . . . . . . . . . . . 16

2.3.1 The Group of Rotation Matrices . . . . . . . . . . . . . 16

2.3.2 Interpreting Rotation Matrices . . . . . . . . . . . . . . 17

2.3.3 Composing Rotations . . . . . . . . . . . . . . . . . . . 18

2.3.4 Axis-Angle Representation . . . . . . . . . . . . . . . . 20

2.3.5 Unit Quaternions . . . . . . . . . . . . . . . . . . . . . . 22

2.4 Positions And Translations . . . . . . . . . . . . . . . . . . . . 28

2.5 Poses And Rigid Transformations . . . . . . . . . . . . . . . . . 29

2.5.1 Change of Coordinate System . . . . . . . . . . . . . . . 30

2.5.2 Screws and Twists . . . . . . . . . . . . . . . . . . . . . 31

2.6 Reverses . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

2.7 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 34

2.7.1 Product of Matrix Exponentials . . . . . . . . . . . . . 34

2.7.2 Denavit-Hartenberg Parameters . . . . . . . . . . . . . . 35

2.8 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 38

2.8.1 Analytic Methods . . . . . . . . . . . . . . . . . . . . . 38

2.8.2 Numerical Methods . . . . . . . . . . . . . . . . . . . . 39

2.9 Key Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

3 Kinematics 43

3.1 Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

3.1.1 Point on a Rotating Body . . . . . . . . . . . . . . . . . 44

4 CONTENTS

3.2 Rotation Time Derivative . . . . . . . . . . . . . . . . . . . . . 45

3.3 Velocity Twists . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3.3.1 Interpretation . . . . . . . . . . . . . . . . . . . . . . . . 46

3.3.2 Point on a Rotating Body . . . . . . . . . . . . . . . . . 47

3.3.3 Coordinate System Change . . . . . . . . . . . . . . . . 47

3.3.4 Observation Point Change . . . . . . . . . . . . . . . . . 47

3.4 Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

3.4.1 Point on a Rotating Body . . . . . . . . . . . . . . . . . 48

3.5 Acceleration Twists . . . . . . . . . . . . . . . . . . . . . . . . 49

3.6 Key Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

4 Rigid Body Dynamics 51

4.1 Inertial Frame of Reference . . . . . . . . . . . . . . . . . . . . 51

4.2 Moments in Dynamics . . . . . . . . . . . . . . . . . . . . . . . 52

4.3 Moments of a Mass Distribution . . . . . . . . . . . . . . . . . 53

4.3.1 Zeroth Moment: Total Mass . . . . . . . . . . . . . . . . 54

4.3.2 First Moment: Centre of Mass . . . . . . . . . . . . . . 54

4.3.3 Second Moment: Inertia Tensor . . . . . . . . . . . . . . 54

4.4 Momentum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

4.4.1 Moment of Momentum . . . . . . . . . . . . . . . . . . . 58

4.4.2 With Body-Frame Along Principal Axes . . . . . . . . . 59

4.5 Energies, Work, and Power . . . . . . . . . . . . . . . . . . . . 60

4.5.1 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . 60

4.5.2 Potential Gravitational Energy . . . . . . . . . . . . . . 61

4.5.3 Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4.5.4 Power . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4.6 Spatial Inertia Matrix . . . . . . . . . . . . . . . . . . . . . . . 62

4.7 Newtonian Mechanics . . . . . . . . . . . . . . . . . . . . . . . 63

4.7.1 Force . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4.7.2 Torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

4.7.3 Newton-Euler Equations . . . . . . . . . . . . . . . . . . 68

4.7.4 Euler’s Laws . . . . . . . . . . . . . . . . . . . . . . . . 70

4.7.5 Euler’s Equations for the Motion of a Body in a Force

Field . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

4.8 Contacts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

4.8.1 The Rigid Body Assumption . . . . . . . . . . . . . . . 71

4.8.2 Types of Frictional Contacts . . . . . . . . . . . . . . . 72

4.8.3 Computing Contact Forces . . . . . . . . . . . . . . . . 75

4.8.4 Multi-Object Interactions . . . . . . . . . . . . . . . . . 81

4.9 Key Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

CONTENTS 5

5 Manipulator’s Dynamics 83

5.1 Lagrangian Mechanics . . . . . . . . . . . . . . . . . . . . . . . 83

5.1.1 Coordinates, Conﬁgurations, and Constraints . . . . . . 83

5.1.2 Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . 84

5.1.3 Hessian . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

5.1.4 Virtual Displacement and Virtual Work . . . . . . . . . 88

5.1.5 D’Alembert Principle . . . . . . . . . . . . . . . . . . . 89

5.1.6 Lagrange’s Equation Of Motion . . . . . . . . . . . . . . 90

5.1.7 Serial Robot Joint-Space Dynamics in Matrix Form . . 92

5.1.8 An Outlook on Lagrangian and Newtonian Mechanics . 94

5.2 Inverse Dynamics for Control . . . . . . . . . . . . . . . . . . . 94

5.2.1 Kinematics Iterations . . . . . . . . . . . . . . . . . . . 95

5.2.2 Dynamics Iterations . . . . . . . . . . . . . . . . . . . . 96

5.3 Direct Dynamics for Simulation . . . . . . . . . . . . . . . . . . 97

5.4 Calibration and Identiﬁcation . . . . . . . . . . . . . . . . . . . 98

5.4.1 Robot Arm Kinematic Calibration . . . . . . . . . . . . 100

5.4.2 Hand-Eye-Robot-World Calibration . . . . . . . . . . . 104

5.4.3 Inertial Parameters Identiﬁcation . . . . . . . . . . . . . 104

5.5 Key Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109

A A Summary’s Summary 111

A.1 Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

A.2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

A.3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116

A.3.1 Newtonian Mechanics . . . . . . . . . . . . . . . . . . . 118

A.3.2 Lagrangian Mechanics . . . . . . . . . . . . . . . . . . . 119

B Useful Mathematical Formulas 123

B.1 Exponentials and Logarithms . . . . . . . . . . . . . . . . . . . 123

B.2 Trigonometric Identities . . . . . . . . . . . . . . . . . . . . . . 124

B.3 Taylor Series Expansion . . . . . . . . . . . . . . . . . . . . . . 124

B.4 Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125

B.5 Norms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127

B.6 Matrix Properties . . . . . . . . . . . . . . . . . . . . . . . . . . 127

C Skew-Symmetric Operator 129

C.1 Similarity Transform over [u]x[v]x . . . . . . . . . . . . . . . . . 130

C.1.1 Longer Derivation . . . . . . . . . . . . . . . . . . . . . 130

D Rotation Time Derivative 131

6 CONTENTS

E Inertia Tensor Derivation 133

F Expressing Inertia Tensors in Any Frame 137

Chapter 1

Introduction

Robotics is a peculiar ﬁeld in that, in contrast to many ﬁelds like physics or

medicine that study the natural world, it is a ﬁeld that strive to material-

ize concepts imagined by humans. Robots have been artistic creations, with

the work of Karel

Capek and Isaac Asimov most notably, well before the ad-

vent of the ﬁrst industrial robots. To this day, roboticists pursue the goal,

perhaps pointlessly, of materializing the cultural concept of a robot as an au-

tonomous human-like machine capable of performing any task with perfect

assiduity. The ﬁeld of robotics is perhaps not deﬁned, like other ﬁelds, as the

study of phenomena that exist or existed in the natural world, but rather as

an eﬀort to develop the physical counterpart of an idea. The search for tech-

nologies, materials, techniques, and systems that might result in improved

robots has led researchers to explore a wide range of ﬁelds, including mathe-

matics, biology, physics, etc. Meanwhile, students entering the ﬁeld of robotics

might be coming from a well-established discipline like electrical engineering

or computer science that might not have covered some of the fundamental

concepts of robotics. Notably, fundamental concepts of geometry, kinematics,

and dynamics are often learned after the facts, and often misunderstood. Real

robots being subject to the dynamics of the physical world, a solid understand-

ing of dynamics is essential to the design of eﬀective robotic systems. While

this information is often available in robotics textbooks, it is often scattered

across books and chapters, making it diﬃcult for a newcomer to get a clear

and concise overview of the fundamental concepts of geometry, kinematics,

and dynamics. This book aims at providing a summary of essential robotics

prerequisite concepts in a concise manner, without assuming prior dynamics

knowledge, and with an emphasis on detailed and clear derivations.

8 CHAPTER 1. INTRODUCTION

Readers are encouraged to treat this book as a handbook : a small textbook

that can be quickly consulted and that is not meant to be read from cover to

cover. At the end of each chapter, a summary of key concepts is provided and

references are suggested for further reading. Additionally, the ﬁrst appendix

provides a summary of the most important equations and concepts covered in

the book, and can serve as a quick refresher that can be read in under an hour.

1.1 Our Robotics Notation Convention

This book covers a wide range of concepts yet aims at being succint. To

accomplish this feat, we use equations to concisely encode information that

could have taken a whole paragraph if written in prose. However, condensing

information in this way introduces the challenge of devising a code that can be

used to encode and decode information easily. Fortunately, the ﬁeld of math-

ematics, over many centuries, has deﬁned an eﬃcient code that we can use.

Fran¸cois Vi`ete (1540-1603), a mathematician who made great contributions

to algebraic notation, also worked as a code breaker for French kings, giving

them the ability to decipher some of the enemy’s critical communications. In

robotics, we need to slightly expand the conventional mathematical notation

to concisely encode speciﬁc information used frequently in our ﬁeld (e.g., the

velocity of a mobile robot as seen from another one). Akin to the early days of

mathematics, there is currently no widely dominating standard for robotics no-

tation apart from what is borrowed from mathematics and physics. However,

roboticists needs are apparent: people desire a clear, concise, and consistent

notation.

We will distinguish between distinguish scalars from vectors and matrices

by using a diﬀerent typography for each mathematical object. While vectors

will be denoted using bold lowercase letters (e.g. v), matrices will be denoted

using bold uppercase letters (e.g. M), and scalars will be denoted using normal

weight letters (e.g. m or K).

In this book, we will make use of the RIGID notation that is designed to be

compliant with the ISO 80000 Standard on Quantities and Units, and concise

yet unambiguous. The RIGID notation deﬁnes the relative position of three

main symbols (subject, basis and coordinate system) around a central element

(the quantity) as shown in Fig. 1.1. The quantity is the main concept that an

equation puts in relation with other concepts. For instance, Newton’s second

law can be encoded in the equation f = ma that relates three quantities: force,

mass, and acceleration. In robotics, it is often desired to encode the object

whose state is referred to by the quantity (e.g. the velocity of the drone). The

RIGID notation refers to this element as the subject — the information encoded

1.1. OUR ROBOTICS NOTATION CONVENTION 9

Figure 1.1: The main symbols used in the RIGID notation convention. The

quantity Q is potentially surrounded by subject s, basis b, coordinate system

c, and other symbols used to express exponentiation, transposition, time dif-

ferentiation, etc.

is the quantity of the subject. The principle of relativity, which essentially

states that relative quantities matter in physics (in contrast to absolute ones),

is central to our understanding of physics. Hence, it is necessary to specify

relative to what object a given quantity is measured, observed, or deﬁned. In

the RIGID convention, this element is referred as the basis. Hence, the quantity

of interest is speciﬁcally the one of the subject as seen from the basis (e.g. the

velocity (quantity) of the drone (subject) as observed from the car (basis)).

In theory, the three elements (quantity, subject, basis) are suﬃcient to build

well-deﬁned equations relating most robotics concepts grounded in physics.

However, robotics in inherently practical, and quantities eventually have to be

computed to enable the robots to move according to plan. That is to say that

an additional code has to be used to translate concepts into numbers that can

be processed by our mathematical tools. As we will see in Sec. 2.2, a coordinate

system enables encoding physical concepts into sequences of numbers, and the

coordinate system is the ﬁnal piece needed in a well-deﬁned robotics notation.

Mentionning every element of every symbol in every equation can hinder

the objective of eﬃciently communicating ideas, the original reason for which

symbols and equations are used instead of prose. Hence, the RIGID convention

deﬁnes rules that, when followed correctly, enable conciseness without creating

any ambiguity. In essence, the rules boils down to:

• mentioning the coordinate system can be ommitted if it is the same as

the basis;

• mentioning the basis can be ommitted if the context allows only for one;

and

• mentioning the subject can be ommitted if the context allows only for

one.

For instance, if the coordinate system symbol is ommitted for a quantity in

an equation and the RIGID convention was used, it can be assumed that the

10 CHAPTER 1. INTRODUCTION

coordinate system is the same as the basis — the information is not lost in

the encoding. In other words,

≡

(1.1)

and if the context allows for only one basis (e.g. the control of a single drone

by a ﬁxed operator),

≡

, (1.2)

which can be further simpliﬁed to

Q (1.3)

when the subject is the only one that can be referred to in the context.

The RIGID notation make space for common operators like transposition,

exponentiation, diﬀerentiation, etc. For instance, if p is the symbol denoting

the position of the subject,

car

boat



5 0 0



(1.4)

can be the velocity of the boat as seen from the car and expressed in the F

coordinate system. Since the transpose of the velocity results in a row vector,

we can deduce that the coordinates of the position p are expressed in a column

vector.

In the following chapters, a special basis called world frame will be fre-

quently used and the letter w will be used to denote it. The world frame will

be assumed to be an inertial frame, an important characteristic that is cov-

ered in Sec. 4.1. Also, the subject will often consist in a rigid body (e.g. the

gripper of a robot manipulator) to which a body frame, symbolized with b, will

be associated. In many scenarios, only the world and body frames are needed

to fully deﬁne the kinematics and dynamics of a robot.

To make it easier to keep track of all the symbols used in the following

chapters, a table of frequently used symbols is provided in Table 1.1.

1.1. OUR ROBOTICS NOTATION CONVENTION 11

Table 1.1: Notation and common symbols used throughout this book

Expression Description (w.r.t ≡ with respect to)

i Unit-length vector i

Transpose of a

[a]

Skew-symmetric operation on a

a · b = a

b Dot product between vector a and b

a × b = [a]

b Cross product of a on b

−1

Inverse of matrix M

Cartesian reference frame named o

{w} Coordinate system named w

Position of a w.r.t. b, expressed in c

Rotation/Orientation of a w.r.t. b

Pose of a w.r.t. b, with position expressed in c

a First derivative of a w.r.t. time

a Second derivative of a w.r.t. time

Linear velocity of a relative to b, expressed in c

ω Angular velocity

a Linear acceleration

α Angular acceleration

= [

]

Spatial/six-dimensional (6D) velocity

= [

]

Spatial (6D) acceleration

ρ(p) Mass density function

m Mass

c Centre of mass, also symbolized with

Inertia tensor of a computed w.r.t. b, expressed in c

Spatial (6D) inertia matrix of body a computed

w.r.t. b, expressed in c

Force exerted at a, experienced in b, expressed in c

Torque exerted at a, experienced in b, expressed in c

= [

]

Wrench (6D)

= m

Linear momentum

Real angular momentum when velocity is

Kinetic energy when velocity is

g ≈ 9.81ˆg Gravitational force on earth

W =

f(s)ds Work (scalar)

P = f ·

p Power (scalar)

U Potential gravitational energy (scalar)

q Generalized coordinates

Q Generalized forces

M(q) System’s mass matrix

C(q,

q) System’s Coriolis matrix

g(q) System’s gravity matrix

12 CHAPTER 1. INTRODUCTION

Chapter 2

Geometry

2.1 Vectors

The position of a particle exists in space even when no mathematical represen-

tation is used to express its position, it’s a natural phenomenon. Consequently,

the distance between two particles a and b also exists in space even though no

representation system is used to describe the distance. This is like having a

map with no scale — the distance between two points on the map exists but

cannot be quantiﬁed. A free vector

connects both particles by starting at

a and reaching b but is not grounded in any representation. In other words,

a free vector has a magnitude and direction but is not ﬁxed in space – no

coordinates are deﬁned for it. Many physical quantities are free vectors, and

the relation between them can be described by vector equations. Many vector

equations are independent of the coordinate system used to express the vectors

– in physics, most of the time, relative quantities matter more than absolute

ones.

The extent to which a vector v is aligned with a unit vector ˆu is given by

projecting the ﬁrst vector onto the second vector with

v · ˆu = v

ˆu = ∥v∥∥u∥cos(θ), (2.1)

where θ is the angle between the two vectors. In 3D, vectors v and u span a

plane that passes through the origin. The normal to the plane is given by the

cross product of the two vectors with

v × u = [v]

u = ∥v∥∥u∥sin(θ)ˆn, (2.2)

14 CHAPTER 2. GEOMETRY

where ˆn is the unit vector normal to the plane spanned by v and u, and [·]

the skew-symmetric matrix deﬁned in appendix C. Note that the cross-product

is well-deﬁned only in 3D. For this reason, and since the cross-product appears

in many equations, we will restrict ourselves to the 3D space.

ˆu

v · ˆu

v × u

Figure 2.1: On the left, v is projected onto ˆu and the extent of the projection is

given by the dot product. On the right, the cross product of v and u produces

a vector that is orthogonal to the plane spanned by v and u.

2.2 Coordinate Systems

A coordinate system {w} deﬁned by a reference frame F

(i.e. its axes) and a

length scale (e.g. meters) can be used to ground positions in a mathematical

representation, enabling the usage of algebra on positions. A free vector that

is expressed in some referenced frame is sometimes called a line vector.

Assuming that vectors are described by 3 × 1 matrices, the coordinate of

a vector v on a basis vector ˆx is given by the extent of the projection of v

onto ˆx with v · ˆx. Relative to three orthogonal basis vectors ˆx, ˆy, and ˆz, the

coordinates of a vector v are given by





v · ˆx

v · ˆy

v ·ˆz









ˆx

ˆy

ˆz





|{z}

v (2.3)

= F

v, (2.4)

where





ˆx

ˆy

ˆz









ˆx

ˆy

ˆz





(2.5)

2.2. COORDINATE SYSTEMS 15

is the matrix that deﬁnes the reference frame F

with the basis vectors ˆx, ˆy,

and ˆz in the ﬁrst, second, and third rows, respectively. This is the orientation

speciﬁcation of the reference frame F

In 3D, a Cartesian reference frame F

is deﬁned by



ˆw



, (2.6)

where the elements ˆw

, ˆw

of the matrix are (by deﬁnition) unit vectors

that are orthogonal to each other, implying that ˆw

ˆw

= 1 and ˆw

ˆw

= 0. The reference frame F

is therefore deﬁned by a 3 × 3 matrix,

which is an element of a group called SO(3).

The orientation of a reference frame F

with respect to a reference frame

can be described with a rotation

, an orientation speciﬁcation that is

further detailed in Sec. 2.3. Since the orientation of a reference frame relative

to another is independent of the position of the frames, no coordinate system

is needed to describe a rotation and its notation has no right sub-script.

2.2.1 Alternate Conventions

When deﬁning the reference frame, the ﬁrst axis ˆw

can be chosen arbitrarily

and the second axis ˆw

must lie in the plane perpendicular to ˆw

. The line

of the last axis ˆw

is determined as being orthogonal to the plane spanned

by ˆw

and ˆw

but its direction can be chosen, giving rise to two conventions:

left-handedness and right-handedness. By far, the most common convention is

the right-handedness where the direction of the ˆw

axis is given by the middle

ﬁnger of the right hand if the thumb is pointing in the ˆw

direction and the

index is pointed in the ˆw

direction as shown in Fig. 2.2. Mathematically,

ˆw

= ˆw

× ˆw

, (2.7)

which respects Hamilton’s convention for the cross product — Hamilton is

the godfather of many important concepts in mathematics and physics. The

analog left-handed convention makes use of the left hand instead of the right

hand and its use is discouraged in robotics as it leads to confusion.

Expressed in coordinate system {w}, the position of particle b relative to

particle a is well-deﬁned as

. This position vector can be laid out in a row

vector or in a column vector, giving rise to yet another convention. A homo-

geneous transformation T is applied to a column vector by pre-multiplying it

while the same transformation is applied to a row vector by post-multiplying

it by the transpose of the transformation. By far, the column vector is

the most widely used in robotics.

16 CHAPTER 2. GEOMETRY

ˆx

ˆy

ˆz

Figure 2.2: The right-hand can be used to form a coordinate system that

respects Hamilton’s convention for the cross product.

2.3 Orientations And Rotations

2.3.1 The Group of Rotation Matrices

A 3 × 3 rotation matrix has nine degrees of freedom but, in 3D, a rotation

has only three. Therefore, not every 3 × 3 matrix is a valid rotation matrix.

To represent a rotation, all columns of the matrix must be orthogonal to

each other. Also, all columns of the matrix must have a unit norm (making

columns orthonormal ). Together, these criterias imply six constraints (three

per criteria). Finally, a rotation is said to be proper if its determinant is

positive, as a rotation matrix with a negative determinant would correspond

to a rotation followed by a reﬂection (an improper rotation).

Mathematically, the set of valid rotation is the Special Orthogonal Group

(abbreviated SO(3)) deﬁned as

SO(3) =



3×3

| R

R = 1

3×3

, det (R) = +1



. (2.8)

More generally, a group is a set G on which a binary operator O{} is deﬁned

such that four axioms are respected:

• Closure: The result from the operator is also in the group (O{a, b} ∈ G).

2.3. ORIENTATIONS AND ROTATIONS 17

• Identity: There exist a unique element I in the group such that O{a, I} =

O{I, a} = a for any a ∈ G.

• Inverse: For any a ∈ G, there is a unique element a

−1

∈ G such that

O{a, a

−1

} = O{a

−1

, a} = I.

• Associativity: For any a, b, c ∈ G, O{c, O{a, b}} = O{a, O{b, c}}

For rotation matrices, the operator of the group is matrix multiplication, which

composes rotations.

2.3.2 Interpreting Rotation Matrices

In 3D, rotation matrices are 3 × 3 matrices that exhibit the same properties

as those of the reference frames detailed in Sec. 2.2. They are used to express

the orientation of a reference frame F

relative to a reference frame F

with

· F





ˆa













ˆa





(2.9)

such that

= F

· F

, (2.10)

where the vectors deﬁning the reference frames are row unit vectors placed in

the physical world.

The ij-th element of rotation matrix

describes the orientation of i-th

axis of frame F

relative to the j-th axis of frame F

– it is eﬀectively the

projection of the i-th axis of frame F

onto the j-th axis of frame F

. More

precisely, the ij-th element is equal to the cosine of the angle θ

between

and b

, and for that reason, the rotation matrix is sometimes called the

directional cosine matrix. Noting the deﬁnition of the dot product as a

·b

∥a

∥



cos(θ

) and since both axes are unit vectors,

cos(θ

) = a

· b

= a

= b

, (2.11)

which supports deﬁning the rotation matrix as





cos(θ

) cos(θ

)

cos(θ

) cos(θ

)

cos(θ

) cos(θ

)









ˆa





. (2.12)

18 CHAPTER 2. GEOMETRY

Figure 2.3: Visual interpretation of the rotation matrix

with respect to the

reference frame F

. The columns of the rotation matrix express the direction

of the axes of reference frame F

relative to reference frame F

Computing the transpose of (2.12) gives





ˆa











ˆa











cos(θ

) cos(θ

)

cos(θ

) cos(θ

)

cos(θ

) cos(θ

)





which shows that the transpose of a rotation matrix is its inverse operation:

−1

. (2.13)

While a rotation can be expressed in many diﬀerent representations, the

rotation matrix is usually the easiest to interpret by inspecting how its axes

are oriented relative to the reference frame it is deﬁned relative to. Indeed,

the components of axis

in F

are obtained by projecting it onto the axes

of F

with



·ˆa



, which corresponds to the ﬁrst column

of the rotation matrix in (2.12). Hence, the columns of the rotation matrix

express the direction of the axes of reference frame F

relative to reference

frame F

as pictured in Fig. 2.3.

2.3.3 Composing Rotations

Any rotation in 3D space can be expressed as a composition of three ele-

mentary rotations, each representing a rotation about one of the axes of the

reference frame. There are mainly two ways of composing rotations: through

2.3. ORIENTATIONS AND ROTATIONS 19

pre-multiplication and through post-multiplication. In general, a transforma-

tion is post-multiplied when it is deﬁned with respect to the axes before any

transformation took place (i.e. the ﬁxed frame). Conversely, a transformation

is pre-multiplied when it is deﬁned with respect to the axes obtained following

previous transformations (i.e. the moving frame).

Figure 2.4: The composition of R

(θ

) and R

(θ

) actively (on the left) and

passively (on the right). On the left, rotations are deﬁned relative to the

previous/moving frame. On the right, rotations are deﬁned relative to the

ﬁrst/ﬁxed frame with the blue shaded area depicting the rotation about the

ﬁxed blue axis. Note that the number of distinct axes is smaller on the left,

making it easier to use when deﬁning successive rotations.

If you use your right hand to represent a frame and you sequentially perform

rotations of your hand about the axes of your ﬁngers, your hand is a moving

frame and you are performing active/alibi/intrinsic rotations. However, if

you are looking at a corner of a room and sequentially performing rotations

about the axes of the corner (i.e. the ﬁxed frame), then you are performing

passive/alias/extrinsic rotations.

• Passive/Alias/Extrinsic Rotations: Rotations (z−y−x) are compounded

through post-multiplication and each rotation is expressed about the

ﬁxed frame. A rotation about the z axis of the ﬁxed frame followed by a

rotation about the y axis of the ﬁxed frame followed by a rotation about

the x axis of the ﬁxed frame is R = R

• Active/Alibi/Intrinsic Rotations: Rotations (z−y

′

−x

′′

) are compounded

through pre-multiplication and each rotation is expressed about the mov-

ing frame. A rotation about the z axis of the initial frame followed by a

20 CHAPTER 2. GEOMETRY

rotation about the y axis of the moved frame (the new y axis) followed

by a rotation about the x axis of the new moved frame is R = R

Elementary rotations in a right-handed 3D Cartesian space are deﬁned as

such:

(θ) =





1 0 0

0 cos(θ) −sin(θ)

0 sin(θ) cos(θ)





(2.14)

(θ) =





cos(θ) 0 sin(θ)

0 1 0

−sin(θ) 0 cos(θ)





(2.15)

(θ) =





cos(θ) −sin(θ) 0

sin(θ) cos(θ) 0

0 0 1





. (2.16)

2.3.4 Axis-Angle Representation

Euler’s rotations theorem state that any sequence of rotations can equivalently

be expressed as a single rotation about some axis. Consequently, any rotation

can be expressed as a tuple (ˆω, θ) where ˆω is the unit-length axis about which

the scalar rotation θ is performed.

For instance, the axis-angle representation can be useful to compute the

rotation that would bring a unit-vector ˆu onto another unit-vector ˆv, both

expressed in the same reference frame. To do so, the cross-product between

both vectors is taken to obtain the rotation axis

ˆω =

ˆu × ˆv

∥ˆu × ˆv∥

that is normal to both vectors. Then, by the deﬁnition of the dot product,

ˆu · ˆv = ∥ˆu∥∥ˆv∥cos(θ) = cos(θ)

such that

θ = arccos (ˆu · ˆv)

is the angle about ˆω between ˆu and ˆv. The orientation diﬀerence is thereby

deﬁned with (ˆω, θ), and the rotation matrix that would produce the same

rotation can be obtained through Rodrigues’ formula.

2.3. ORIENTATIONS AND ROTATIONS 21

Rodrigues’ formula can be used to obtain a rotation matrix from the axis-

angle representation with

R = e

[ˆω]

(2.17)

= 1

3×3

+ sin(θ) [ˆω]

+ (1 − cos(θ)) [ˆω]

(2.18)





c + ˆω

(1 − c) ˆω

ˆω

(1 − c) − ˆω

s ˆω

ˆω

(1 − c) + ˆω

ˆω

(1 − c) + ˆω

s c + ˆω

(1 − c) ˆω

ˆω

(1 − c) − ˆω

ˆω

(1 − c) − ˆω

s ˆω

ˆω

(1 − c) + ˆω

s c + ˆω

(1 − c)





(2.19)

where c = cos(θ), s = sin(θ), ˆω = [ˆω

, ˆω

], and e

[ˆω]

is the matrix

exponential of [ˆω]

θ.

The matrix exponential can be considered to be an extension of Euler’s

formula

jθ

= cos(θ) + j sin(θ) =

n=0

∞

(jθ)

(2.20)

to hyper-complex numbers such that

θ[ ˆω]

n=0

∞



θ [ˆω]



= 1

3×3

+ sin(θ) [ˆω]

+ (1 − cos(θ)) [ˆω]

. (2.21)

When a rotation matrix is provided and one wishes to obtain the equivalent

(ˆω, θ) tuple, the matrix logarithm:

θ = arccos



tr (R) − 1



(2.22)

[ˆω]

2 sin(θ)



R − R



(2.23)

can be used for that purpose. (2.22) is clearly undeﬁned when R = 1

3×3

− 1

3×3

= 0 (this is the singularity of this representation), and when

tr (R) = −1 as arccos(−1) is undeﬁned. If the provided rotation matrix is

identity, then any axis can be chosen with θ = 0. If tr (R) = −1, then one of

22 CHAPTER 2. GEOMETRY

the three following solutions can be selected

θ = π with (2.24)

ˆω =

2(1 + R

3,3

)





1,3

2,3

1 + R

3,3





or (2.25)

ˆω =

2(1 + R

2,2

)





1,2

1 + R

2,2

3,2





or (2.26)

ˆω =

2(1 + R

1,1

)





1,1

1 + R

2,1

3,1





, (2.27)

where R

i,j

is the (i, j)-th entry of the rotation matrix.

For small rotations, the axis-angle representation is nearly singular and

can behave badly in practice due to numerical errors since R ≈ 1 will produce

very large arccos values in (2.22) and 1/ sin(θ) will also become very large.

2.3.5 Unit Quaternions

To alleviate some of the problems due to the presence of the singularity in

the axis-angle representation, an additional parameter and a constraint can

be used.

Starting from the expanded matrix of (2.18) and performing a change of

variables such that

= cos(θ/2), (2.28)

= ˆω

sin(θ/2), (2.29)

= ˆω

sin(θ/2), (2.30)

= ˆω

sin(θ/2), (2.31)

the matrix in (2.18) can be rewritten as

R =





2(e

+ e

) − 1 2(e

− e

) 2(e

+ e

)

2(e

+ e

) 2(e

+ e

) − 1 2(e

− e

)

2(e

− e

) 2(e

+ e

) 2(e

+ e

) − 1,





(2.32)

where the elements [e

, e

]

are called the Euler parameters. Addition-

ally, enforcing

∥[e

, e

]∥ = 1

2.3. ORIENTATIONS AND ROTATIONS 23

restricts the 4-dimensional parameter space to valid rotations. Euler param-

eters can be conveniently encoded in a quaternion, making it possible to use

quaternions’ algebra to rotate vectors.

A quaternion is a hyper-complex number

q = q

+ q

i + q

j + q

k (2.33)

where

j, and

k are unit-length basis vectors and q

, q

, and q

are real

numbers. A quaternion is usually more conveniently represented as a vector

q =









, (2.34)

where q

is the real part and q

is the vectorial part. A quaternion with q

= 0

is sometimes said to be pure. The operations that can be performed on quater-

nions diﬀer from those that are permitted with other rotation representations.

For instance, quaternion addition

q + p =











+ p



(2.35)

follows from vector addition and is therefore commutative and associative.

The quaternion product can be concisely expressed using the left quaternion

product matrix

[q]

⊗







−q







= q

4×4



0 −q

−[q

]



(2.36)

such that quaternion product can be expressed as

q ⊗ p = [q]

⊗

p, (2.37)

where q ⊗ p denotes the product of two quaternions.

Vectors expressed in quaternion form with v =



0, v

, v



can be ro-

tated using quaternions with the (colloquially named) sandwich product

q ⊗ v ⊗ q

∗

, (2.38)

which corresponds to the action Rv where v is the vector being rotated.

The exponential of a quaternion is deﬁned as

= e



cos(∥q

∥)

∥q

∥

sin(∥q

∥)



, (2.39)

24 CHAPTER 2. GEOMETRY

and produces a quaternion. The logarithm of the quaternion is deﬁned as

log(q) =



log(∥q∥)

uθ



, (2.40)

and produces a 4-dimensional vector.

Quaternions have the following properties

q ⊗ p = p ⊗ q (2.41)

(q ⊗ p) ⊗ r = q ⊗ (p ⊗ r) (2.42)

q ⊗ (p + r) = q ⊗ p + q ⊗ r (2.43)

∥q∥ =

+ q

(2.44)

∗



−q



(2.45)

(p ⊗ q)

∗

= q

∗

⊗ p

∗

(2.46)

−1

∗

∥q∥

+ q



−q



(2.47)

where ∥·∥ is the norm, (·)

∗

is the complex conjugate, and (·)

−1

is the inverse.

Additionally, unit quaternions enjoy the following properties

∥q∥ = 1 (2.48)

−1

= q

∗

(2.49)

q =



cos(ϕ) u sin(ϕ)



, (2.50)

where a rotation of ϕ about u is performed. Although (2.50) might seems

like a way to convert a rotation represented as axis-angle into a quaternion,

this is not exactly how it is done. Indeed, the quaternion representation has

a particularity which is that it rotates twice slower than the 3D rotation it

represents. For instance, if two quaternions are separated by an angle ϕ, then

the equivalent rotation is done over an angle θ = 2ϕ.

Indeed, as hinted by the factors of 2 in (2.32), there exists two antipodal

quaternions (i.e., −q and q) mapping to the same rotation. Consequently, to

represent a 3D rotation of θ about some axis ˆω with a quaternion, the ϕ in

(2.50) must be equal to θ/2. This leads to the following relationship between

a quaternion and its equivalent axis-angle representation

q =



cos(θ/2) ˆω sin(θ/2)



, (2.51)

2.3. ORIENTATIONS AND ROTATIONS 25

where ˆω and θ are those deﬁned in Sec. 2.3.4. To convert a quaternion into

the axis-angle representation, the parameters are obtained with

θ = 2 atan(∥q

∥/q

) (2.52)

ˆω = q

/ ∥q

∥, (2.53)

where ˆω = q

for unit quaternions.

A unit quaternion can be easily obtained from a rotation matrix with

1 + tr (R)

(2.54)





3,2

− R

2,3

1,3

− R

3,1

2,1

− R

1,2





(2.55)

where R

i,j

is the (i, j)-th entry of the rotation matrix. The reverse operation,

in which a rotation matrix is obtained from a quaternion, is performed with

R =



− q



3×3

+ 2q

]

(2.56)





+ q

− q

2(q

− q

) 2(q

+ q

)

2(q

+ q

) q

− q

+ q

− q

2(q

− q

)

2(q

− q

) 2(q

+ q

) q

− q

+ q





. (2.57)

The equation in (2.54) is singular when tr (R) = −1, producing a division

by zero, and also breaks when tr (R) ≤ −1 as the square root of a negative

number is not real. Several techniques exist to robustly convert a rotation

matrix to a unit-quaternion with the aim of reducing the consequences of

numerical inaccuracies on the result.

As empirically measured in A Survey on the Computation of Quaternions

from Rotation Matrices by Sarabandi and Thomas, Cayley’s method seems to

26 CHAPTER 2. GEOMETRY

be the fastest and most accurate technique. It starts by deﬁning

(1+R

)

+ (R

−R

)

+ (R

−R

)

+ (R

−R

)

(2.58)

(1+R

−R

)

+ (R

−R

)

+ (R

)

+ (R

)

(2.59)

(1−R

−R

)

+ (R

)

+ (R

−R

)

+ (R

)

(2.60)

(1−R

−R

)

+ (R

)

+ (R

)

+ (R

−R

)

(2.61)

and then uses the largest positive number in [e

, e

] to determine the

signs of the elements of the unit-quaternion. The rationale behind choosing

to use the largest positive number is that since we need to assume that an

element is positive to resolve the sign ambiguity, we should be choosing the

element that is the less likely to change sign due to a small perturbation from

numerical inaccuracies. That way, we end up with one of the two antipodal

quaternions that can represent the rotation. Consequently, Cayley’s method

2.3. ORIENTATIONS AND ROTATIONS 27

stipulates that with i = argmax([e

, e

]),

if i = 0





























sign



3,2

− R

2,3



sign



1,3

− R

3,1



sign



2,1

− R

1,2









, (2.62)

if i = 1





























sign



3,2

− R

2,3



sign



2,1

+ R

1,2



sign



1,3

+ R

3,1









, (2.63)

if i = 2





























sign



1,3

− R

3,1



sign



2,1

+ R

1,2



sign



3,2

+ R

2,3









, (2.64)

if i = 3





























sign



2,1

− R

1,2



sign



1,3

+ R

3,1



sign



3,2

+ R

2,3









, (2.65)

such that Euler parameters with consistent signs are obtained. These param-

eters can then be used in a quaternion to perform rotations on vectors.

A random orientation can easily be obtained with quaternions, an oper-

ation that is much more diﬃcult to perform using other representations. To

obtain a uniformly distributed rotation, four numbers can be sampled from

the Gaussian distribution (not uniform, Gaussian) and three of those are to

be normalized to produce a unit quaternion. Indeed, samples q ∈ S

are

uniformly distributed over S

for

q =

∥[a, b, c, d]∥



a b c d



, (2.66)

where a, b, c, d ∈ N (0, 1).

28 CHAPTER 2. GEOMETRY

Interpolating between two quaternions q

and q

using a parameter t ∈

[0, 1] can also be performed easily. The ﬁrst step is to ensure that the angle

between the quaternion vectors is acute such that the shortest path will be

followed. This can be veriﬁed by making sure that the angle between the two

quaternions

· q

= q

= cos(∆ϕ) < 0

should be acute. If it’s not the case, then you can simply set q

to its antipodal

quaternion −q

then start the process by computing the rotation diﬀerence

with

δ = q

∗

⊗ q

(2.67)

and then obtaining the corresponding axis-angle representation with

θ = 2 atan(∥δ

∥/δ

) (2.68)

ˆω = δ

/ ∥δ

∥ (2.69)

to ﬁnally compute the interpolated quaternion with

= q

⊗



cos(tθ/2)

ˆω sin(tθ/2)



. (2.70)

A critical advantage of the unit-quaternion representation is that it is trivial

to normalize the quaternion to enforce its constraint. Conversely, it can be

complicated to re-orthogonalize a rotation matrix such that rows and columns

are orthogonal (its constaints). Due to numerical inaccuracies, the composition

of many rotations will inevitably lead to constraint violation.

2.4 Positions And Translations

The position of the origin of F

relative to F

is deﬁned by the free vector

. When

is expressed in F

, the position coordinates of

are denoted





such that

= p

ˆw

+ p

ˆw

+ p

ˆw

, (2.71)

where the basis vectors ˆw

, ˆw

of F

are weighted by the position coor-

dinates to produce the position vector. Hence, we say that

is the position

of F

relative to F

expressed in F

A position vector

can be expressed in a coordinate system F

with





ˆw





, (2.72)

2.5. POSES AND RIGID TRANSFORMATIONS 29

where the vector is projected onto the basis vectors of F

to produce the

position coordinates. Hence,





ˆc





(2.73)





ˆc



ˆw

+ p

ˆw

+ p

ˆw



ˆc



ˆw

+ p

ˆw

+ p

ˆw



ˆc



ˆw

+ p

ˆw

+ p

ˆw







(2.74)





ˆc

· (p

ˆw

) + ˆc



ˆw



+ ˆc

· (p

ˆw

)

ˆc

· (p

ˆw

) + ˆc



ˆw



+ ˆc

· (p

ˆw

)

ˆc

· (p

ˆw

) + ˆc



ˆw



+ ˆc

· (p

ˆw

)





(2.75)





ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw

ˆc

· ˆw





{z }









|{z}

, (2.76)

where (2.71) was used to expand the position vector and

was identiﬁed

from (2.12). Therefore, the coordinate system in which position vector is

expressed in can be changed by pre-multiplying is by the rotation matrix

relating the orientation of one coordinate system to the other. Succintly,

. (2.77)

From standard vector algebra we have that

= 0, (2.78)

, (2.79)

k · (

) = k

+ k

, (2.80)

where k is a scalar. Hence, the target of the position vector can be changed

with

(2.81)

as long as the vectors are expressed in the same coordinate system.

2.5 Poses And Rigid Transformations

The pose of a reference frame is deﬁned by the position of its origin and by the

orientation of its orthonormal axes. This information is bundled into a pose



0 1



(2.82)

30 CHAPTER 2. GEOMETRY

Figure 2.5: Visual depiction of the vector algebra for 2

−

Note that vector algebra is independant of reference frames, but if vectors are

expressed in a reference frame, it must be the same for all vectors.

that deﬁnes the pose of b relative to a with position expressed in c. Note that

although the pose has a right sub-script, it only applies to the position.

A pose

can be transformed into another pose by pre-multiplying it by

a homogeneous transformation

which produces



a e

d b

0 1



(2.83)

where the operation only makes sense if {e} = {a} (i.e. the target of the

transformation is the reference of the pose), {c} = {b} (i.e. the transformation

is expressed in the same frame it is deﬁned with respect to), and {a} = {f}

such that

is a valid change of reference frame as per (2.77). If those

rules are respected, we have



a a

d b

0 1



(2.84)



0 1



(2.85)



0 1



(2.86)

(2.87)

2.5.1 Change of Coordinate System

Sometimes, a pose

expressed in some coordinate system {c} must be

expressed in another coordinate system {d}. This change of coordinate system

is done by pre-multiplying the position vector in

by a rotation

. Since

2.5. POSES AND RIGID TRANSFORMATIONS 31

only the position component of the pose is changed, it is not possible to simply

multiply the pose by an homogeneous transformation built from

. Instead,

you must deﬁne



b d

0 1



(2.88)

which is not equal to

=



0 1



(2.89)

However, as indicated in (2.84), a pose usually needs to be expressed in

the same frame its deﬁned with respect to such that a standard homogeneous

transformation can be applied to it.

2.5.2 Screws and Twists

The Chasles-Mozzi theorem states that any rigid transformation can be ex-

pressed as a displacement over the thread of a screw. Indeed, a particle moving

along the thread of the screw will experience a rotation about the axis of the

screw as well as a translation along the same axis. This screw representa-

tion can be useful in representing revolute and prismatic joints with a single

compact representation.

A screw is deﬁned by its unit axis ˆs, thread pitch h, and a point q located

anywhere on the axis (the origin of the axis can be chosen easily). The pitch h

is the ratio of linear motion to angular motion such that a screw with h = ∞

is a pure translation while a screw with h = 0 is a pure rotation. The motion

over a given screw can be expressed by specifying the magnitude θ of the

rotation about the screw axis. With such a deﬁnition, the displacement due

to the rotational motion is given by

R = e

[ˆs]

(2.90)

p = hθˆs +



3×3

− e

[ˆs]



q (2.91)

such that an homogeneous transformation matrix can be easily built as from

the screw parameters.

Interestingly, if the magnitude θ describes the rate of rotation about the

screw axis, then the screw representation can be used to describe the linear

and angular components of the velocity. In that case, the pitch represents the

ratio of linear velocity to angular velocity and

ν =





hˆsθ −ˆsθ × q

ˆsθ

hω − ω × q

ˆsθ

if ω = 0 (2.92)

32 CHAPTER 2. GEOMETRY

deﬁnes a twist where the linear velocity v is the sum of a component along

the axis and a component orthogonal to the axis (leading the thread towards

and away from the axis). A twist is a motion about a screw, which can be

considered as the coordinate system the motion is deﬁned relative to.

A unit screw

S is expressed from the twist components as

S =











ω −

ω × q

if ∥ω∥ = 1

if ∥ω∥ = 0 and ∥v∥ = 1

(2.93)

such that

Sθ describes a rigid transformation.

When given the components of a twist, it is possible to ﬁnd parameters

for the screw that the motion is performed about. Of course, since q was

arbitrarily chosen on the screw axis, an inﬁnite number of solution exists,

although the simplest one is to choose the q that is at the intersection between

the screw axis and the plane orthogonal to the axis. Hence, for ν =



v ω



we have







h =

ˆs = ω q =

ω×v

if ∥ω∥ = 0

h = ∞ ˆs = v q = 0

if ∥ω∥ = 0

(2.94)

that describes the screw parameters for the given twist.

The exponential map that produces homogeneous transformations from

unit screws described with twist components is

T = e

[

]

(2.95)

[

ω]



3×3

θ + (1 − cos(θ)) [

ω]

+ (θ − sin(θ)) [

ω]



1×3

(2.96)

where



[

ω]

0 0



(2.97)

is 4 ×4 matrix representation of the screw. The reverse operation can be done

with the matrix logarithm that maps a screw to a homogeneous transformation.

To do so, the transformation is ﬁrst inspected to see if R = 1

3×3

that indicates

that ω = 0, v = p/ ∥p∥, and θ = ∥p∥. Otherwise,

ω and θ are obtained

through (2.22) and

v = 1

3×3



(2 − θ cot(θ/2)) [

ω]

− θ [

ω]



(2.98)

2.6. REVERSES 33

provides the missing information.

The adjoint representation of a rigid transformation is the 6 × 6 matrix

Ad (T) =



R [p]

3×3



(2.99)

where

T =



R p

0 1



(2.100)

is the pose deﬁned in (2.82), can be used to change the reference frame that a

twist or screw is expressed in with

= Ad (

)

(2.101)



[

]

3×3 w



(2.102)

and has the following properties





Ad (

) = Ad





(2.103)

Ad (T)

−1

= Ad



−1



(2.104)

2.6 Reverses

It is useful to know how a position, rotation or pose can be reversed as the

reverse can appear in algebra. The reverse of a position vector is a vector with

the same magnitude but with a reversed direction. This is done by multiplying

the components of the position vector by −1. The reverse of a rotation is given

by its inverse and its transpose since R

−1

= R

due to the orthonormality of

the vectors forming the rotation. The pose being a bundle of a rotation and a

position vector, its reverse Rev(·) is deﬁned using a combination of the above

rules.

Rev(

) = −

(2.105)

Rev(

) = (

)

−1

(2.106)

In general, if the pose is expressed in a coordinate system diﬀerent from the

one it is deﬁned with respect to, then we have

Rev(

) =



−

0 1





0 1



(2.107)

34 CHAPTER 2. GEOMETRY

which can then be expressed in another coordinate system {d} as discussed

previously. Note that the reverse of a pose IS NOT equal to the inverse of

the pose when it is expressed in a coordinate system diﬀerent from the one it

is deﬁned with respect to.

Rev(

) =

= (

)

−1

(2.108)

However, if a pose is expressed in the same coordinate system its deﬁned

with respect to (e.g.

), the reverse of a pose is given by its inverse

as in

(

)

−1



−

0 1





0 1



(2.109)

noting that the position vector is pre-multiplied by the rotation

to change

its coordinate system from {a} to {b} as the result of the inverse is expressed

in {b} while the original pose was expressed in {a}.

2.7 Forward Kinematics

Although kinematics is the subject of the next section, forward kinematics

describes the overall geometry of the robot such that the end-eﬀector position

can be computed if the joint actuations and link lengths are known. It can be

argued that forward kinematics (and its reverse operation, the inverse kine-

matics) is ill-named as it does not necessarily imply the the robot is moving

(kinema- means motion, coined by Amp`ere). An hypothesis for this naming

is that the kinematics parameters were studied in the context of moving rigid

bodies, and that forward kinematics uses the same parameters to describe po-

sitions. Also, the ﬁeld of statics studies forces between immobile rigid bodies,

so reusing that term would have led to ambiguities.

2.7.1 Product of Matrix Exponentials

Since prismatic and revolute joints can easily be described by unit screws, the

kinematic description of a robot can be deﬁned through a list of unit screws,

all expressed relative to the world/base frame. Importantly, all unit screws

are deﬁned when the robot is in its zero conﬁguration (i.e. all joints are set to

zero).

For prismatic joints, the screw axis ω is set to zero and the v is set to the

unit axis of the base frame along which the motion takes place as deﬁned in

2.7. FORWARD KINEMATICS 35

(2.93). For instance, a prismatic joint moving in the direction ˆz of the base

frame is noted

S =



v 0





0 0 1 0 0 0



(2.110)

where the unit screw S is deﬁned from the twist components v and ω, which is

always set to zero for strictly prismatic joints. A revolute joint moving about

an axis that is oriented mid-way between the ˆx and ˆy axes of the robot base

frame, and which is crossing the origin of the base frame, is noted

S =



−ω × q ω





0 0 0 cos(π/4) sin(π/4) 0



(2.111)

where the screw pitch h in (2.93) is always zero for strictly revolute joints, and

where the screw point q can be set to any point on the screw axis (the origin

was use here to simplify).

A constant matrix M is deﬁned as

M =

θ=0

(2.112)

describes the pose of the end-eﬀector relative to the base frame when the robot

is in its zero conﬁguration. With the end-eﬀector and all joints deﬁned, the

pose of the end-eﬀector in any given conﬁguration θ is obtained with

(θ) = e

]

. . . e

]

M (2.113)

where the exponential map deﬁned in (2.95) is used to transform twists into

homogeneous transformations. The formula in (2.113) is called the product of

exponentials and is used to compute the forward kinematics of a robot deﬁned

through unit screws.

2.7.2 Denavit-Hartenberg Parameters

A succint description of the structure of a robot is useful to deﬁne the position

and velocity of the end-eﬀector as a function of the joint displacements (i.e.

forward kinematics). For robots built from rigid links, the pose of a given link

is fully determined by the lengths and angles of links and joints closer to the

base. Consequently, the pose of any link can be determined by a sequence

of homogeneous transformations, each describing the reference frame of a link

with respect to the one of the previous link.

The Denavit-Hartenberg (DH) convention is a minimal parametrization

that describes each homogeneous transformation using only four values. The

authors of the DH convention proved that there is no parametrization with

fewer parameters. Although the pose of a raference frame has six degrees of

36 CHAPTER 2. GEOMETRY

ˆz

i−1

ˆx

i−1

ˆz

ˆx

ˆz

i+1

Figure 2.6: Proximal modiﬁed DH parametrization.

freedom, and hence would need at least six values to be deﬁned, the DH con-

vention introduces two constraints that must be respected by every reference

frame deﬁning the robot structure.

In its original deﬁnition, the DH notation led to ambiguities when used to

describe closed-loop and tree-like robots. A modiﬁed version of the convention

(now called Modiﬁed DH ) was introduced by Khalil and Kleinﬁnger to avoid

ambiguities of the original formulation. Two variants emanated from the mod-

iﬁed DH notation, the distal variant and the proximal variant. With the distal

variant, the i-th joint is at the distal end of link i, farther from the root/base

of the robot — it is the most popular version. The proximal variant (pictured

in Fig. 2.6) slightly simpliﬁes the notation by having the i joint be closer to the

base of the robot — Lipkin argues in A Note on Denavit-Hartenberg Notation

in Robotics that it is the best DH convention. In this document, we deﬁne

the proximal variant only, and will refer to this variant as DH to simplify the

text.

Each joint in the DH convention can be either prismatic or rotary but more

complicated joints (e.g. spherical, helical) can be easily modeled by superim-

posing joints. For instance, a helical joint can be modeled as a prismatic joint

superimposed atop a rotary joint. Each joint is speciﬁed relative to the previ-

ous joint by two parameters related to the previous joints (a

i−1

and α

i−1

), and

two parameters related to the joint itself (d

and ϕ

). The following constraints

2.7. FORWARD KINEMATICS 37

must be respected for any frame deﬁning a joint:

• the ˆz

axis must coincide with the i-th joint actuation axis and direction,

• the ˆx

i−1

axis must be perpendicular to ˆz

The joint actuation axis is the displacement vector axis along which a prismatic

joint slides, or about which a rotary joint revolves.

Therefore, the origin of the i-th joint can be localized by intersection the

ˆx

i−1

axis with ˆz

. The ˆy

axis is then determined from ˆz

and ˆx

by following

the right-hand rule. This procedure implies that the ˆx

and ˆy

axes depends

on the deﬁnition of the following joint. Hence, the end-eﬀector frame cannot

be deﬁned arbitrarily as its ˆx axis must be perpendicular to the last joint’s ˆz

axis.

Once all frames have been correctly deﬁned, the four parameters of each

joint can be extracted, and optionally an additionnal set of parameters for the

base frame and the end-eﬀector frame. The parameters are deﬁned as such,

with their order deﬁning the order of operations:

1. a

i−1

is the distance along ˆx

i−1

between ˆz

i−1

and ˆz

2. α

i−1

is the angle about ˆx

i−1

that would bring ˆz

i−1

to ˆz

if they shared

their origin,

3. d

is the distance along ˆz

between the intersection with ˆx

i−1

and the

origin of the frame, equal to zero for a revolute joint and variable for a

prismatic joint,

4. ϕ

is the angle about ˆz

that would bring ˆx

i−1

to ˆx

if they shared their

origin, equal to zero for a prismatic joint and variable for a revolute joint.

For a prismatic joint, d

will be given by the (variable) actuation length, to

which is possibly added an oﬀset. Similarly, for revolute joints, ϕ

will be given

by the actuation angle that is also possibly oﬀset.

In the case that ˆz

i−1

and ˆz

intersects, then they span a plane, and the

ˆx

i−1

axis is deﬁned as one of the two possible axes perpendicular to the plane.

If ˆz

i−1

and ˆz

are parallel, then there are an inﬁnite number of possibilities for

ˆx

i−1

and any possibility can be selected.

The homogeneous transformation between adjacent frames is built from a

composition of active transformations with one of the two rightmost transfor-

38 CHAPTER 2. GEOMETRY

mation being dependant on the i-th joint actuation.

i−1



3×3

i−1

ˆx

i−1

1×3



ˆx

i−1

(α

i−1

) 0

3×1

1×3



3×3

ˆz

1×3



ˆz

(ϕ

) 0

3×1

1×3



(2.114)







cos(ϕ

) −sin(ϕ

) 0 a

i−1

sin(ϕ

) cos(α

i−1

) cos(ϕ

) cos(α

i−1

) −sin(α

i−1

) −d

sin(α

i−1

)

sin(ϕ

) sin(α

i−1

) cos(ϕ

) sin(α

i−1

) cos(α

i−1

) d

cos(α

i−1

)

0 0 0 0 1







(2.115)

2.8 Inverse Kinematics

The inverse kinematics problem aims to ﬁnd all combinations of joint angles

that results in the end-eﬀector being in a desired pose. In 3D, robots with less

than 6 degrees of freedom (DoF) are called under-actuated, and those with

more than 6 DoF are over-actuated. The inverse kinematics of under-actuated

robots will yield no solution for any desired end-eﬀector pose that cannot

possibly be reached due to actuation limitations. Even for robots with 6 or

more DoF, not all poses can be reached due to link lengths, joint limits, and

self-collisions. However, inverse kinematics does not integrate such constraints

(except the link lengths which are implicit), and the solutions found must be

ranked using an adequate criteria to select the best one.

Over-actuated robots will always have an inﬁnite number of solutions to

the inverse kinematics problem, and a selection criteria must be implemented

via a numerical method.

For simple kinematic structures, analytical close-form solutions, which are

the fastest to evaluate, can be found. However, for more complex kinematic

structures, an iterative numerical approach must be used instead.

2.8.1 Analytic Methods

With the desired pose of the end-eﬀector relative to the base of the robot given

the serial kinematic chain is closed and becomes a loop. In such a loop, the pose

of any frame relative to any other frame can be deﬁned either by composing

homogeneous transformations forward or backward in the loop. For instance,

2.8. INVERSE KINEMATICS 39

for a 6-DoF robot, we have

3 3

1 1

w w

ee 6

5 5

(2.116)

where (2.116) represents a system of 12 equations (as there are twelve parame-

ters in a 4×4 homogeneous transformation) with 6 unknowns (the joint angles,

one per transform). Many such systems of equations can be generated for the

robot, yielding a great number of equations. The idea is to ﬁnd a sub-system

of equations that is can be solved for a sub-set of the unknowns. From there,

other equations can be simpliﬁed by considering the previously identiﬁed sub-

set of unknowns, which is now known. Such a procedure can yield analytic

equations that are very fast to evaluate for a given desired pose.

In the speciﬁc case of 6 DoF robots with a spherical wrist, the 3-2-1 kine-

matic structure can be exploited with Pieper’s technique to greatly simplify

the derivation of solutions. With such a kinematic structure, the reference

frame of the three joints farthest from the base can be deﬁned such that they

share their origin, equivalent to a pure spherical joint. The location of this

origin is given by the position vector in

ee ee

(2.117)

which is

(2.118)

and is equal to

. Since

is ﬁxed,

can be

directly computed from the end-eﬀector position. A system of three equa-

tions can be built relating

to the three joint angles closest to the base

of the robot. This system being much smaller than the original, it follows

that ﬁnding a solution is much easier. Once equations for the ﬁrst three joint

angles are determined, a second system of equations containing joint angles

for the three other joints can be deﬁned. In sum, Pieper’s technique enables

the decomposition of a harder problem into two simpler ones.

2.8.2 Numerical Methods

For many systems, analytical solutions to the inverse kinematics problem do

not exist, and numerical methods that iteratively evaluate potential solutions

are needed. One such method makes use of the inverse Jacobian, which maps

task-space velocities to joint-space velocities. The idea is that by setting the

task-space velocities to be proportional to the distance between current pose

and the desired pose, the inverse Jacobian can provide the joint-space velocities

that must be followed to take a step in the approximate direction of the desired

40 CHAPTER 2. GEOMETRY

pose. This is equivalent to a gradient descent method since the Jacobian is

eﬀectively a matrix of ﬁrst-order partial derivatives. As any gradient descent

method, if the initial point is to far from the desired point, odds are that

the algorithm will be unable to converge to the desired point. Consequently,

the algorithm must start from a conﬁguration in which the end-eﬀector pose

is known, and the algorithm is iterated to reach an end-eﬀector pose that is

slightly closer to the desired pose. In other words, there is an outer loop that

progressively move the target pose toward the desired pose, and there is an

inner loop, which must run signiﬁcantly more often, that ﬁnds joint angles

reaching the current target.

The task-space velocity twist can be obtained through the matrix logarithm

of the pose diﬀerence

= Ad (

) log (

w w

) (2.119)

where

is the current target pose. Then, the velocity is followed for a unit

step size with

i+1

= q

+ J

(q)

(2.120)

where q

i+1

is the joint angles that should bring the robot closer to the goal.

The inner loop can be stopped when the updates are small enough, at which

point the outer loop can modify the target pose to be closer to the desired

end-eﬀector pose.

Other numerical techniques can make use of the inertial information to

minimize the energy expenditure, or can be modiﬁed to take joint limits into

account. For instance, a Levenberg-Marquadt scheme can be used to introduce

an adaptive damping that make sure that the pseudo-inverse is always far from

singular.

2.9 Key Concepts

• A vector represents the location of a point relative to another one in

some vector space.

• A Cartesian reference frame consists in three orthogonal unit vectors

that form basis directions.

• A coordinate system is needed to express the vector as a sequence of

numbers. A coordinate system is a reference frame that is associated

with a physical quantity (e.g., meters or km/h).

• Rotation matrices are orthonormal matrices whose columns describe ba-

sis directions relative to some reference frame.

2.9. KEY CONCEPTS 41

• Rotations are composed through matrix multiplication, and the inverse

of a rotation is given by its transpose.

• When composed through post-multiplication, the rotation is performed

relative to a ﬁxed reference frame, while pre-multiplication performs the

rotation relative to a moving reference frame.

• A sequence of multiple rotations can be expressed as a single rotation

about some axis via the axis-angle representation. The axis-angle repre-

sentation is minimal (only three parameters) but posess a singularity at

identity.

• The matrix logarithm of a rotation matrix yields the axis-angle represen-

tation, and the matrix exponential of an axis-angle representation yields

a rotation matrix.

• Unit quaternions encode rotation axis and magnitude in a four-dimensional

vector space to avoid the singularity of the axis-angle representation, but

cover all rotations twice.

• A pose is a homogeneous transformation that describes the position and

orientation of a reference frame relative to another one.

• A pose can be expressed in a diﬀerent coordinate system by pre-multiplying

it by a rotation matrix.

• Any rigid transformation can be expressed as a twist: the motion about

a screw axis.

• The twist can be represented as a 6D vector, combining linear and an-

gular components.

• A screw axis has a unit axis, a thread pitch, and a screw point.

• The screw pitch is the ratio of the linear and angular components, while it

is zero for purely rotational motions, it is inﬁnite for purely translational

motions.

• The adjoint of a rigid transformation is a 6 × 6 matrix that can be used

to express a twist in a diﬀerent coordinate system.

• The forward kinematics equations deﬁne the pose of the end-eﬀector as

a function of the robot geometry and the joint actuation.

42 CHAPTER 2. GEOMETRY

• The forward kinematics of a serial robot can be expressed as a product

of matrix exponentials, each representing the end-eﬀector motion that is

due to a joint actuation.

• The Denavit-Hartenberg convention is a minimal parametrization (four

per joint) that can be used to describe the kinematic structure of a robot.

Several variations exist.

• The inverse kinematics problem aims to ﬁnd the joint angles that result

in a desired end-eﬀector pose for a given robot.

• While the inverse kinematics of simple robots can be solved analytically,

more complex robots require numerical methods to ﬁnd a solution.

Chapter 3

Kinematics

As mentioned in Sec. 2.1, position is a Euclidean vector. An important prop-

erty of vectors is that their derivatives are also vectors, implying that velocity

and acceleration are vectors, which will be referred to as

and

re-

spectively — the kinematic of {a} observed from {b} expressed in the {c}

coordinate system.

In a 3D world, position (and therefore its derivatives) is described by a 3D

vector. However, as a body can translate and rotate in space, the kinematics

is characterised by linear and rotational or angular components — while a

point only has a position, a rigid body also has an orientation.

However, the linear and angular components are only intermediates used

to compute the real kinematics of all points on the rigid body. In general,

all points on a rigid body can have a diﬀerent kinematic but they all share the

same description of the kinematic. In other words, the velocity of all points

on a body is described with the same linear and angular components but the

velocity evaluated at each point can diﬀer from one point to another.

Velocity and acceleration are commonly described using six-dimensional

(6D) vectors, called spatial vectors or twists









, (3.1)

which describe screwing motions along a directed line as deﬁned in (2.92).

Importantly, 6D spatial vectors are not merely a stack of two 3D vectors but

really are elements of a six-dimensional vector space.

44 CHAPTER 3. KINEMATICS

3.1 Velocity

Velocity is the time-derivative of the position vector. Since a body is com-

posed from a multitude of points, each point on a rigid-body has a diﬀerent

velocity in general. The description of the body’s kinematics through linear

and angular components, as described in Sec. 3.3.1, can be used to succintly

describe the velocity of all points on the body. While the linear components v

describes the instantaneous ﬂow of points passing through an origin, the an-

gular components ω describes the instantaneous axis of rotation about which

the body is revolving. One must be careful not to confuse the linear velocity

v of a body with the real velocity of its origin

p, although they can be equal

in some circumstances (for instance if ω = 0). In general, linear velocities

cannot simply be pre-multiplied by a rotation matrix to express it in

another reference frame.

The linear and angular components can be combined with

p = v + ω × p (3.2)

to obtain the real velocity of any point on the body.

Since the axis of rotation ω does not depend on the linear velocity, it can be

easily expressed in any frame through a pre-multiplication by an adequate ro-

tation matrix. Also, as any vector, angular veocities can be composed through

vector addition. For instance, considering the scenario depicted in Fig. 3.1,

the angular velocity of {o} is given by

b b

(3.3)

where

is the angular velocity of {b} relative to {w} and expressed in {w}.

3.1.1 Point on a Rotating Body

The real velocity and acceleration of a point-mass in space is really what

matters in many dynamics equations. The velocity of the origin of {o} in

Fig. 3.1, where {b} moves (linearly and rotationaly) with respect to {w}, is

computed with

(3.4)

(3.5)

(3.6)

(3.7)

(3.8)

(3.9)

3.2. ROTATION TIME DERIVATIVE 45

{w}

{b}

{o}

= ?

Figure 3.1: The kinematic relations shown with the red arrows are known and

the one shown with the blue arrow is computed.

where the identity

R (a × b) = (Ra) × (Rb) (3.10)

can be used. The rightmost term in (3.8) corresponds to the velocity contri-

bution due to the rotation of {b} about {w}.

3.2 Rotation Time Derivative

As derived in appendix D, the total time derivative of a rotating vector is

(

) =

(3.11)

where

is the instantaneous angular velocity of {b} with respect to {w}

and expressed in the inertial/ﬁxed frame {w}. The derivative of a rotation

matrix is

(3.12)

which is an important result.

46 CHAPTER 3. KINEMATICS

3.3 Velocity Twists

3.3.1 Interpretation

The spatial vectors describing the velocity of a body are usually either ex-

pressed in a moving/body frame F

or in a ﬁxed/inertial frame F

as pic-

tured in Fig. 3.2. When the velocity is expressed in F

, the linear components

describe the real velocity of the origin and the angular components describe

how the real velocity of the other points in the rigid body can be obtained.

However, when the velocity is expressed in F

, the linear velocity of a body

can be thought as a measure of the ﬂow of points passing through the origin

(pretending that the body is large enough) as the body moves, which is not

necessarily equal to the velocity of the F

’s origin. Indeed, the origins of both

frames will not usually overlap, and therefore their velocity will be diﬀerent in

general.

Figure 3.2: Interpretation of the two velocity twists describing the kinemat-

ics of the revolving link. The smaller circle passes through the body frame

while the larger one passes through the world frame. The velocity of a point

travelling along the smaller circle and being instantaneously coincident with

corresponds to the linear components of the velocity twist expressed in

the body frame. Conversely, the velocity of a point travelling along the larger

circle and being instantaneously coincident with F

corresponds to the linear

components of the velocity twist expressed in the world frame.

The velocity twist might be easier to interpret when expressed in the body

frame, but expressing the velocity twist in the world frame might be useful for

dynamics. In section Sec. 3.3.3, a relation that enables changing the frame in

which a velocity twist is expressed in will be highlighted.

3.4. ACCELERATION 47

3.3.2 Point on a Rotating Body

The real velocity of a point {o} that is ﬁxed in F

can be obtained from the

velocity twist via



3×3

−[

]



(3.13)



3×3

−[

]







(3.14)

(3.15)

3.3.3 Coordinate System Change

Since the velocity can be expressed as a twist (from (2.92)), the adjoint repre-

sentation of an homogeneous transformation deﬁned in (2.99) can be used as

done in (2.101) to change the coordinate system the velocity is expressed in

with





= Ad (

)



[

]

3×3 w



(3.16)

where

is the homogeneous transformation between the two coordinate

frames.

3.3.4 Observation Point Change

Velocities are observed from a point of view, and it is often useful to know

what would be the result of the observation from another point of view. This

change of observation point can be performed on twists with

+ Ad (

)

(3.17)

where the adjoint transformation from (2.101) was used to express all twists

in {w}. When comparing the above equation to the ones in Sec. 3.1.1, clearly

the use of velocity twists becomes apparent as the equations are much more

succint and less error-prone.

3.4 Acceleration

Acceleration is the time-derivative of the velocity vector, and it can be de-

scribed through linear and angular components, similarly to velocity vectors.

48 CHAPTER 3. KINEMATICS

To obtain an expression for the angular acceleration of {o} in the situation

depicted in Fig. 3.1, equation (3.3) is diﬀerentiated with respect to time

(

) (3.18)

(

b b

) (3.19)

b b

× (

b b

) (3.20)

where the derivative of a rotated vector from (3.11) was used.

The above equation reveals that, in contrast to how angular velocities are

composed with (3.3), an additional velocity-dependant term in the rightmost

term of (3.20) appeared.

3.4.1 Point on a Rotating Body

Given the situation depicted in Fig. 3.1, the velocity of {o} is given by the

superposition of three terms

(3.21)

(3.22)

that are respectively caused by the linear velocity of {b} relative to {w}, an-

gular velocity of {b} relative to {w}, and real velocity of {o} relative to {b}.

Diﬀerentiating 3.22, we obtain

(

) =

(

) +

(

) +

(

) (3.23)

(

) (3.24)



(

) ×





(

)



(3.25)

and using the result from (3.11) to solve the derivatives

b b

(3.26)

× (

) (3.27)

× (2

) +

(3.28)

+ 2

× (

) +

(3.29)

3.5. ACCELERATION TWISTS 49

which produces the important relation between

and

, the acceleration

of a body {o} in a rotating frame {b} to the one in a ﬁxed frame {w}, as

+ 2

× (

) +

(3.30)

−

− 2

−





−

(3.31)

when all vectors are expressed in the same frame.

3.5 Acceleration Twists

The equation in (3.30) deﬁnes the acceleration of all points on a rotating body.

However, the equation contains velocity-dependant terms, and therefore does

not deﬁne a helicoidal ﬁeld that could be parametrized with a screw.

In contrast, the acceleration twist deﬁned as

λ =









(3.32)

deﬁnes a helicoidal vector ﬁeld that can be parametrized with a screw axis.

As a true vector, the acceleration twist enjoys the properties of vectors, like

the one of vector addition such that

λ =

λ +

λ (3.33)

Similarly to how the components of the velocity twists are combined in

(3.13), the acceleration of a point on a rotating body can be easily obtained

with



3×3

−[

]



(3.34)



3×3

−[

]







(3.35)

(3.36)

where a and α are respectively the linear and angular components of the accel-

eration twist. Note that although the symbol used to describe the components

of the acceleration twist are the same as those used to describe classical accel-

eration, those quantities are not equal in general.

The linear components of the acceleration twist can be intepreted as being

the rate of the ﬂow of points going through the origin as pictured in Fig. 3.2,

which is not necessarily equal to the acceleration of the origin. For a body

rotating at a constant velocity, the rate of the ﬂow of points passing through

the origin will be equal to zero, although most points are accelerating as their

velocity vector is constantly changing direction.

50 CHAPTER 3. KINEMATICS

3.6 Key Concepts

• In general, the velocities of all points on a rigid body are diﬀerent.

• Decomposing a rigid body velocity into linear and angular components

is useful to succintly describe the kinematics of all points on the body.

• The linear velocity describes the instantaneous ﬂow of points passing

through the origin, while the angular velocity describes the instantaneous

axis of rotation about which the body is revolving.

• The linear and angular components can be combined to obtain the real

velocity of any point on the body.

• A velocity twist is a six-dimensional vector consisting of the linear and

angular components of the velocity. It inherits the properties of vectors

and of twists.

• A velocity twist can be expressed in a diﬀerent coordinate system by

pre-multiplying it by the adjoint of the transformation between the two

coordinate systems.

• Similar to velocity, the acceleration of a rigid body can be decomposed

into its linear and angular components.

Chapter 4

Rigid Body Dynamics

Dynamics, also referred to as kinetics in older book, studies how forces inﬂu-

ence the motion of a body.

4.1 Inertial Frame of Reference

Imagine you are on a long highway in Quito (Ecuador), which is nearly ex-

actly on the equator. You are driving at a constant speed, and your perception

of the road is that it is perfectly straight. You know that the gravitational

acceleration is 9.81 m/s

, however, the accelerometer in your smartphone pre-

tends that it is in fact 9.78 m/s

instead. What could possibly explain this

discrepancy?

The skewed perception of the accelerometer is due to the fact that, as you

drive, you are in fact rotating about the axis of the earth. This revolute mo-

tion implies that your velocity vector is constantly changing direction, which

induces a rotational acceleration about the axis of the earth. The error in the

accelerometer’s estimate was due to the (incorrect) assumption that you were

not accelerating, resulting in a discrepancy between the observed kinematics

and the measured forces.

A ubiquitous term in dynamics is inertial frame, which designate a refer-

ence frame that does not accelerate. In such a frame, Newton’s laws are valid,

since the inertial reference frame does not rotate about any axis. Indeed, ro-

tating implies that the velocity direction changes over time, and therefore that

the frame accelerates (even if the magnitude of the velocity does not change).

The cosmological principle states that the same phenomenon can be ob-

served independently of the observer’s location in the universe and indepen-

52 CHAPTER 4. RIGID BODY DYNAMICS

dently of the direction of observation. According to this principle, the universe

would be isotropic and homogeneous, and hence would not have any center. In

other words, the cosmological principle states that any phenomena observed in

an inertial frame can be equivalently observed in any other inertial reference

frame. Consequently, it does not matter in which inertial reference frame the

rigid object dynamics is observed in, all are equivalent.

The obvious question becomes the one of ﬁnding what is the inertial frame

of our universe. It turns out that this is a very complicated question but that

a weighted average of the heaviest quasars’ (the heaviest stars) velocities could

give us an approximate location. This is the basis of the International Celestial

Reference System whose origin is located at the barycenter of our solar system

and whose axes are such that very massive and very far celestial bodies do not

appear to rotate. In the context of robotics, any frame in which Newton’s laws

are suﬃciently accurate can be considered as an inertial frame. In practice,

we can almost always consider that a frame ﬁxed somewhere in the workspace

of the robot is an inertial frame, which we will refer to as the world frame F

4.2 Moments in Dynamics

In mathematics, the Nth moment of a function f(x) is a characteristic deﬁned

∞

−∞

f(x)dx, (4.1)

and is determined by the shape of f(x). In physics, the moment of something

usually refer to the ﬁrst moment, which involves the product of a distance and

the physical quantity. The moment of a physical quantity is formally deﬁned

q, (4.2)

where q is a physical quantity observed in an inertial frame and

is a po-

sition relative to a local frame about which the moment is computed. It is

particularly important to note that q is observed in an inertial frame. Since

many fundamental physical quantities in dynamics (including mass) vary de-

pending on the frame in which they are observed, several equations will only be

applicable when quantities are related through a common ground, the inertial

frame.

Three moments are central to dynamics: the moment of force, the moment

of momentum and the moment of inertia. The moment of force, also called

torque, is deﬁned as

, (4.3)

4.3. MOMENTS OF A MASS DISTRIBUTION 53

where

is the force on {a} observed in the inertial frame F

and expressed

in the local frame F

. For a point mass i located at

relative to the local

frame F

, the moment of mass is deﬁned as

, (4.4)

where the mass is measured in the inertial frame F

. Surprisingly, the mea-

sured mass of a body varies depending on the observer’s velocity. Indeed, as

highlighted by the famous E = mc

equation, mass is tied to energy, and

therefore to the frame it is observed in. However, since the discrepancy be-

tween the measured mass and the mass observed in the inertial frame is very

small (unless the observer is moving at a signiﬁcant fraction of the speed of

light), we will consider the mass to be independent of the frame it is measured

in (m ≡

m). The moment of momentum (also called angular momentum) is

deﬁned as

, (4.5)

where

is the momentum (to be deﬁned in Sec. 4.4) of {a} observed in the

inertial frame F

and expressed in the local frame F

4.3 Moments of a Mass Distribution

In robotics, we are often interested in the dynamics of objects that we model as

rigid bodies: bodies for which the relative position between any two points does

not change over time. A rigid body is a mass distribution with volume V ⊂ R

and mass density function ρ(p) mapping any point in V to a nonnegative mass

density.

The Nth moment of a body’s mass distribution is determined by integrating

over the moments of all point masses making up the body, which can be

expressed as

[

]

dm, (4.6)

where the integral is over the mass distribution M of the body and

is the

position of the i-th point mass. Since the mass density of the rigid body might

not be homogeneous, a more general deﬁnition is given as

[

]

ρ(

)dV, (4.7)

where

is the position of the i-th point in the volume V of the body relative

to a frame F

ﬁxed on the body.

54 CHAPTER 4. RIGID BODY DYNAMICS

4.3.1 Zeroth Moment: Total Mass

The zeroth moment of a mass distribution is given by

[

]

ρ(

)dV = 1

3×3

ρ(

)dV

| {z }

= m1

3×3

, (4.8)

where m is the total mass of the body.

4.3.2 First Moment: Centre of Mass

The ﬁrst moment of a mass distribution is given by

[

]

ρ(

)dV =

ρ(

)dV

| {z }

= m [

]

, (4.9)

where the identity [u]

+ [v]

= [u + v]

was used, and where

is the

centre of mass of the body. For bodies of homogeneous mass density (that is,

ρ(p) = ρ ∀ p ∈ V for some ρ ∈ R

), the location of the centre of mass is given

by the geometrical centre, or centroid, of the body. The ﬁrst moment represents

the weighted location of the centre of mass and is the point about which

zero torque is exerted when the mass of the body is uniformly accelerated.

Consequently, the motion of a rigid body under uniform acceleration can be

equivalently described by a single point positioned at

with mass equals to

m. Also, the centre of mass being the point around which the body’s mass

is symmetrically distributed, any rigid body is easier to accelerate about its

centre of mass than about any other point.

4.3.3 Second Moment: Inertia Tensor

The second moment of the body’s mass distribution is given by

[

]

ρ(

)dV =

[

]

[

]

ρ(

)dV =

, (4.10)

which is the inertia matrix (also called inertia tensor ) of the body computed

relative to F

The inertia tensor I is a 3×3 symmetric and positive-deﬁnite matrix where

the I

element expresses how torque applied about axis e

will produce angular

acceleration around axis e

. Analogously, the I

element expresses how the

rotation about axis e

will produce angular momentum around axis e

4.3. MOMENTS OF A MASS DISTRIBUTION 55

The diagonal elements of I are called the moments of inertia while the

oﬀ-diagonal elements are termed products of inertia. When computed relative

to some frame F

, symmetries of the mass distribution about the axes of F

will make some products of inertia be zero. When I

= 0, no torque about

, however large it is, will produce any angular acceleration about e

. This is

crucial for racing cars; a rotation about the axle of an imbalanced wheel will

produce an angular momentum around another axis, making the wheel wobble.

Carefully balancing the mass distribution of the wheels reduces products of

inertia to values close to zero, which ultimately enables the car to sustain

greater accelerations.

For a frame F

whose origin is the same as F

but whose axes are oriented

with

relative to F

, the inertia matrix computed relative to F

is given

[

]

ρ(

)dV (4.11)

[

]

ρ(

)dV (4.12)

[

]

[

]

ρ(

)dV (4.13)

[

]

b a

| {z }

[

]

ρ(

)dV (4.14)

[

]

ρ(

)dV (4.15)

[

]

ρ(

)dV

| {z }

(4.16)

b b

b a

, (4.17)

where (4.14) makes use of the identity [Ru]

= R [u]

. The relation in

(4.17) deﬁnes how the inertia matrix changes when the coordinate system used

to express point positions is changed.

Given a frame F

whose origin is located at the centre of mass and whose

axes are aligned with the body frame F

, the inertia matrix computed relative

56 CHAPTER 4. RIGID BODY DYNAMICS

to F

is expressed as

[

]

ρ(

)dV (4.18)

[

]

ρ(

)dV (4.19)

[

]

ρ(

)dV +

[

]

ρ(

)dV

− [

]

[

]

ρ(

)dV

| {z }

−

[

]

ρ(

)dV

| {z }

[

]

(4.20)

= [

]

ρ(

)dV

| {z }

[

]

ρ(

)dV

| {z }

(4.21)

= m [

]

, (4.22)

where

is the inertia matrix of the body computed relative to the centre of

mass. The relation between

and

in (4.22) is known as Steiner’s theorem

or as the parallel axis theorem.

The equations in (4.22) and (4.17) can be combined to relate the iner-

tia tensor of a body computed relative to frames with diﬀerent origins and

orientations with



− m [

]

[

]



+ m [

]

[

]

(4.23)

where the position of the centre of mass relative to body frame F

is given by

There exists a set of axes, called the principal axes of inertia, about which

the mass distribution is symmetric. Such an inertia tensor

is obtained

when p in (4.10) is deﬁned relative to c and expressed in F

where c denotes

the centre of mass and F





denotes the principal orientation

of the mass distribution. The structure of

is particularly simple as its oﬀ-

diagonal elements are zero and its diagonal elements are strictly positive. Due

to its special meaning,

is the common denominator of all inertia tensor

expressions for a given body.

Computing the eigenvectors of any inertia matrix for a given body yields

the principal axes of inertia, with the eigenvalues being the principal moments

of inertia. The existence of principal axes of inertia has an important implica-

tion: the motion of a rigid body about its centre of mass is strictly determined

by the principal moments of inertia (in fact only their ratios really matters).

4.4. MOMENTUM 57

Consequently, from a dynamics perspective, any rigid body can be modeled as

an ellipsoid whose radii are given by the principal moments of inertia oriented

along the principal axes of inertia.

Finally, inertia is additive, meaning that the inertia of a body can be

computed as the sum of the inertia of all its constituent point-masses.

4.4 Momentum

Momentum is deﬁned as the product of inertia and velocity. For a point-mass,

the momentum is given by

= m

(4.24)

when observed from {b}, and expressed in the {c} coordinate system. The

momentum is a conserved quantity, which means that it does not change over

time in a closed system. Consequently, the momentum is a characteristic of

a system. From (4.41), it is apparent that the momentum inherits the vector

properties of the velocity. The total momentum of a system of point-masses

is therefore given by the sum of the momenta of all the point-masses. For a

body whose volume is V

⊂ R

and whose mass density is given by ρ(p

), the

momentum of the body is given by

ρ(p

)dV (4.25)

(

) ρ(p

)dV (4.26)

ρ(p

)dV +

ρ(p

)dV (4.27)

ρ(p

)dV

| {z }

ρ(p

)dV (4.28)

= m

+ [

]

ρ(p

)dV

| {z }

(4.29)

= m

= m (

) (4.30)

where the integral in (4.29) equals zero by the deﬁnition of the centre of mass

(c × ma = 0).

58 CHAPTER 4. RIGID BODY DYNAMICS

4.4.1 Moment of Momentum

The moment of momentum of a point mass i in a rigid body whose local frame

is F

and that is rotating about inertial frame F

is deﬁned as

, (4.31)

where

is the position of the point mass relative to the local frame F

. Like

the momentum, the moment of momentum is a conserved quantity, making it

a characteristic of interest and the basis upon which the equations of motion

for a rotating body will be derived in Sec. 4.7.3.

Assuming that the body moves about inertial frame F

, the moment of

momentum computed relative to F

is given by

ρ(p

)dV (4.32)





ρ(p

)dV (4.33)

ρ(p

)dV +





ρ(p

)dV (4.34)

ρ(p

)dV ×

−

ρ(p

)dV (4.35)

ρ(p

)dV

| {z }

[

]

[

]

ρ(p

)dV

| {z }

(4.36)

= m

(4.37)

where the deﬁnition of the centre of mass and inertia matrix were used in

(4.36). The ﬁrst term in the right-hand side of (4.37) represents the moment

of momentum that is due to the motion of the centre of mass, while the second

term represents the moment of momentum that is due to the motion of the

body about F

. The expression in (4.37) is part of K¨onig’s theorem. There

is a signiﬁcant source of confusion in the naming convention used to describe

the terms in (4.37). Indeed, either

b w

are widely termed angular

momentum. The ambiguity comes from the fact that, when considering a

revolving body with no translational motion (e.g., a spinning top), ε = Iω.

We will follow the advice of Peter Hughes and Roy Featherstone, and name

real angular momentum or real moment of momentum the left-hand side of

(4.37) and intrinsic angular momentum the rightmost term in (4.37).

4.4. MOMENTUM 59

The equation in (4.37) expresses the momentum in the body frame and

requires that the motion of the body be also expressed in F

. It is usually

more practical to express the motion of the body in the inertial frame F

with





(4.38)

= m

b b

b w

| {z }

(4.39)

= m

b b

b w

, (4.40)

in which

and

are expressed in the world frame. The equations for the

linear and angular momentum expressed in F

, on which stands the equations

of motion, are given by

= m

= m (

) (4.41)

= m

b b

b w

, (4.42)

where F

, F

, and F

are respectively the centre of mass frame, the body

frame, and the world frame that is assumed to be an inertial frame.

4.4.2 With Body-Frame Along Principal Axes

The simplest expression for the intrinsic momentum is produced when a ref-

erence frame F

is ﬁxed at the centre of mass of the body and aligned with

the principal axes of inertia. In such a case, the inertia matrix is diagonal and

each component of the intrinsic angular momentum is given by

(4.43)





0 0













(4.44)









, (4.45)

where the angular momentum about some axis only depends on the angular

velocity about this same axis.

60 CHAPTER 4. RIGID BODY DYNAMICS

4.5 Energies, Work, and Power

Energy, whose unit is the Joule, is a conserved quantity – a closed system

will conserve its energy over time. As Maxwell remarks, the absolute value of

energy in a system is unknown and unimportant as “all phenomena depend on

the variations of energy and not on its absolute value”.

4.5.1 Kinetic Energy

The kinetic energy of a point mass {i} whose mass is m

is given by integrating

its momentum over the velocity with

p =

, (4.46)

where

is the real velocity of the point mass observed and expressed in F

For a rigid body, as depicted in Fig. 4.1, the kinetic energy is given by

i w

ρ(p

)dV (4.47)

(

)

(

) ρ(p

)dV (4.48)

b w

+ 2

(

)

+ (

)

(

)

| {z }

=ω

[p]

ρ(p

)dV (4.49)



+ 2

[

]

ρ(p

)dV

| {z }

[

]

ρ(p

)dV

| {z }



(4.50)

| {z }

linear

+ m

[

]

b b

b w

| {z }

rotational

, (4.51)

in which the middle term cancels if F

is at the centre of mass such that

= 0. In such a case, the kinetic energy is reduced to the linear and

rotational terms in (4.51). In (4.49), the rightmost term in the integral was

4.5. ENERGIES, WORK, AND POWER 61

rearranged with

(

)

(

) = ([

]

)

([

]

) (4.52)

= ([

]

)

([

]

) (4.53)

[

]

[

]

(4.54)

[

]

. (4.55)

In (4.50), the deﬁnitions of the mass, centre of mass, and inertia matrix were

used to resolve the integral. As a reminder,

m =

ρ(p

)dV (4.56)

ρ(p

)dV (4.57)

[

]

ρ(p

)dV . (4.58)

Figure 4.1: Reference frame F

is ﬁxed on the rigid body, F

is the inertial

frame, and is the centre of mass.

4.5.2 Potential Gravitational Energy

The potential gravitational energy is the amount of kinetic energy that would

appear if a mass was to fall from a given point to the lowest potential energy

level of the system. Close to the earth, the potential gravitational energy of

mass m is

U = m ∥g∥h (4.59)

where h is the height of the centre of mass above the reference ground, and

the gravitational acceleration is g = [0, 0, −9.81]

m/s

62 CHAPTER 4. RIGID BODY DYNAMICS

4.5.3 Work

Work is done when applying a force on a mass along a displacement. Doing so

changes the kinetic and/or potential energy of the mass. Therefore, the units

of work are Joules (scalar).

W =

f(s) · dr (4.60)

4.5.4 Power

Power is the amount of energy transferred per unit of time. Its units are

Joules per second or Watts. Power can also represent the product of force

with velocity and is the time derivative of work.

P =

∆W

∆t

(4.61)

P = f ·

p (4.62)

4.6 Spatial Inertia Matrix

The spatial inertia matrix, also called pseudo-inertia or system inertia matrix,

can be used to relate momenta and velocities, expressed as the 6-dimensional

vectors





(4.63)





(4.64)

built from the usual 3-dimensional vectors. For a rigid body, the spatial inertia

matrix can be deﬁned as

I =



3×3

−m [c]

m [c]



(4.65)

which is, like the inertia matrix, symmetric and positive-deﬁnite. The matrices

in (4.65) and in (4.63) can be related with

E = Iν (4.66)

and with

K =

Iν (4.67)

4.7. NEWTONIAN MECHANICS 63

provided that the vectors are all expressed in the same reference frame. In

(4.67), the kinetic energy is related to the spatial velocity through the spatial

inertia, and since the kinetic energy must be positive, then the spatial inertia

matrix must be symmetric positive-deﬁnite given any non-zero velocity. Note

that for more complicated systems, a system inertia matrix diﬀerent from

(4.65) can be determined such that the relations in (4.66) and (4.67) still

stand.

4.7 Newtonian Mechanics

Newton’s laws of motion were initially deﬁned for point-masses and Euler pro-

posed formulations that are well-suited to rigid bodies. Newtonian Mechanics

usually refer to the combination of Newton’s and Euler’s laws of motion with

the socalled Newton-Euler equations.

4.7.1 Force

Newton’s ﬁrst law of motion introduces the concept of inertia and states that

a body will stay at rest unless a force is acted upon it. With the concept of

momentum in mind, the ﬁrst law leads nicely to Newton’s second law

f =

(4.68)

that states that a force acting on a body will change the rate of its momentum.

The ﬁrst two laws thereby imply that a body will conserve its momentum.

Newton’s third law stems from the idea of conservation of momentum and

states that two interacting bodies will be subject to opposed forces—a two-

body system conserving its momentum has to have internal forces that cancel

each other.

The expression in (4.68) deﬁnes a force as a physical phenomenon aﬀecting

the momentum of a body. Assuming that the mass of the body is constant, a

force acting on a body will change the direction or the magnitude of the body’s

velocity. If the body is only accelerating in a straight line, the force required

to produce this acceleration is given by the simplest form of Newton’s second

law,

) = m

, (4.69)

where F

is a frame ﬁxed in the accelerating body, F

is the inertial frame,

and F

is a frame at the centre of mass of the body with axes aligned with

. In general, when the body frame F

is accelerating relative to the inertial

64 CHAPTER 4. RIGID BODY DYNAMICS

frame F

, through a change of direction or through a linear acceleration, the

expression for the momentum of a body (as derived in (4.30)) is given by

= m (

) . (4.70)

The rate of change of the momentum is then given by

(m (

)) (4.71)

= m



(

) +

(

) +

(

)



, (4.72)

and using d/dt(u × v) = du/dt × v + u × dv/dt,

= m



(

) +

(

)



(4.73)

and using d/dt(Rv) = R ˙v + ω × Rv,

= m

× (

)



(

) +

× (

)



, (4.74)

= m



× (

)



, (4.75)

and since

= m (

+ 2

) . (4.76)

The result in (4.76) can be validated by comparing it to the expression in

(3.30) for the acceleration of a point in a rotating frame. Indeed, starting with

Newton’s second law

= m

(4.77)

and using (3.30) with {o} = {c}

+ 2

× (

) +

(4.78)

yields

= m

| {z }

+ m (

+ 2

) . (4.79)

4.7. NEWTONIAN MECHANICS 65

as an extension of Newton’s second law to rotating bodies.

Assuming that

= m (

+ 2

× (

) +

) (4.80)

= m (

) (4.81)

+ m (2

) (4.82)

+ m (

× (

)) (4.83)

+ m (

) (4.84)

is the force that would be measured by a sensor located at F

. The term in

(4.81) is Newton’s Second Law relating the linear acceleration of a body to

the force it experiences. The additional terms on the right-hand side of (4.80)

are called ﬁctitious forces as they are not due to a real physical force (i.e.

gravitational, electromagnetic, or nuclear). The term in (4.82) is the Coriolis

force, while the term in (4.83) is the centrifugal force, and (4.84) is called

Euler’s force. Fictitious forces are added to correct the skewed perception of

kinematics in the rotating frame and reconcile the requirement that the sum

of all forces on a body must be zero (i.e., forces sensed must be explained by a

velocity change). In other words, ﬁctitious forces compensate for what is not

“seen” by a force sensor located at F

due to its own motion. The classical

example if the one of a person in a carousel. As the carousel spins about its

axis, the person has no linear velocity or acceleration. Nonetheless, the person

will feel a force pushing them outwards from the centre of the carousel: the

centrifugal force. Likewise, a force sensor held by the person will measure an

outward force. However, once the carousel stops spinning, the person will not

be feeling the outward force anymore, as if it had vanished. A real force would

not appear or vanish depending on the motion of the sensor, a true force either

exists or does not. Fictitious forces are therefore not real forces, but rather

form an adjustment enabling the sensor to take its own motion into account.

In robotics, ﬁctitious forces are crucial to the identiﬁcation of a manipu-

lated object’s inertial parameters (i.e., mass, centre of mass, and inertia ma-

trix). Indeed, when a robot manipulates an object, the forces and torques

measured by the robot are inﬂuenced by the motion of the sensor. However,

the goal of the operation is to identify inertial parameters that are independent

of the sensor’s motion such that these parameters can be used in a variety of

applications. Hence, when identifying inertial parameters, ﬁctitious forces will

be used to correct the skewed perception of the sensor and produce estimates

that are independent of the sensor’s motion.

66 CHAPTER 4. RIGID BODY DYNAMICS

4.7.2 Torque

Just as the concept of force was deﬁned by Newton as the rate of change

of momentum, Euler related the change in the moment of momentum to a

moment of force, or torque, applied to a body. With the moment of momentum

of a point mass deﬁned as

(4.85)

in (4.31), the rate of change of the real angular momentum of a body is given



ρ(p

)dV



, (4.86)

where V ⊂ R

is the volume of the body. Another expression for

can be

obtained by performing a time diﬀerentiation of the angular momentum in

(4.42) with

b b

b w

) . (4.87)

Equating the expressions in (4.86) and (4.87) yields



ρ(p

)dV



| {z }

b b

b w

)

| {z }

, (4.88)

where A and B are introduced to simplify the derivation. We will ﬁrst com-

pute A, then compute B, and ﬁnally combine the two expressions. Since the

derivative and integral are linear operations, we have that

A =



ρ(p

)dV



(4.89)

(

) ρ(p

)dV , (4.90)

and using d/dt(u × v) =

u × v + u ×

ρ(p

)dV (4.91)

| {z }

(

)

ρ(p

)dV +

ρ(p

)dV

{z }

(4.92)

4.7. NEWTONIAN MECHANICS 67

where the rightmost term is identiﬁed as being the total moment of force acting

on the body since f = ma. Continuing, we have

ρ(p

)dV +

ρ(p

)dV

| {z }

(4.93)

ρ(p

)dV

| {z }

(4.94)

= m

, (4.95)

which provides, for the ﬁrst time, an expression featuring the torque (or mo-

ment of force) exerted on a body. The expression for B is computed as

B =

b b

b w

) (4.96)

= m

+ m

b b

b w

b b

b w

(4.97)

by using d/dt(u×v) =

u×v+u×

v and d/dt(Rv) = R ˙v+ω ×Rv. Combining

A and B, we ﬁnd that

A = B (4.98)

| {z }

= m

| {z }

b b

b w

b b

b w

, (4.99)

in which common terms cancel out. The result is Euler’s law of motion for a

rotating rigid body:

= m

b b

b w

b b

b w

, (4.100)

which relates the total torque exerted on a body to its kinematics. (4.100) can

be used to predict the motion of a body onto which a force is applied on a

point remote from the origin of the body. Such a force will induce a moment of

force, or torque, relative to F

that will change its angular acceleration. Note

that, due to the leftmost term of the right-hand side of (4.100), if the origin

of F

is not coincident with the centre of mass, the body will also experience

a linear acceleration. However, when F

= F

, the leftmost term cancels out

and the body will only experience a change in angular acceleration.

In a nutshell, Euler’s second law of motion ((4.100)) states that the torque

acting on a body will produce angular acceleration (a change in the rate of

its angular momentum), and potentially a linear acceleration too. The inertia

matrix provides the mapping between torque and angular acceleration.

68 CHAPTER 4. RIGID BODY DYNAMICS

4.7.3 Newton-Euler Equations

In general, a body can be subject to multiple forces acting upon it. By virtue

of being vector quantities, forces can be added together to form a resultant

force acting on a body. Unless all forces are applied at the origin, or unless

there is a perfect balance of forces, a net moment of force will be exerted

on the body (i.e., a torque) about its origin. When analysed relative to the

body frame, the motion of the body will therefore be inﬂuenced both by a

force exerted on the origin and by a torque exerted about the origin. The

Newton-Euler equations of motion deﬁne how forces and torques acting on a

body inﬂuence its motion.

In Sec. 4.7.1, we derived an expression describing the inﬂuence that a force

has on the linear acceleration of a body whose velocity might be changing

direction or magnitude (i.e., the body might be accelerating). In Sec. 4.7.2,

diﬀerentiating the moment of momentum with respect to time led to an ex-

pression describing how a moment of force inﬂuences the angular acceleration

of a body and potentially also contributes to a linear acceleration. We now

have done most of the work and all is left is to combine the two expressions

into the Newton-Euler equation of motion for an accelerating rigid body. We

will ﬁrst express the equation of motion as a set of two equations, one for lin-

ear motion and one for angular motion, and then combine them into a single

equation.

In Sec. 4.7.1, we obtained the expression

= m (

+ 2

× (

) +

) (4.101)

describing how the force

acting on a body inﬂuences its linear acceleration.

In Sec. 4.7.2, we derived

= m

b w

, (4.102)

which describes how the moment of force

inﬂuences the acceleration of the

body it is exerted on. Putting the two equations together yields the Newton-

Euler equations of motion for a rotating rigid body:

= m (

+ 2

× (

) +

) (4.103)

= m

b w

, (4.104)

where all quantities are expressed in F

, which is assumed to be an inertial

frame. The force

and torque

are respectively acting on and about F

and are expressed as a function of the body kinematics and inertial parameters.

4.7. NEWTONIAN MECHANICS 69

With Centre of Mass Rigidly Attached

Equation (4.103) and (4.104) can be further simpliﬁed by assuming that the

centre of mass does not move relative to F

. For instance, in the context of a

robot manipulating an object, we can simplify the equations if we assume that

the object will not slip in the gripper. In such a case,

= 0 and

= 0,

and the Newton-Euler equations becomes

= m (

× (

) +

) (4.105)

= m

b w

, (4.106)

where the Coriolis force does not appear anymore. We can usually select the

body-ﬁxed frame F

such that the centre of mass (presumably) does not move

relative to it. Making use of skew-symmetric matrices and of matrix notation,

the above equations can be rewritten as







3×3

−m [

]

m [

]







m [

]

[

]

[

]

b w



. (4.107)

The equations are further simpliﬁed when F

= F

, but such a situation

rarely occurs in robotics. An equivalent, but slightly more practical formu-

lation is obtained by expressing the equations in the body-ﬁxed frame F

such that the inertial parameters do not need to be transformed into F

via

a continuously changing rotation matrix. The resulting equations become







3×3

−m [

]

m [

]











, (4.108)

in which the kinematics are now expressed in F

The system of equations in (4.108) can be concisely written as

(4.109)

where

(4.110)

and the skew-symmetric matrix of the velocity twist is deﬁned as









. (4.111)

70 CHAPTER 4. RIGID BODY DYNAMICS

The wrench

is a six-dimensional spatial force vector that can be expressed

in a diﬀerent reference frame through the relation

= Ad (

)

−T

(4.112)

where Ad (

) is the adjoint matrix of the pose of F

relative to F

, as

deﬁned in (2.99).

4.7.4 Euler’s Laws

Euler’s laws of motion generalize Newton’s second law to rotating rigid bodies

and state that

(4.113)

, (4.114)

where f and τ are respectively the total force and torque exerted upon the

body. Note that the above equations are only true if the momentum

is measured and expressed in an inertial frame. Making use of (4.41)

and (4.42), where the body reference frame F

is at the centre of mass such

that

= 0, Newton’s law can be formulated as

= m

d (

)

= m

(4.115)

(4.116)

where velocities and accelerations are expressed in the inertial frame F

through the use of (3.11).

4.7.5 Euler’s Equations for the Motion of a Body in a

Force Field

When F

is chosen to be located at the centre of mass and that is it also aligned

with the principal axes of inertia, the equation for the angular momentum is

greatly simpliﬁed such that

= I

(4.117)

and the rotational kinetic energy from (4.51) becomes

rot

b w



+ I



(4.118)

4.8. CONTACTS 71

where I

is the moment of inertia of the principal axis labeled as x. The

expression for the torque in (4.116) becomes











= I

− (I

− I

)ω

= I

− (I

− I

)ω

= I

− (I

− I

)ω

(4.119)

that are referred as Euler’s equations for the motion of a body in a force ﬁeld.

Since the equations in (4.119) only depends on the principal axes of inertia,

two objects with diﬀerent shapes but with the same principal axes of inertia

will move in the same way. For instance, an equivalent ellipsoid can be deﬁned

to represent a rigid body.

4.8 Contacts

4.8.1 The Rigid Body Assumption

In previous chapters, rigid transformations were used to succinctly describe

the pose and motion of complete objects. The assumption underlying the use

of rigid transformations is that objects are perfectly rigid, which has important

ramiﬁcations. A direct consequence of assuming that bodies are perfectly rigid

is that even an inﬁnitely large force will not deform the body — it has an inﬁ-

nite Young’s modulus. Since there is no internal deformation in which energy

can be stored, the force applied to a rigid body is instantaneously transferred

across the object, violating the ﬁnite speed of sound (e.g. about 5000 m/s in

steel). Also, when considering several contact forces exerted on a rigid body,

the rigid body assumption implies that all forces can be summed together to

obtain the net force acting on the body. Although greatly simplifying dynamic

analyses, the rigid body assumption can also lead to indeterminate situations

where multiple, equally valid, sets of forces could be applied to the body. For

instance, when considering a perfectly rigid chair resting on the ground whose

weight is 10 Newtons, the set of forces at each leg could be {2.5, 2.5, 2.5, 2.5}

Newtons or {3, 3, 2, 2} Newtons, or any other combination that sums to 10

Newtons.

In robotics, simple models are ubiquitous: cameras are modelled as pin-

holes, robot links are modelled as rigid bodies, contacts are modelled as in-

ﬁnitesimal points, friction is modelled as a step function, and so on. Although

it is often reasonable to use simple models, one must be cautious about the

implications of the underlying assumptions.

72 CHAPTER 4. RIGID BODY DYNAMICS

A Slightly Relaxed Object Model

To avoid issues pertaining to the rigid body assumption (i.e. indeterminacies

and instantaneous force propagation), a slightly relaxed version of the rigid

body model can be used. Instead of considering objects as perfectly rigid,

we can consider that a ﬁnite number of contact points, distributed over the

contact interface between two objects, can slightly deform while the rest of

the object remains rigid. Hence, the Young’s modulus (or equivalently, the

stiﬀness or spring constant) of the material at the contact points is considered

to be ﬁnite but very large such that a very large force at the contact point

is required to produce a very small deformation. Under this relaxed model,

the contact points are said to be deforming in the elastic regime as opposed

to the plastic regime where deformation is permanent. Denoting the material

stiﬀness by κ, and the deformation at the contact point by δ, the relation

∥f∥ = κδ (4.120)

expresses that the force required to deform the material is proportional to the

deformation. From (4.60), the potential energy stored in the material due to

the deformation is given by

U =

∥f∥dδ =

κδdδ =

κδ

∥f∥

2κ

(4.121)

such that the potential energy stored in the material is proportional to the

square of the force applied to the contact point. Extending D’Alembert Prin-

ciple (see (5.46)) led to the principle of least action, which states that the path

taken by a system between two points in conﬁguration space is the one that

minimizes energy expenditure. Essentially, a dynamical system will follow the

path that minimizes the energy spent, or as Maupertuis put it, “Nature is

thrifty in all its actions”. Hence, in the context of our contact model, the ob-

jects will deform in such a way that the potential energy stored in the contact

points is minimized, which from (4.121) is equivalent to minimizing the square

of the contact forces. Mathematically,

min

∥f

∥

(4.122)

expresses the objective that nature will follow when determining the contact

forces, we shall do the same.

4.8.2 Types of Frictional Contacts

Many tasks like grasping, placing, or pushing, involve interacting with objects

through frictional contacts, and modelling friction as accurately as possible is

4.8. CONTACTS 73

crucial for the success of these tasks. The ﬁeld of tribology studies this very

phenomenon and has developed several models to describe the frictional forces

that arise when two surfaces are in contact. Tribologists have shown that fric-

tion depends on many factors such as the roughness of the surfaces, chemical

bonding, local lubrication, the real contact area (which is usually much smaller

than the apparent contact area), the temperature, etc. Although more com-

plex models can describe friction more accurately, they also introduce more

parameters and dependencies that are diﬃcult to estimate, if observable at

all. Hence, in robotics, simple models are often preferred since the uncertainty

in the model parameters far outweighs the beneﬁts of a more accurate model.

The Coulomb friction model (sometimes referred to as Amontons’), poten-

tially augmented with viscous drag and Stribeck eﬀect, is typically used. As

shown in Fig. 4.2, the Coulomb friction model assumes that the friction force

is proportional to the real contact area that is itself proportional to the force

pressing the two surfaces together.

The Coulomb friction model is deﬁned as

(

≤ µ

if v = 0

= µ

if v > 0

(4.123)

where F

is the tangential friction force, F

is the normal force exerted on

the contact interface, µ

is the static friction coeﬃcient that is experienced

when the objects are immobile, and µ

is the kinetic friction coeﬃcient that

is experienced when the relative velocity v between the two surfaces is greater

than zero. At very low velocities, empirical results suggest that the coeﬃcient

of friction is not equal to µ

but rather a value between µ

and µ

. This

phenomenon is known as the Stribeck eﬀect and results in a smooth transition

between the static and kinetic friction regimes, as shown in Fig. 4.2. As the

relative velocity between the two surfaces increases, the contact between the

two surfaces becomes lubricated and enters an hydrodynamic regime where a

signiﬁcant viscous drag force is experienced. The friction model resulting from

the Coulomb model augmented with viscous drag and Stribeck eﬀect can be

expressed as

(v) = F



+ (µ

− µ

−mv



+ dv (4.124)

where m is the Stribeck constant and d is the viscous drag slope. A smooth

Coulomb model is obtained by setting m ≫ 1 and d = 0, with the magnitude

of m determining how quickly the friction force transitions from the static to

the kinetic regime.

Since empirical results suggest that the real contact area is much smaller

than the apparent contact area, the assumption that objects are contacting

74 CHAPTER 4. RIGID BODY DYNAMICS

0 0.5 1 1.5 2 2.5 3

0.2

0.4

0.6

0.8

1.2

static

= F

kinetic

= F

Hydro dy namic Regime

Velocity v (m/s)

Friction F

(Newtons)

(v) = F



+ (µ

− µ

−mv



+ dv

Static Friction

Kinetic Friction

Viscous Drag Slope d

Figure 4.2: Friction force as a function of velocity. When immobile, a body is

subject to a static friction force F

static

= µ

, and when in motion, a lower

kinetic friction force F

kinetic

= µ

is experienced. The Stribeck eﬀect is

apparent during the transition between the static and kinetic regimes, where

coeﬃcient of friction smoothly decreases as velocity increases. As velocity

increases, the interface between the two surfaces enters the hydrodynamic

regime and a signiﬁcant viscous drag force dv is experienced.

4.8. CONTACTS 75

1 2

1,3

1,1

2,3

2,1

3,3

3,2

4,3

4,2

Figure 4.3: Three objects in contact through four contact points. At each

contact point, two frames are deﬁned: one for each object in contact. Each

frame is deﬁned with a normal directed outward from the surface of the object

it belongs to. In this ﬁgure,

i,j

expresses the orientation of a frame attached

to the j-th object and located at the i-th contact point relative to the world

frame F

through point-like contacts can be made with relatively stiﬀ materials. Also,

contact points can be assumed to be in the elastic regime, with the local

deformation being linearly proportional to the force sustained by the contact

point. Discretized point contact can be modelled as hard or soft contacts, with

the former only transmitting forces while the latter also resisting torque about

the surface normal. In practice, discretizing the contact area into a set of

contact points brings about the problem of determining their location, which

is ultimately determined by microscopic variations in the shape of the objects.

An optimistic solution is to assume that the contact points are located on the

the convex hull of the contact area, such that they are best positioned to resist

torque about the surface normal.

4.8.3 Computing Contact Forces

According to D’Alembert principle (see Sec. 5.1.5), the sum of all external

forces acting on a body in motion must be equal and opposed to the sum

of the inertial forces. Hence, the sum of wrenches due to contact forces and

of inertial forces must result in a zero wrench — an equilibrium. The force

acting on a body j at the i-th contact point can be expressed in a local frame

i,j



ˆu ˆv ˆn



, where ˆn is the outward surface normal and ˆu, ˆv are two

orthogonal vectors tangent to the surface. The wrench due to the contact force

76 CHAPTER 4. RIGID BODY DYNAMICS

is given by

i,j



i,j

[

]

i,j



(4.125)

where f

is the force acting at the i-th contact point expressed in F

i,j

whose

orientation relative to the world frame is given by

i,j

. By virtue of the

action-reaction principle (i.e., Newton’s third law, see Sec. 4.7.1), the wrench

exerted on an object at a contact point is equal and opposite to the wrench

exerted on the other object at the same contact point. Hence,

i,a

= −

i,b

(4.126)

for two objects a and b in contact at the i-th contact point. The condition in

(4.126) can either be enforced by deﬁning

i,a

= f

(4.127)

i,b

= −f

(4.128)

i,a

i,b

(4.129)

or by deﬁning

i,a

= f

(4.130)

i,b

= f

(4.131)

i,a

= −

i,b

(4.132)

where the second set of equations results in a minimal set of force vectors (only

one per contact point), which reduces the size of the problem when solving for

the contact forces. Also, in the second set of equations, normals are oriented

outward for both objects, following a common convention. However, deﬁning

i,a

= −

i,b

results in one of the contact frame being left-handed, which

can be confusing when performing computations. It is therefore recommended

to be very careful when using the second set of equations.

With the inertial wrench

exerted on object j given by the equation

of motion in (4.103)–(4.104), the equilibrium equation can be expressed as

i,j

= 0 (4.133)

that is simpliﬁed to

i,j

+ m



3×3

[

]







−g





= 0 (4.134)

4.8. CONTACTS 77

in static (or quasi-static) situations where g is the gravitational acceleration

constant (e.g. ≈ 9.81 m/s

The system of linear equations deﬁned in (4.133) can be solved to obtain

contact forces acting on the body, but with two caveats. First, (4.133) will be

under-determined with the rank of the coeﬃcient matrix not greater than 6

but with at least 3 tri-dimensional forces to solve for (9 unknowns). Second,

the direction of the force acting at a contact point is constrained to (i) be

compressive since glue would be required to sustain a tensile force, and (ii)

respect friction limits at the contact point. Due to the ﬁrst constraint, contact

forces are often referred to as unilateral forces since the contact force can only

push on the body and not pull. With a contact force expressed in its local

frame as f





, the compressive constraint can be expressed

≤ 0 (4.135)

if the surface normal ˆn

is pointing outward.

A contact point is said to be sticking if the tangential force is less than

some threshold given by the friction model, and slipping otherwise. Using the

Coulomb friction model, the contact condition at the i-th contact point can

be expressed as

c = µ



−



(4.136)

where µ

is the friction coeﬃcient at the i-th contact point, and f

is the

tangential force acting at the i-th contact point. The contact point is said to

be sticking if c > 0 or equivalently if



≥



(4.137)

where the left hand side represents the maximal tangential force that can be

sustained by the contact point. Squaring and rearranging terms in (4.136)

yields

+ f

≤ µ

(4.138)

that represent the equation of a cone with axis along the normal force and

with an angle of atan(µ

), as pictured in Fig. 4.4. Any contact force lying

inside the cone will result in a sticking contact, while any contact force lying

outside the cone will result in a slipping contact.

Enforcing (4.137) can be diﬃcult due to the non-linear nature of (4.136),

and a common approach is to linearize the Coulomb friction model. To do so,

the friction cone is approximated as a N-sided pyramid, as shown in Fig. 4.5.

A greater number of sides will result in a better approximation of the cone,

but will also result in a more complex deﬁnition that is more computationally

78 CHAPTER 4. RIGID BODY DYNAMICS

ˆu

ˆv

ˆn

atan(µ

)



Figure 4.4: The Coulomb friction cone at the i-th contact point whose normal

is ˆn

, coeﬃcient of friction is µ

, and with an angle of atan(µ

). The contact

force f

must lie inside the cone for the contact condition c to be positive and

the contact point to be sticking.

expensive to enforce. The proportion of friction cone volume covered by the

pyramid is given by

1 −

N sin(2π/N )

2π

(4.139)

where N is the number of sides of the pyramid. Hence, a simple four-sided

pyramid will result in a volume diﬀerence of about 35% with the cone, which

means that about a third of the forces inside the friction cone will end up

outside the pyramid and incorrectly identiﬁed as slipping contacts. In com-

parison, an octagonal pyramid will result in a volume diﬀerence of about 10%

with the cone.

The area of the N -sided polygon inscribed in the friction circle, as shown

in Fig. 4.5, is deﬁned by the set of inequalities

cos



2πi



+ sin



2πi



≤ µf

cos





∀i ∈ {0, . . . , N − 1} (4.140)

where i ∈ N is the index of the side of the polygon. The inequalities can be

rewritten in matrix form as

Cf ≤ 0

N×1

(4.141)

4.8. CONTACTS 79

with C deﬁned as

C =







1 0 −µ cos





cos



2πi



sin



2πi



−µ cos





cos



2π(N−1)



sin



2π(N−1)



−µ cos











(4.142)

such that a square and octagonal approximation of the friction cone result in

C =







1 0 −µ/

√

0 1 −µ/

√

−1 0 −µ/

√

0 −1 −µ/

√







and C =







1 0 −µ cos(π/8)

√

2 1/

√

2 −µ cos(π/8)

0 1 −µ cos(π/8)

−1/

√

2 1/

√

2 −µ cos(π/8)

−1 0 −µ cos(π/8)

−1/

√

2 −1/

√

2 −µ cos(π/8)

0 −1 −µ cos(π/8)

√

2 −1/

√

2 −µ cos(π/8)







(4.143)

respectively.

Determining the contact forces in an assembly of multiple (nearly) rigid

bodies in contact can be formulated as the following quadratic problem with

linear constraints. With P

being the set of contact points on the j-th body

and O the set of bodies in the assembly, the contact forces can be determined

by solving

min

i∈P

∥f

∥

(4.144)

s.t.

i∈P

i,j

= 0 ∀j ∈ O (4.145)

≤ 0 ∀i ∈ P (4.146)

≤ 0

N×1

∀i ∈ P (4.147)

The optimization problem deﬁned above is a quadratic program with linear

constraints that can be solved very quickly with standard solvers. The problem

can be cast into the following standard form

min

Px + q

x (4.148)

s.t. l ≤ Ax ≤ u (4.149)

80 CHAPTER 4. RIGID BODY DYNAMICS

(a) (b)

µ ∥f

∥

θ/2

µ∥f

∥ cos(θ/2)

cos θ f

sin θ

Figure 4.5: (a) Four-sided and eight-sided pyramidal approximations of the

Coulomb friction cone. A greater number of sides will result in a better ap-

proximation of the cone. (b) A cut of the friction cone perpendicular to the

normal axis with an inscribed octagon. Here θ = 2π/N is the angle between

two consecutive sides of the polygon.

by deﬁning

x =



···



∈ R

3|P|

(4.150)

P = 1

3|P|×3|P|

(4.151)

q = 0

3|P|×1

(4.152)

as the objective, and

l =



···

|O|

−∞

N|P|

−∞

|P|



(4.153)

u =



···

|O|

N|P|

|P|



(4.154)

4.9. KEY CONCEPTS 81

as the lower and upper bounds, and

A =













··· B

|P|1

··· B

|P|2

···

. ···

1|O|

2|O|

··· B

|P||O|













N×3

··· 0

N×3

···

. ···

N×3

|P|















0 0 1



1×3

··· 0

1×3



0 0 1



1×3

···

. ···

1×3

···



0 0 1















(4.155)

where











i,j

[

]

i,j

if i ∈ P

6×3

otherwise.

(4.156)

In (4.156), the local contact frames at the i-th contact point, located on an

interface between object a and b, are deﬁned such that

i,a

= −

i,b

resulting in

i,b

being a left-handed coordinate system. By doing so, the size

of x is reduced by half, and the action-reaction principle is enforced without

the need to introduce additional constraints.

4.8.4 Multi-Object Interactions

This is a placeholder for the upcoming section on multi-object interactions.

4.9 Key Concepts

• An inertial frame of reference is a frame that does not accelerate. In an

inertial frame, Newton’s laws of motion apply without modiﬁcation.

• Any rotating frame of reference accelerates due to the change of velocity

direction.

82 CHAPTER 4. RIGID BODY DYNAMICS

• The moment of some physical quantity is deﬁned as the cross product of

a position vector and the physical quantity.

• Important moments include: moment of force, moment of momentum,

and moment of mass.

• In the equations of motion, the zero-th, ﬁrst, and second moments of a

mass distribution appear.

• The zero-th moment is the total mass, the ﬁrst moment is the centre of

mass, and the second moment is the inertia tensor. These moments can

be bundled into a spatial inertia matrix.

• While the total mass is guaranteed to be positive, the centre of mass is

always located inside the convex hull of the mass distribution, and the

inertia tensor is always positive-deﬁnite.

• An eigendecomposition of the inertia tensor can be used to determine

the principal axes of inertia, which are the axes along which the inertia

tensor is diagonal.

• The momentum of a rigid body is the product of its mass and its velocity,

it is a conserved quantity, and it is additive.

• For a rotating body, the angular momentum (or moment of momentum)

is also a characteristic of interest.

• The kinetic energy of a rigid body is given by the integral over the

momentum of all particles in the body. Kinetic energy therefore inherits

the properties of momentum (i.e., conservation and additivity).

• The equations of motion for a rotating rigid body relate the forces and

torques acting on the body to its kinematics.

• Deriving the equations of motion involves diﬀerentiating the linear and

angular momentum of the body with respect to time.

Chapter 5

Manipulator’s Dynamics

5.1 Lagrangian Mechanics

5.1.1 Coordinates, Conﬁgurations, and Constraints

The minimal set of coordinates q = [q

, . . . , q

]

that can be used to fully de-

scribe the position of all particles in a system is called the generalized coordi-

nates. The number of generalized coordinates should be equal to the number

of degrees of freedom minus the number of constraints. The motion of the

system can be described as a trajectory within the space of generalized coor-

dinates called the conﬁguration space. A conﬁguration is a speciﬁc point in

the conﬁguration space that can be described using the system’s generalized

coordinates.

A holonomic constraint reduces the number of coordinates necessary to

fully describe the system’s position and can be written as

f (q

, . . . , q

, t) = 0, (5.1)

where all terms inside the parentheses are independant from each other. In

Cartesian space, constraining a particle to stay on the surface of a sphere is

an instance of a holonomic constraint, since, instead of three Cartesian coor-

dinates, only two (e.g., azimuth and polar angles) are required to describe the

particle’s position. If, instead of describing the particle’s position with Carte-

sian coordinates, azimuth and polar angles were used (with a given radius),

the holonomic constraint would be implicitly enforced. Conversely, a non-

holonomic constraint restrict possible system conﬁgurations without reducing

the number of coordinates necessary to describe it. In Cartesian space, con-

straining a particle to stay inside a given box is an instance of a nonholonomic

84 CHAPTER 5. MANIPULATOR’S DYNAMICS

constraint, as it is impossible to describe all positions within the box using less

than three coordinates. When the state of a system not only depends on its

generalized coordinates but also on the trajectory taken or on derivatives of

the generalized coordinates, it means that the system is under nonholonomic

constraints.

5.1.2 Jacobians

A Jacobian matrix is a matrix of partial derivatives

(q) =

∂r

∂q

∂r

∂q

···

∂r

∂q

, (5.2)

in which each column is a basis vector that relates how a small change in a

generalized coordinate q

will produce a change in r. For a robot with m joints,

the Jacobian relating the velocity of the joints to the spatial velocity of the

end-eﬀector is a 6 × m matrix. The matrix

(q) =

∂r

∂q

∂r

∂q

···

∂r

∂q

(5.3)







∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q







(5.4)

is called the space Jacobian of the system and maps motions in the conﬁgura-

tion space to motions in the task space via



















∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q

∂r

∂q

∂r

∂q

. . .

∂r

∂q



















(5.5)

r = J

(q)

q. (5.6)

The above matrix is often called space Jacobian because it expresses task space

velocity in the space/inertial frame. An alternative Jacobian, termed body

5.1. LAGRANGIAN MECHANICS 85

Jacobian J

(q), can be deﬁned such that the task space velocity is expressed

in the body/moving frame. The relation between the two follows from (3.16)

(q) = Ad (

) J

(q), (5.7)

which uses the adjoint representation deﬁned in (2.101).

For manipulators, the i-th column J

(q)

of the space Jacobian represents

the screw vector describing the i-th joint axis in the world frame as a function

of the conﬁguration of the robot. Hence, we have that

(q)

= Ad (

i−1

(q)) S

, (5.8)

where

i−1

(q) is a function of the joint angles and S

is the screw deﬁnition

of the i-th joint as in (2.110) or (2.111). As a reminder, the deﬁnition of the

unit-screw for a given joint is











−ω × q

if revolute

if prismatic

, (5.9)

where ω and v are angular and linear axes, respectively, and q is a point

anywhere on the screw axis.

Making use of the space Jacobian, the equations

= J

(q)

q (5.10)

(q)

q + J

(q)

q (5.11)

Q = J

(q)

W (5.12)

W = J

(q)

−T

Q (5.13)

map joint-space quantities to task-space quantities and are widely used in

robotics. For instance, (5.10) can be used to compute what the end-eﬀector

velocity of a manipulator will be for given joint-space velocities (a manip-

ulator’s joints are the generalized coordinates). However, note that (5.10)

produces a six-dimensional twist, which needs to be transformed into a lin-

ear and angular velocity bundle before being used in 3D dynamics equations

(4.103) and (4.104). Such transformation can be built from (3.13) and (3.34)







3×3

−[

]

3×3



(5.14)







3×3

−[

]

3×3



, (5.15)

86 CHAPTER 5. MANIPULATOR’S DYNAMICS

where

is the point for which the kinematics is to be computed (e.g., the

origin of the end-eﬀector frame).

Note that if the Jacobian is not square and full rank, as with underactuated

or over-actuated robots, it cannot be simply inverted as in (5.13). A matrix is

full-rank when its rank is equal to its number of rows or columns, whichever

is the smallest. The left pseudo-inverse of the Jacobian, deﬁned as

(q)

= J

(q)



(q)J

(q)



−1

(5.16)

(q)

(q) = 1, (5.17)

can be used instead, and produces the solution minimizing ||˙q||

For under-actuated robots, the mapping from joint velocities to end-eﬀector

velocity will not cover the six elements of the spatial velocity, and the Jacobian

will be taller than it is wide. This is captured by the null-space of the Jacobian

given by

Null (J

(q)) = {

q | J

(q)

q = 0}, (5.18)

spanning the joint velocities that do not result in any motion of the end-

eﬀector. With an over-actuated robot, the rank of Null (J

(q)) can be greater

than zero, which means that even if the end-eﬀector is kept ﬁxed, parts of the

robot can move. In contrast, with an under-actuated robot, rank(J

(q)) < 6

such that velocity cannot be produced in some directions. The right pseudo-

inverse of the Jacobian, deﬁned as

(q)



(q)



−1

(q)

(5.19)

(q)J

(q)

= 1, (5.20)

can be used to ﬁnd the solution minimizing the L2-norm of the error between

the desired end-eﬀector velocity and the actual end-eﬀector velocity. The in-

ability to generate velocity in some directions can be turned into an advantage

as the robot will be able to resist inﬁnite force in those directions. For in-

stance, the vertical force exerted on a SCARA robot will be sustained by the

(presumably inﬁnitely strong) structure of the robot and not by the (much

weaker) joints. SCARA robots are therefore useful to lift heavy objects and

move them in the plane.

Importantly, when J

(q) is rank-deﬁcient and det (J

(q)) = 0, it eﬀectively

maps generalized velocities to a lower dimensional space in which some task-

space velocities cannot be represented. This happens when the generalized

coordinates are in a singular conﬁguration resulting in the robot being unable

to make any further motion in some directions. Typically, a singularity is

reached when the axes of two joints become collinear, or when the axes of

5.1. LAGRANGIAN MECHANICS 87

three joints are parallel. In that situation, (5.19) can be used to make a

motion approximately following the desired motion, hoping to move away from

the singular conﬁguration.

Close to a singularity, the joint velocities required to make any further

motion can be unrealisable by the robot hardware, and it might be a good

idea to prevent such conﬁgurations from being reached in the ﬁrst place. To

avoid reaching a singular conﬁguration, the manipulability of the end-eﬀector

can be monitored through manipulability indices like Yoshikawa’s

det



(q)J

(q)



, (5.21)

which is proportional to the volume of the velocity ellipsoid. When the robot

comes close to a singular conﬁguration, the manipulability index approaches

zero, indicating that the robot is unable to move in some directions. At this

point, the robot can try to move in a direction that would increase the ma-

nipulability index.

5.1.3 Hessian

In (5.11), the time-derivative of the Jacobian is used to map joint-space ac-

celerations to task-space accelerations. The Hessian tensor H

(q) =

(q) is

a three dimensional tensor of size (N × N × N ) where N is the number of

generalized coordinates (e.g., the number of joints in a robot). Each slice of

the Hessian describes

∂J

(q)

∂q

, the Jacobian changes for an inﬁnitesimal change

in a given generalized coordinate. Hence, the Hessian is deﬁned as

(q) =

∂J

(q)

∂q

∂J

(q)

∂q

, ··· ,

∂J

(q)

∂q

. (5.22)

If H

(q)

a,b,c

denotes the element of the Hessian in the a-th slice, b-th row,

and c-th column, then H

(q)

i,1:6,j

is the j-th column of the i-th slice of the

Hessian. For serial manipulators, the Hessian is symmetric, and can be built

from the Jacobian with

(q)

i,1:6,j

= H

(q)

j,1:6,i



(q)

3:6,j



(q)

1:3,i



(q)

3:6,j



(q)

3:6,i

∀ j ∈ {i, . . . , N}

∀ i ∈ {1, . . . , N},

(5.23)

where J

(q)

a:b,i

denotes the vector built from the a-th to b-th rows inclusively

of the i-th column of the Jacobian.

88 CHAPTER 5. MANIPULATOR’S DYNAMICS

5.1.4 Virtual Displacement and Virtual Work

An inﬁnitesimal deviation from a system’s trajectory while still respecting all

constraints is called a virtual displacement δr (the keyword virtual empha-

sizes the fact that the displacement is hypothetical) and is given by the total

diﬀerential

δr =

∂r

∂q

δq

(5.24)

∂r

∂q

∂r

∂q

···

∂r

∂q



δq

, δq

, ··· , δq



(5.25)

= J

(q)δq, (5.26)

where J

(q) =

∂r

∂q

∂r

∂q

···

∂r

∂q

is the space Jacobian of the system,

which maps motions in the conﬁguration space to motions in the task space.

Starting from

δr = J

(q)δq (5.27)

and diﬀerentiating with respect to time, we get

= J

(q)

(5.28)

∂r

∂q

∂r

∂q

···

∂r

∂q



˙q

, ˙q

, ··· , ˙q



(5.29)

= J

(q)

q (5.30)

since J

(q) is constant for a given conﬁguration.

As outline in (4.60), work is produced by applying a force along a distance.

The virtual work δW produced by applying a force along a virtual displacement

is therefore deﬁned as

δW = f · δr (5.31)

= f ·

∂r

∂q

δq

(5.32)

f ·

∂r

∂q

δq

(5.33)



∂r

∂q



| {z }

δq

(5.34)

= Q

δq (5.35)

= Q · δq, (5.36)

5.1. LAGRANGIAN MECHANICS 89

where the distributivity of the dot product, and where a ·b = a

b = b

a were

used. From the above equations, we can say that the generalized forces Q

produce work when performed over the distance δq. Combining (5.24) with

(5.31) and leveraging the fact that work is a scalar (which is equal to its own

transpose), we can state

δW = Q

δq = f · δr (5.37)

= f · (J

(q)δq) (5.38)

= f

(q)δq (5.39)

= f

(q) (5.40)

Q = J

(q)

f (5.41)

where the transpose of the Jacobian J

(q) is used to map from task-space

forces f to conﬁguration space forces Q.

5.1.5 D’Alembert Principle

For a particle to perform zero virtual work along some virtual displacement,

the equation

f · δr = 0 (5.42)

must hold. If the virtual displacement itself cannot be zero, then f must be.

With

being the sum of all external forces acting on the particle, we have

from Newton’s second law that

= m

(5.43)

− m

= 0, (5.44)

which provide us with an expression where the total force (including the inertial

force that is due to the motion of the point mass) acting on the particle, in an

inertial frame, is zero. We can therefore rewrite (5.42) as

(

− m

) · δr = 0 (5.45)

since F

= F

for a point mass. Equation (5.46) is called D’Alembert Principle

that can be stated more generally as



f −



· δr = 0 (5.46)

· δr =

· δr (5.47)

expressing that the sum of all external forces along the virtual displacement

must be equal to the time derivative of the total momentum along the virtual

displacement.

90 CHAPTER 5. MANIPULATOR’S DYNAMICS

5.1.6 Lagrange’s Equation Of Motion

Starting from the D’Alembert Principle, we get

(

− m

) · δr = 0 (5.48)

· δr = m

· δr (5.49)

= m

· δr (5.50)

= m

∂r

∂q

δq

(5.51)



∂r

∂q

δq



(5.52)

by using the distributivity of the dot product. Furthermore, we note that since

the Jacobian J

(q) appears in both (5.24) and (5.30),

∂

∂r

∂q

, (5.53)

and since, from (5.1), the generalized coordinates are considered independent

of time,



∂r

∂q



∂





∂q

∂

∂q

, (5.54)

provides us with an expression for the time-derivative of the partial diﬀerential

of the displacement with respect to a generalized coordinate. Using the identity

(a · b) = a ·

(b) + b ·

(a) (5.55)

we get



∂r

∂q



= m



∂r

∂q



∂r

∂q

) (5.56)

or equivalently

∂r

∂q



∂r

∂q



− m



∂r

∂q



(5.57)



∂



− m

∂

∂q

, (5.58)

5.1. LAGRANGIAN MECHANICS 91

where (5.53) and (5.54) were used. Using the identity

∂

∂a

(b · b) = b ·

∂b

∂a

+ b ·

∂b

∂a

= 2b ·

∂b

∂a

, (5.59)

the equation in (5.58) can be simpliﬁed into

∂r

∂q



∂



− m

∂

∂q

(5.60)



∂





| {z }



−

∂

∂q





| {z }

(5.61)



∂

(

)



−

∂

∂q

(

) , (5.62)

in which the kinetic energy deﬁned in (4.46) can be identiﬁed. Rewriting (5.52)

using (5.62) yields

f · δr =



∂r

∂q

δq



(5.63)



∂

(

)



−

∂

∂q

(

) δq



, (5.64)

where the total force acting on the point mass can be written as

f · δr = Q · δq (5.65)

δq

(5.66)

to obtain

δq



∂

(

)



−

∂

∂q

(

) δq



(5.67)

or equivalently



∂

(

)



−

∂

∂q

(

) , (5.68)

which express an element of the generalized force vector as a function of the

kinetic energy. In the case where a potential ﬁeld (e.g., gravity) also acts on

92 CHAPTER 5. MANIPULATOR’S DYNAMICS

the point mass (pretty much always the case), the total energy of the system

can be described by the Lagrangian function

L = K − U, (5.69)

where U is the potential energy. In such a case, Lagrange’s equation of motion

becomes



∂L

∂



−

∂L

∂q

, (5.70)

which can be further simpliﬁed if the potential ﬁeld is velocity independent

(like gravity is) into



∂K

∂



−

∂L

∂q

(5.71)

that is more convenient to use for robotics applications.

5.1.7 Serial Robot Joint-Space Dynamics in Matrix Form

For open chain robots with rigid links (i.e., serial robots), the equation of

motion in (5.71) can be conveniently written as

Q = M(q)

q + C(q,

q + g(q), (5.72)

where M(q) is the mass matrix, C(q,

q) is the Coriolis matrix, and g(q) is

the gravity vector. In general, the mass matrix can be very tedious to derive

as it depends on the kinematics, dynamics and generalized coordinates of the

system. Fortunately, for a n degrees of freedom serial robot, the mass matrix

can be obtained with

M(q) =

i=1



(q)





(q)



, (5.73)

where

is the spatial inertia of the i-th link computed relative to its centre

of mass F

and expressed in the robot base frame F

. The Jacobian matrix

(q) is such that

(q)

q, (5.74)

and since the kinetic energy of a rigid link i is

, (5.75)

5.1. LAGRANGIAN MECHANICS 93

the total kinetic energy of the robot becomes

K =

i=1

(5.76)

i=1



(q)





(q)



q (5.77)

i=1

T i

(q)



(q)



q (5.78)

M(q)

q, (5.79)

where (5.73) was used.

The Coriolis matrix C(q,

q) is a n × n matrix where the (i, j)-th element

is given by

C(q,

i,j

k=1

i,j,k

(q)

(5.80)

with

i,j,k

(q) =



∂M(q)

i,j

∂q

∂M(q)

i,k

∂q

−

∂M(q)

j,k

∂q



, (5.81)

the n × n × n matrix of Christoﬀel symbols.

The gravity vector g(q) is a n × 1 vector that contains the components of

the generalized forces due to gravity with

g(q) =

∂U

∂q

(5.82)

g(q)

∂U

∂q

(5.83)

j=1

∂h

(q)

∂q

, (5.84)

where h

(q) is the height above the base of the robot of the j-th link centre of

mass, and g is the gravity constant. Determining the gravity vector requires

deﬁning every h

(q) as a function of the joint coordinates, and then comput-

ing

∂h

(q)

∂q

for i = 1, ..., n. The gravity vector expresses the torques that are

required to hold the robot immobile in a given conﬁguration.

94 CHAPTER 5. MANIPULATOR’S DYNAMICS

5.1.8 An Outlook on Lagrangian and Newtonian Me-

chanics

Comparing Lagrangian and Newtonian mechanics, a few noteworthy caracter-

istics can be pointed out to help a practitioner choose which framework is the

most adequate for the analysis of a given problem.

Lagrangian mechanics is advantageous as it implicitly respects motion con-

straints through the choice of generalized coordinates. With the Lagrangian

formulation, no need to explicitly enforce constraints as an additional step

(which needs to be done when using Newtonian mechanics). Furthermore, the

Lagrange equation is very short and self-contained, making some derivation

simpler than their Newtonian counterpart. Obviously, the dynamics described

by the equations is the same.

In many scenarios, the choice of generalized coordinates is far from ob-

vious, making the use of the Lagrangian formulation more diﬃcult. Also,

the Lagrange-Euler equation requires to perform diﬀerentiation, which is not

required with the Newtonian formulation. Most importantly, and this is prob-

ably why Newtonian mechanics is predominantly used in robotics, performing

inverse dynamics on a regular 6-DoF manipulator with the Lagrangian formu-

lation is about 100 times more computationally expensive. Direct dynamics is

also more eﬃciently performed when using the Newtonian framework.

5.2 Inverse Dynamics for Control

Controlling a serial manipulator consists in applying torques at its joints such

that the robot reaches a speciﬁc conﬁguration where each link is in a desired

pose. The Recursive Newton-Euler Algorithm (RNEA) computes the kine-

matics and dynamics of serial chains of rigid bodies connected with revolute

or prismatic joints in an iterative manner. The RNEA can be run for each

timestep of a robot position trajectory to get the sequence of torques that

needs to be applied by the motors to follow the trajectory. The algorithm

consists in two main steps: (1) computing the kinematics of the links starting

from the ﬁxed base and following the chain up to the tip, and (2) computing

the forces and torques that each link is subject to, starting from the tip and

following the chain down to the ﬁxed base. Each step is repeated i times for a

manipulator with i links but step (1) needs to be done for all links before step

(2) can be started for any link. The i-th link is assumed to be actuated by a

joint whose general coordinate is q

(it can be any type of joint but we will

restrict ourselves to revolute and prismatic joints) — the RNEA also assumes

that

and

are known for each link.

5.2. INVERSE DYNAMICS FOR CONTROL 95

i+1

Figure 5.1: Reference frame F

is ﬁxed on the i-th link with its origin located

on the actuation axis, which deﬁnes the Z direction. The point highlighted

with is the centre of mass of the i-th link whose frame F

is aligned with

. The frame F

is the inertial frame to which the base of the robot is

assumed to be rigidly attached. The i-th joint is actuated by q

, and

5.2.1 Kinematics Iterations

The kinematics of the base link (or 0-th link) is known, by deﬁnition, as the

base is assumed to be rigidly attached to an inertial frame F

. Consequently,

the iterative algorithm starts by computing the kinematics of the ﬁrst link

based on the known kinematics of the base and the torque applied to the ﬁrst

joint. The kinematics of the second link can then be computed from the one

of the ﬁrst and the kinematics of the (i + 1)-th link can be computed from the

one of the i-th link. Speciﬁcally, the kinematics of each link’s centre of mass

is needed as it is used in the Newton-Euler equations (4.103) and (4.104),

which are used for the dynamics iterations.

The velocity of the 0-th link is assumed to be zero but could be set to some

other values if the robot was on a mobile base. The acceleration of the base

(or 0-th link) can be set to the opposite of the gravity acceleration to allow

the robot to compensate for the force of gravity, which is usually the desired

behaviour.

Table 5.1: Values of some kinematic variables depending on the type of joint

and assuming that the actuation axis is in the same direction as the Z axis of

i−1

Revolute Joint 0



0 0





0 0



Prismatic Joint



0 0





0 0



The angular velocity of the link attached to a revolute joint computed

96 CHAPTER 5. MANIPULATOR’S DYNAMICS

relatively to the previous link will be non-zero while the linear velocity will be

zero. For a prismatic joint, its the opposite, as outlined in Table 5.1.

Using values from Table 5.1 depending on the type of joint, the kinematics

of the (i + 1)-th is given from the one of the i-th link by

i+1

| {z }

0 if Prismatic

(5.85)

i+1



i+1



i+1

| {z }

0 if Revolute

(5.86)

i+1

(5.87)

i+1



i+1



i+1

| {z }

0 if Prismatic

(5.88)

i+1



i+1



(5.89)

i+1



i+1



i+1

| {z }

0 if Revolute

i+1

+ 2

i+1

(5.90)

i+1



i+1





i+1



i+1

which is derived from (3.30), (3.3), (3.20), and (3.4).

5.2.2 Dynamics Iterations

After having performed the kinematic analysis, the velocities and accelerations

of each link can be used to compute the forces and/or torques Q that need to

be applied in order to ﬁght inertia and set the robot in motion. Since the robot

is assumed to be a serial chain of links, each link has exactly two neighboring

links except from the last link that has only a single one. The reaction force

on the last link depending only on its own inertia (and possibly the payload’s),

the process is performed iteratively starting from the link farthest from the

base.

With each iteration, the actuation force of the i-th joint is computed from

(4.115) and (4.116) based on the actuation force of the (i + 1)-th joint as

5.3. DIRECT DYNAMICS FOR SIMULATION 97

Table 5.2: Values of some dynamics variables depending on the type of joint

and assuming that the actuation axis is in the same direction as the Z axis of

i−1

Revolute Joint 0



0 0 Q



Prismatic Joint



0 0 Q



follows:

i−1

i+1

(5.91)

= m

i+1

(5.92)

i−1

i+1

(5.93)





i+1



i+1



i+1

(5.94)





(5.95)

where m

is the mass of the i-th link and

is the inertia tensor of the i-th

link computed relative to its centre of mass and expressed in F

such that it

is independent of the conﬁguration of the robot.

Friction in the joints can be taken into account with

+ µ

sign(

) + µ

(5.96)

where µ

is the Coulomb static friction coeﬃcient and µ

is the viscuous fric-

tion coeﬃcient of the joint.

5.3 Direct Dynamics for Simulation

The direct dynamics problem is the one of ﬁnding the motion that would result

from applying a sequence of driving forces at the robot’s joints. In other words,

from the equation of motion in (5.72), the goal is to solve

M(q)

q = Q − (C(q,

q + g(q)) (5.97)

for

q when

q and q are known. It is assumed that the initial conﬁguration

and velocity are known. The mass matrix M(q) can be computed from (5.73)

and the generalized forces due to the term C(q,

q + g(q) can be obtained

by computing the inverse dynamics with

q = 0. Gaussian elimination, or

98 CHAPTER 5. MANIPULATOR’S DYNAMICS

any other method, can then be used to solve the systems of equations for the

acceleration vector

q. A numerical integration scheme (e.g., Euler integration,

Runge-Kutta) can ﬁnally be used to obtain values for

q and q at the next

timestep from the acceleration

q. The newly computed

q and q are used as

the initial condition for the subsequent iteration and this process is repeated

for every timestep of the trajectory.

5.4 Calibration and Identiﬁcation

In Sec. 5.2 and Sec. 5.3, we showed how modeling a robot manipulator as a

series of actuated rigid links allows us to relate forces and torques applied at

the actuators to the motion of the links. This ubiquitous approach to robot

control is relatively simple, computationally eﬃcient, and can be accurate in

practice. Nonetheless, when working with real robots (in contrast to com-

puter simulations), diﬀerences between our rigid body model and the actual

robot will inevitably arise. These modeling errors can be due to incorrect

model assumptions (e.g., assuming rigid links when the robot will slightly

bend) and incorrect model parameters (e.g., in the relative poses of the links).

While modeling assumptions will determine the equations describing the robot

model, coeﬃcients appearing in the equations are called model parameters and

typically have to be identiﬁed through a calibration procedure.

As illustrated in Fig. 5.2, the calibration procedure is based on the idea

of forming a loop in the kinematic structure of the robot. Sometimes, closing

the loop requires taking a measurement with a sensor (e.g., a camera). For

instance, in Fig. 5.2, section #4 of the kinematic loop denoted by arrows repre-

sents an observation made with the camera mounted on the end-eﬀector of the

robot. Without this observation, calibration would not be possible in Fig. 5.2

as the kinematic chain would be open. When the robot end-eﬀector is moving

throughout the calibration procedure and a sensor is used to observe its pose

(like in Fig. 5.2), the calibration is called open-loop. In contrast, some calibra-

tion methods keep the end-eﬀector ﬁxed in a speciﬁc pose while the robot is

moved to diﬀerent conﬁgurations, which is called closed-loop calibration. Since

closed-loop calibration does not require the use of a sensor (whose observations

are always noisy), it can result in a simpler setup and identity parameters more

accurately. However, closed-loop calibration can only be achieved with certain

types of robots, such as serial manipulators with more than 6 degrees of free-

dom, which allow links of the robot to move while the end-eﬀector is ﬁxed in

space. In contrast, open-loop calibration can be performed with any type of

robot, provided that a sensor is available to observe the end-eﬀector pose.

Identifying all the parameters of a robot model is usually too laborious

5.4. CALIBRATION AND IDENTIFICATION 99

Base

End-eﬀector

Camera

{w}

Figure 5.2: Serial manipulator equipped with a camera at its end-eﬀector. The

robot model is split into four sections, each represented by an arrow. Forming

a loop in the kinematic structure enables the calibration procedure to identify

some parameters of the model.

due to the (typically) large number of parameters and the complexity of the

complete model. Instead, most calibration procedures assume that some pa-

rameters are known and focus on identifying a subset of parameters. While this

approach simpliﬁes each step of the overall calibration procedure, it might also

require to iterate over the procedure several times (each time using the newest

set of parameters) to obtain a suﬃciently accurate model due to the interde-

pendence of the parameters. For instance, calibrating the robot in Fig. 5.2

might require iterating between calibration section #2 (the robot arm) and

section #1 and #3 (respectively the pose of the base and the pose of the cam-

era relative to the end-eﬀector). While calibrating #2 is usually called robot

kinematic calibration, determining #3 is termed hand-eye calibration (referring

to the camera as the eye and the gripper as the hand ). Sometimes, #1 and

#3 are jointly identiﬁed in a procedure called hand-eye-robot-world (HERW)

calibration.

In the following, we will ﬁrst present a method for identifying the kine-

matic parameters of a serial manipulator, which requires ﬁnding a solution to

a non-linear optimization problem. Then, we will present a hand-eye-robot-

world calibration procedure that involves solving a linear system of equations.

Finally, we will go one step further and introduce inertial parameters identi-

ﬁcation, which is the process of determining some of the dynamic parameters

of the robot model.

100 CHAPTER 5. MANIPULATOR’S DYNAMICS

5.4.1 Robot Arm Kinematic Calibration

Calibrating a manipulator is crucial to ensure that the robot follows prescribed

trajectories as accurately as possible. In turn, following trajectories accurately

is essential to avoid colliding with objects in the environment and to grasp or

place objects where they are intended to be. Due to the length of its links,

small diﬀerences between the robot model and the actual robot can result

in large end-eﬀector positioning errors. These diﬀerences can be due to small

manufacturing defects or assembly errors, and are inevitable in practice. While

some robot manufacturers will perform calibration before shipping the robot,

all robot users should be on the lookout for inaccuracies in the robot model

and perform calibration if necessary.

A Primer on Iterative Least Squares Optimization

A manipulator’s forward kinematics is usually described as a non-linear func-

tion of the joint coordinates q and robot model parameters ϕ. For instance,

in Sec. 2.7.1, the forward kinematics of a serial manipulator is given by the

product of exponentials

(θ) = e

]

. . . e

]

M, (5.98)

in which S

is the screw axis of the i-th joint, θ

is the joint coordinate of the

i-th joint, and M is the pose of the end-eﬀector when all joint coordinates are

zero. In the product of exponentials, the parameters of the robot model deﬁne

screw axes and the end-eﬀector zero conﬁguration pose. Clearly, the pose of

the end-eﬀector

does not vary linearly with respect to the parameters.

Dealing with non-linear models is typically much more diﬃcult than dealing

with linear models, which can usually be solved with linear algebra. A common

approach to identifying the parameters of a non-linear model relies on solving

a sequence of small linear sub-problems, hoping to converge to the solution

of the overarching non-linear problem. Assuming that we have a non-linear

function f(ϕ) representing a non-linear model parametrized by ϕ, the Taylor

series expansion of the model yields

y = f(ϕ + ∆ϕ) (5.99)

= f(ϕ) +

∂f

∂ϕ

∆ϕ + . . . , (5.100)

where the ellipsis indicates higher order terms and where y is the output of

the model. Neglecting higher order terms (possibly a bold assumption), the

5.4. CALIBRATION AND IDENTIFICATION 101

model becomes

y = f(ϕ) +

∂f

∂ϕ

∆ϕ (5.101)

y − f (ϕ)

| {z }

∂f

∂ϕ

|{z}

∆ϕ

|{z}

(5.102)

b = Ax, (5.103)

the familiar linear system of equations that can be solved for x with

x =





−1

b. (5.104)

If the Jacobian matrix A directs the optimization process towards the solution,

the diﬀerence between f (ϕ + ∆ϕ) (i.e., the model output) and the actual

output (e.g., the measured end-eﬀector pose) should decrease. However, the

approximation made when neglecting higher order terms will usually result in

the parameter update ϕ + ∆ϕ not being optimal and several iterations of the

linearization procedure will be required to converge suﬃciently close to the

solution (with no guarantee of actually converging towards a good solution).

The process by which the parameters are iteratively updated by solving a

sequence of least squares problems is called iterative least squares optimization

and can be seen as a gradient descent method.

A Method Based on Screw Axes

The following kinematic calibration method is due to [1] and is based on the

idea of representing robot axes as unit screws. Unit screws are particularly

appealing in the context of kinematic calibration as they can seamlessly be used

to represent both revolute and prismatic joints. Perhaps more importantly,

screw axes can be parametrized with four parameters, three of which represent

the direction ˆs of the screw axis and one of which represents the pitch h that

relates linear and angular motion along the screw axis (see Sec. 2.5.2 for more

details about screw axes and twists). Hence, when identifying the parameters

of a robot model, a maximum of four distinct parameters can be identiﬁed for

each joint. When calibrating a robot, parameter redundancy (e.g., having more

than four parameters per joint) can result in increased diﬃculty in identifying

the parameters as the optimization procedure searches in a larger solution

space. The DH parametrization of a serial robot (see Sec. 2.7.2) also uses only

four parameters per joint, and has been proved to be minimal (in the sense

that no convention with fewer parameters exists). While the DH convention

102 CHAPTER 5. MANIPULATOR’S DYNAMICS

has been used in robot kinematic calibration, it can become discontinuous

under certain circumstances and is therefore more diﬃcult to optimize over.

With serial robots, a maximum of 4R + 2T + 6 parameters can be identi-

ﬁed, where R is the number of revolute joints, T is the number of prismatic

joints, when the end-eﬀector pose is observed. When only the position of

the end-eﬀector is observed, the number of identiﬁable parameters is reduced

to 4R + 2T + 3 as the orientation of the end-eﬀector cannot be determined.

While observing the position provides three pieces of information, observing

the full pose provides six. Hence, prior to performing the calibration, at least

(4R + 2T + 6)/6 diﬀerent pose measurements should be made to ensure the

identiﬁability of the parameters. In practice, due to sensor noise, it is recom-

mended to take many more measurements than the minimum number required

to achieve accurate identiﬁcation.

On a per-joint basis, a set of four parameters are identiﬁed but, as described

in Sec. 2.5.2, screw axes lie in a six dimensional vector space. Hence, there is

a need to deﬁne a mapping between a set of four parameters and the screw

axis of a joint. For the i-th joint whose unit screw axis is





, (5.105)

the mapping can be deﬁned as follow:

, (5.106)

× (

+ 1) / ∥

+ 1∥, (5.107)

(5.108)



3×1



. (5.109)

The matrix B

is a 6 × 4 matrix that maps the four parameters of the i-th

joint to a unit screw axis

with

= B

, (5.110)

where ϕ

is the vector of parameters for the i-th joint. All B

matrices are

bundled in a large block-diagonal matrix

B =







0 . . . 0

0 B

. . . 0

0 0 . . . B







, (5.111)

5.4. CALIBRATION AND IDENTIFICATION 103

where B

is an identity matrix whose size is 3 × 3 if only the position of the

end-eﬀector is observed, or 6 × 6 if the full pose is observed.

According to [1], a linear approximate of the end-eﬀector pose diﬀerence

that is due to an error in the i-th joint is given by

= Ad (

i−1

) − Ad (

) , (5.112)

where Ad (·) is the adoint representation of a pose, as deﬁned in (2.99), and

is the pose of the i-th link relative to the robot base F

. The end-eﬀector

pose diﬀerence that is due to an error in the zero conﬁguration pose is simply

= Ad (M) , (5.113)

where M =

q=0

is the pose of the end-eﬀector when all joint coordinates

are zero. As mentioned in Sec. 2.5.2, the adjoint representation can be used

to express a twist in a diﬀerent coordinate system. Given that twists describe

rigid motions (the diﬀerential of a pose diﬀerence), it might not be surprising

that the adjoint representation appears in the relationship between end-eﬀector

pose diﬀerence and screw axis error. The Jacobian matrix of the linearized

problem is then given by

Q =



. . . Q



, (5.114)

where J is the number of joints of the robot, and where Q is a 6×6(J+1) matrix

mapping joint screw axes errors to end-eﬀector twist errors. Mathematically,

logm



ee b

−1



≈ QB∆ϕ

|{z}

∆ν

, (5.115)

where logm(·) is the matrix logarithm deﬁned in (2.98) and

is the pose

of the end-eﬀector as predicted by the robot model with parameters ϕ. If

end-eﬀector positions are measured instead of full poses, the left-hand side of

(5.115) is replaced by the position diﬀerence

−

The calibration process consists in solving (5.115) for the parameter update

∆ϕ with the method outlined in Sec. 5.4.1 where

A = QB, (5.116)

b = logm



ee b

−1



−

, (5.117)

x = ∆ϕ. (5.118)

104 CHAPTER 5. MANIPULATOR’S DYNAMICS

At every iteration, the robot model is updated with

= Ad



∆ϕ



, (5.119)

M = e

∆ϕ

M, (5.120)

where

is the screw axis of the i-th joint, ∆ϕ

is the parameter update for

the i-th joint, and e

is the matrix exponential deﬁned in (2.95).

5.4.2 Hand-Eye-Robot-World Calibration

This is a placeholder for the coming section on hand-eye-robot-world calibra-

tion.

5.4.3 Inertial Parameters Identiﬁcation

Accurate model-based control of robot arm requires a precise knowledge of

the inertia that the robot will need to ﬁght to accelerate the links of the arm.

Indeed, the direct and inverse dynamics algorithms rely on the knowledge

of the mass, centre of mass, and inertia tensor of each link. The inertial

parameters of the i-th link can be grouped into the vector



, (5.121)

[x], m

[y], m

[z], (5.122)

[xx],

[xy],

[xz],

[yy],

[yz],

[zz]



(5.123)

in which the unique elements of the zero-th, ﬁrst, and second moments of the

mass distribution are concatenated. Unfortunately, in general, not all inertial

parameters can be identiﬁed. Indeed, parameters that do not contribute to

the inertia fought by the robot when its joints are accelerating cannot be

dynamically identiﬁed and need to be obtained from CAD models. The set of

parameters that can be identiﬁed are called base parameters and can often only

be identiﬁed in linear combinations due the fact that the motion is governed

by the sum of the inertias.

For instance, only a single element of the inertial parameters of the ﬁrst

link proximal to the base can possibly be identiﬁed since that link will only

ever experience acceleration about the axis of the ﬁrst joint – the motion of

the ﬁrst link is limited. Furthermore, the sensing of the inertia of the ﬁrst

link is usually only done through a joint torque sensor located as close as

possible to the rotor – sensing is limited and the mass of the ﬁrst link is never

measured. In contrast, a conventional 6-axes force-torque sensor attached to

5.4. CALIBRATION AND IDENTIFICATION 105

the end-eﬀector of a 6-dof manipulator can be used to identify the full set of

inertial parameters of a load attached to the sensor. Indeed, the robot has

the capability to generate motions where the dynamics will depend on all the

parameters, and the sensor has the capability to measure all the forces and

torques experienced by the load.

One technique used to eliminate the parameters that are not base param-

eters, and therefore do no contribute to the dynamics, consists in running the

dynamics iterations of the inverse dynamics algorithm (from Sec. 5.2) sym-

bolically for a diverse set of robot conﬁgurations. It will become apparent

that some parameters are never used in the equations and that others only

appear in linear combinations. Another technique relies on a singular values

decomposition of the regressor matrix A that is deﬁned below.

Setup Using Newtonian Mechanics

From (4.108), the equations of motion for the i-th link are

= m

+ m





+ m

(5.124)

= m

(5.125)

and represent the forces due to the motion of the i-th link. Using skew-

symmetric matrices, the equations can be expressed as









(5.126)













−









− ω

+ ω

−ω

− ω

+ ω

− ω

−ω

− ω

+ ω





vech





where

= c

is the location of the centre of mass of link i relative to the

reference frame ﬁxed in link i, and vech(·) is the vector-half operator which

extracts the unique elements of

in the order shown in (5.121). Concate-

nating the matrices that are pre-multiplying the inertial parameters in (5.126)

106 CHAPTER 5. MANIPULATOR’S DYNAMICS

such that



3×9

3×1

3×9



(5.127)

3×1













3×6

3×1

−





3×6







3×10

1×4

− ω

+ ω

−ω

− ω

1×4

+ ω

− ω

−ω

1×4

−ω

− ω

+ ω







produce a data matrix that can be used in

= A

(5.128)

to compute the forces and torques that are due to the motion of the i-th link

given its inertial parameters in ϕ

For serial manipulators, a given joint has to ﬁght the inertia of the links

that are between the joint and the end eﬀector or load. Therefore, the torques

induced by the motion of the links further down the serial chain on the i-th

joint is





1×5







−T



(5.129)

in which the torque sensor is assumed to be about the joint Z axis and where

the adjoint matrix Ad (·)

−T

is used to express a wrench vector in another

frame, similarly to (3.16).

The equation in (5.129) is again linear in the inertial parameters such that



··· Q



| {z }

= K



··· ϕ



| {z }

(5.130)

where



1×5







−T

(5.131)

is the torque contribution on joint i from the inertia of link j from the m-th

observation. When multiple observations are obtained, the matrices Q and K

in (5.130) are stacked as



··· Q



| {z }



··· K



| {z }

ϕ (5.132)

5.4. CALIBRATION AND IDENTIFICATION 107

such that the whole problem is reduced to the linear relationship

Q = Kϕ. (5.133)

When the goal is to identify a load, and that a force-torque sensor at the

end-eﬀector is available, all terms in the equations of motion should be com-

puted relative to the sensor frame such as to be compatible with the readings

from the sensor. The data matrix can be built directly without resorting to

any additional steps done for the robot link identiﬁcation.

Note on Using the Lagrangian Formulation

Similar to the setup using Newtonian mechanics, the goal is to express the

generalized forces as a linear combination of the inertial parameters (as in

(5.133)). To do so, the elements in (5.72) that depend on the kinematics must

be separated from the ones that are function of the inertial parameters. Due

to the complicated structure of the equations in (5.73) and (5.80), it is far from

obvious to come up with a linear relationship between generalized velocities

and inertial parameters. An alternative is to perform the forward kinematics

from q,

q to obtain the task-space kinematics of each link. Then, the

identiﬁcation setup using Newtonian mechanics could be done.

Identiﬁcation Procedure

When the regressor matrix has full rank, the solution to the ordinary least

squares problem

min

||Kϕ − Q||

(5.134)

ϕ =





−1

Q . (5.135)

However, K

K is not invertible when K is not full-rank, which is usually the

case when either links motion or joints sensing is limited – in that case the

regressor must be made full-rank by reducing the set of inertial parameters to

the base parameters.

A load ﬁxed to a 6-axes force-torque sensor at the end-eﬀector of a 6-dof

manipulator will produce a matrix K that has full-rank due to the fact that

motion and sensing is not limited in this case. Consequently, the inertial

parameters of the load can be estimated by solving (5.134). If a bias exists in

the force vector Q, for instance if the torques at the joints are used to compute

the load wrench, the identiﬁcation trajectory can be performed without load

108 CHAPTER 5. MANIPULATOR’S DYNAMICS

(0) and then with the load (L), and the diﬀerence Q = Q

− Q

can be used

in (5.134).

For a given trajectory, some parameters might be very poorly excited and

will hinder the identiﬁcation of the other parameters. Sometimes, it is best

to eliminate those parameters to allow a more accurate identiﬁcation of the

others. To do so, the columns of the regressor matrix K are ﬁrst normalized

to unit length such that each parameter is scaled equally. Then, the singular

values decomposition (SVD) of the scaled regressor is performed with

SVD(K) = UΣV

(5.136)

where U and V are orthogonal matrices, and Σ is the diagonal matrix of

singular values. The condition number is then computed with

κ(K) =

max

min

(5.137)

where σ

max

is the largest singular value and σ

min

is the smallest. The condi-

tion number expresses the sensitivity of the estimate to noise in the regressor

matrix and in the observation vector. As a rule of thumb, a condition number

below (or on the order of) 100 implies that the regressor is well-conditioned,

while a condition number on the order of 1000 implies that the regressor will

be very sensitive to noise and that the estimate will be uncertain.

To attenuate the impact of the poor identiﬁability of some parameters on

the overall estimate, parameters associated with low singular values should

be iteratively eliminated until the condition number becomes more reasonable

(parameters associated with singular values that are zero are unidentiﬁable).

To do so, the column of V that is associated with σ

min

is inspected for an

element that would be signiﬁcantly larger than the other elements. If such

an element exists, and is at the j-th position in the column, the associated

parameter ϕ

is eliminated and the procedure can be repeated.

An estimate for the uncertainty in the identiﬁed parameters

ϕ can be

obtained by computing a covariance matrix

S estimate through the sum of

squared residuals

= (Kϕ − Q)

(Kϕ − Q) (5.138)

with

S =





−1

(5.139)

where ν is the number of statistical degrees of freedom, which is given by the

total number of observations (e.g. torques measurements) minus the number

of parameters to estimate.

5.5. KEY CONCEPTS 109

Maximizing the minimum singular value σ

min

, the condition number κ(K)

in (5.137), or the determinant of (K

−1

are all observability indices that

can be used to evaluate the quality of an identiﬁcation trajectory.

5.5 Key Concepts

• The minimal set of coordinates that can be used to describe the position

of all points on a robot are the generalized coordinates.

• The space spanned by the generalized coordinates is the conﬁguration

space. Each point in this space is a robot conﬁguration.

• Holonomic constraints applied to a robot reduce the number of gener-

alized coordinates for the robot. Holonomic constraints can depend on

the robot conﬁguration and on time but not on the robot velocity or

acceleration.

• The Jacobian matrix is a matrix of partial derivatives describing the

relationship between the generalized velocities and the task-space veloc-

ities.

• While the space Jacobian expresses spatial velocities in the inertial frame,

the body Jacobian expresses spatial velocities in the body frame. The

adjoint representation relates the two Jacobians.

• Each column of the space Jacobian describes the screw axis of the cor-

responding joint.

• The transpose of the inverse of the space Jacobian maps joint-space forces

to task-space forces.

• When the rank of the Jacobian is less than 6, the robot cannot generate

all task-space velocities.

• At a singular conﬁguration, the Jacobian becomes rank-deﬁcient.

• With over-actuated robots, the null-space of the Jacobian is not empty

and some robot motion can happen while the end-eﬀector is stationary.

• When a robot approaches a singular conﬁguration, very large joint ve-

locity are required to make any progress. At this point, a manipulability

index can be used to move away from the singularity.

110 CHAPTER 5. MANIPULATOR’S DYNAMICS

• The Hessian describes how the Jacobian changes with respect to the

generalized coordinates. It is used to map joint-space kinematics to

task-space accelerations.

• The Lagrangian formulation of rigid-body dynamics relates changes in

energy levels to generalized forces.

• Using the Lagrangian formulation can be advantageous when a system

must respect a set of constraints. Finding the set of generalized co-

ordinates respecting the constraints will result in equations of motions

implicitly respecting the constraints. In practice, the Newtonian formu-

lation is often preferred as it is faster to compute.

Appendix A

A Summary’s Summary

A.1 Geometry

A line vector has magnitude, direction, and a starting point, while a free

vector only has the ﬁrst two.

A transformation is done on a column vector by pre-multiplying it by

the transformation.













= T













(A.1)

Vectors can be added together if they are expressed in the same reference

frame.

(A.2)

Any rotation can be expressed as a sequence of three principal rotations.

Passive rotations are compounded through post-multiplication and are ex-

pressed about the ﬁxed frame. Active rotations are compounded through

pre-multiplication and are expressed about the moving frame.

A cartesian reference frame is deﬁned from three unit vectors that are

orthogonal to each other.

·F





ˆa













ˆa





(A.3)

Element ij -th is the cosine of the angle between a

and b

. The rows of

express the direction of an axis in F

relative to axes in F

111

112 APPENDIX A. A SUMMARY’S SUMMARY

Rotations have nine elements but six constraints: three comes from the

orthogonality of the columns, and three comes from the normality of the

columns. A rotation is proper if the determinant is positive, meaning that

there is no reﬂection. The Special Orthogonal Group SO(3) is the group of

rotations.

SO(3) =



3×3

| R

R = 1

3×3

, det (R) = +1



(A.4)

A group is a set on which a binary operator O{} is deﬁned, respecting four

axioms: closure (O{a, b} ∈ G), identity (O{a, I} = O{I, a} = a), inverse

(O{a, a

−1

} = O{a

−1

, a} = I), and associativity (O{c, O{a, b}} = O{a, O{b, c}}).

For rotation matrices, the group operation is matrix multiplication.

Euler’s rotation theorem states that any sequence of rotations can be

expressed as a single rotation θ about some axis ˆω. Axis-angle can be useful

to compute the rotation that brings ˆu onto ˆv with

ˆω =

ˆu × ˆv

∥ˆu × ˆv∥

(A.5)

θ = arccos (ˆu · ˆv) (A.6)

Rodrigues Formula transform an axis-angle into a rotation matrix and de-

ﬁnes the matrix exponential

R = e

[ˆω]

= 1

3×3

+ sin(θ) [ˆω]

+ (1 − cos(θ)) [ˆω]

(A.7)

The inverse operation can be done with the matrix logarithm that is singular

at R = 1 and nearly singular for small rotations.

Euler parameters introduce an additional parameter and constraint to

alleviate this problem.

e =







cos(θ/2) ˆω sin(θ/2)



(A.8)

∥e∥ = 1 (A.9)

Euler parameters can be encoded in unit quaternions to operate on vec-

tors with

q ⊗



0, v

, v



⊗ q

∗

(A.10)

which corresponds to Rv. Quaternions rotate twice slower than rotations, so

two antipodal quaternions map to the same rotation. A random orienta-

tion can be obtained by sampling four numbers from a Gaussian distribution,

and normalizing three of them. Interpolating between quaternions can be

A.1. GEOMETRY 113

done by computing the rotation diﬀerence, getting the axis-angle, and inter-

polating.

δ = q

∗

⊗ q

(A.11)

θ = atan(∥δ

∥/δ

) (A.12)

ˆω = δ

/ ∥δ

∥ (A.13)

= q

⊗



cos(tθ)

ˆω sin(tθ)



(A.14)

Its trivial to enforce a unit quaternion’s constraint while its much more

diﬃcult to do the same with rotations.

The Chasles-Mozzi theorem states that any rigid transformation can

be expressed as a displacement over the thread of a screw. A screw is

deﬁned by its unit axis ˆs, thread pitch h, and a point q on the axis. The pitch

is the ratio of linear to angular motion.

A twist is a motion about a screw , which can be considered as the co-

ordinate system the motion is deﬁned relative to. A unit-screw S is expressed

from the twist components as

S =











ω −

ω × q

if ∥ω∥ = 1

if ∥ω∥ = 0 and ∥v∥ = 1

(A.15)

such that a rigid transformation can be deﬁned as Sθ If the θ instead expresses

the rate of motion, the velocity twist is deﬁned as

ν =







hω − ω × q

ˆsθ



(A.16)

where the linear velocity v is the sum of a component along the axis and a

component orthogonal to the axis (leading the thread towards and away from

the axis).

The adjoint representation of a rigid transformation can be used to

change the reference frame that a twist is expressed in with

Ad (T) =



R [p]

3×3



(A.17)

A pose can be reversed through the inverse only if it is expressed in the

114 APPENDIX A. A SUMMARY’S SUMMARY

same frame it is deﬁned relative to.

c c



c c

a b

0 1



(A.18)

= (

)

−1

(A.19)

(

)

−1

(A.20)

Forward kinematics computes the end-eﬀector pose from the joint posi-

tions.

= FK(q) (A.21)

Unit-screws can be used to deﬁne every actuation axis of a robot in its zero

conﬁguration. Prismatic joints are deﬁned with ω = 0 and with v being the

axis expressed in the robot base frame.

S =



v 0



(A.22)

Revolute joints are deﬁned with h = 0, with ω being the axis expressed in the

robot base frame, and q being any point on the screw axis.

S =



−ω × q ω



(A.23)

With the pose of the end-eﬀector being M =

θ=0

the forward kinematics

is given by the products of exponentials formula

(θ) = e

]

. . . e

]

M (A.24)

The structure of a serial robot can be deﬁned from a sequence of homoge-

neous transformations, each describing the frame of a link relative to the frame

of the previous link. The Denavit-Hartenberg (DH) convention is a min-

imal parametrization that uses only four values per joint. Two constraints

are needed:

• the ˆz

axis must coincide with the i-th joint actuation axis and direction,

• the ˆx

i−1

axis must be perpendicular to ˆz

After the frame are placed, the parameters can be extracted as

1. a

i−1

is the distance along ˆx

i−1

between ˆz

i−1

and ˆz

2. α

i−1

is the angle about ˆx

i−1

that would bring ˆz

i−1

to ˆz

if they shared

their origin,

A.2. KINEMATICS 115

3. d

is the distance along ˆz

between the intersection with ˆx

i−1

and the

origin of the frame,

4. ϕ

is the angle about ˆz

that would bring ˆx

i−1

to ˆx

if they shared their

origin.

such that the transformation

i−1

= Tran(ˆx

i−1

, a

i−1

)Rot(ˆx

i−1

, α

i−1

)Tran(ˆz

, d

)Rot(ˆz

, ϕ

) (A.25)

is deﬁned for each joint.

A.2 Kinematics

The derivative of vectors are also vectors, so the derivative of the position

vector is the velocity vector.

All points on a rigid body can have diﬀerent kinematics but they all

share the same description of the kinematics. The velocity of all points

within rigid body can be describes with the linear and angular velocities. The

linear velocity can be thought as a measure of the ﬂow of points passing

through the origin.

Angular velocities can be simply added

b b

(A.26)

but the real velocity of a point is the product of linear and angular compo-

nents

(A.27)

since

R (a × b) = (Ra) × (Rb) (A.28)

The time-derivative of a rotation is given by

(A.29)

such that the time-derivative of a rotated vector is

(

) =

(A.30)

The adjoint of a transformation can be used to express velocity in a diﬀerent

reference frame with





= Ad (

)

(A.31)

116 APPENDIX A. A SUMMARY’S SUMMARY

which can also simplify the composition of velocities with

+ Ad (

)

(A.32)

The acceleration of a point {o} that is deﬁned relative to a rotating

reference frame F

is given by

+ 2

× (

) +

(A.33)

A.3 Dynamics

Dynamics studies how forces inﬂuence the motion of a body.

An inertial frame is a frame that does not accelerate, implying that it

does not rotate since a change in the velocity direction would involve acceler-

ation. A practical point of view is that any frame in which Newton’s laws

are suﬃciently accurate can be considered an inertial frame.

The N-th moment of a mass distribution is given by

ρ(p)dV (A.34)

to produce the zero-th (mass), ﬁrst (centre of mass), and second moment

(inertia tensor)

ρ(p)dV =

ρ(p)dV = m (A.35)

ρ(p)dV = mc (A.36)

ρ(p)dV =

[p]

ρ(p)dV = I (A.37)

The inertia tensor is symmetric positive-deﬁnite (due to rotational ki-

netic energy being positive) where the ij-th element expressed how torque

applied about axis e

will produce angular acceleration around axis e

Equivalently, the ij-th element expressed how a rotation about e

will pro-

duce angular momentum around e

. The diagonal elements are the mo-

ments of inertia while the oﬀ-diagonal elements are the products of inertia.

Symmetries in the mass distribution will cancel products of inertia, which is

desired for a car wheel such that it does not wobble.

The parallel axis theorem can move the origin of the reference frame

I =

I − m [

]

[

]

(A.38)

A.3. DYNAMICS 117

and a similarity transform can change the alignment of the reference axes

with

I =

(A.39)

The principal axes of inertia along which the mass is mostly distributed

strictly determine the motion of a rigid body about its centre of mass. A

singular values decomposition of the inertia tensor can produce

I =

(A.40)

in which the principal moments of inertia are the eigenvalues.

Like forces, inertia is additive.

The momentum deﬁned as

= m

(A.41)

is a conserved quantity, it does not change over time in a closed system.

The angular momentum is the moment of the momentum and is deﬁned

|{z}

Real

= m

| {z }

Intrinsic

(A.42)

where the intrinsic angular momentum is a vector that can be expressed in a

diﬀerent frame with





ω (A.43)

Konig’s theorem states that the angular momentum is the sum of a

component due to the motion of the centre of mass, and a component due to

the motion of the body about its centre of mass.

ε =

(A.44)

When the inertia tensor is computed relative to the principal axes of inertia,

the intrinsic angular momentum is

= I

(A.45)

Energy (in Joules) is a conserved quantity. “All phenomena depend

on the variation of energy and not on its absolute value” (Maxwell).

The kinetic energy of a body is deﬁned as

K =

(

)

| {z }

Linear

(

) +

b w

| {z }

Rotational

(A.46)

118 APPENDIX A. A SUMMARY’S SUMMARY

where the middle term cancels if the origin of F

is at the centre of mass {c}.

Work is done when applying a force on a mass along a displacement.

W =

f(s) · dr (A.47)

Power is the amount of energy transferred per unit of time.

P =

∆W

∆t

= f ·

p (A.48)

The system inertia matrix

I =



3×3

−m [c]

m [c]



(A.49)

relates momenta to velocities with

E = Iν (A.50)

which is, like the inertia matrix, symmetric and positive-deﬁnite.

The system inertia matrix can also be used to compute the kinetic energy

with

K =

Iν (A.51)

making it a central piece of dynamics. Since the kinetic energy must be posi-

tive, the spatial inertia matrix must be symmetric positive-deﬁnite.

A.3.1 Newtonian Mechanics

Newton’s laws of motion:

1. A body will stay at rest unless a force is acted upon it — inertia exists.

2. A force acting on a body will change the rate of its momentum —

conservation of momentum.

3. Two interacting bodies will be subject to opposed forces.

In an inertial frame, Newton’s laws are observed as

= m

d (

)

= m

(A.52)

d (

)

(A.53)

A.3. DYNAMICS 119

where the origin of F

is at the centre of mass and all quantities are observed

in the inertial frame.

When the origin of F

is at the centre of mass and its axes are aligned

with the principal axes of inertia, the angular momentum is

= I

(A.54)

such that the rotational kinetic energy is

rot

b w



+ I



(A.55)

and Euler’s equations are simpliﬁed to











= I

− (I

− I

)ω

= I

− (I

− I

)ω

= I

− (I

− I

)ω

(A.56)

highlighting that the motion of a body only depends on its principal mo-

ments of inertia.

When the motion of a body is observed in a rotating/non-inertial reference

frame (always), ﬁctitious forces are added to accurately depict the motion

with

= m







+ 2

| {z }

Coriolis

× (

)

| {z }

centrif ugal

| {z }

Euler







(A.57)

= m

(A.58)

It can be useful to express everything in F

such that inertia tensor and centre

of mass do not change over time.

A.3.2 Lagrangian Mechanics

The minimal set of coordinates q that can be used to fully describe the po-

sition of all particles in a system are the generalized coordinates, which

deﬁne the conﬁguration. The number of generalized coordinates is given

by the number of degrees of freedom minus the number of holonomic con-

straints, which is a type of constraint that depends only on the conﬁguration

and possibly on time (e.g. a point on a sphere). Conversely, non-holonomic

constraints are deﬁned by conﬁguration derivatives (like velocity), on the

120 APPENDIX A. A SUMMARY’S SUMMARY

path taken (e.g. rolling sphere), or have coordinates dependencies (e.g. point

in a cube).

The space Jacobian is deﬁned as

(q) =

∂r

∂q

∂r

∂q

···

∂r

∂q

(A.59)

and maps generalized velocities to real velocities through

= J

(q)

q (A.60)

and generalized forces to real forces and torques through

Q = J

(q)

W (A.61)

The wrench can be expressed in a diﬀerent reference frame with

= Ad (

)

−T

(A.62)

If the Jacobian is not square and full rank, it cannot be inverted, and

the reverse mapping has either no solution or many solutions. For under-

actuated robots, some velocity directions are impossible, and the Jacobian

must be shrink to account for that. For over-actuated robots, many joint-space

velocities can achieve the desired task-space velocity and a criteria is needed to

choose how to move (e.g. minimal energy). In singular conﬁgurations, the

Jacobian loses rank and eﬀectively maps generalized velocities to space with

less dimensions than the task-space. In this case, an approximate motion

can be performed by using the pseudo-inverse of the Jacobian

(q)



(q)



−1

(q)

(A.63)

An inﬁnitesimal deviation from the system’s trajectory while still respect-

ing all constraints is called a virtual displacement δr. Applying a force

along δr produces virtual work δW = f · δr = Q · δq.

D’Alembert principle states that

· δr =

· δr (A.64)

from which Lagrange’s equation of motion



∂K

∂



−

∂L

∂q

(A.65)

can be derived, where L = K − U is the total energy of the system.

A.3. DYNAMICS 121

For a serial robot, the equation of motion is written

Q = M(q)

|{z}

mass matrix

q + C(q,

| {z }

Coriolis matrix

q + g(q)

|{z}

gravity vector

(A.66)

where

M(q) =

i=1



(q)





(q)



(A.67)

C(q,

i,j

k=1

i,j,k

(q)

(A.68)

i,j,k

(q) =



∂M(q)

i,j

∂q

∂M(q)

i,k

∂q

−

∂M(q)

j,k

∂q



(A.69)

g(q)

∂U

∂q

j=1

∂h

(q)

∂q

(A.70)

with the gravity vector expressing the torques that are required to hold the

robot immobile.

The total kinetic energy of the system is obtained with

K =

M(q)

q (A.71)

Lagrangian mechanics is advantageous as it implicitly respects motion

constraints and produce shorter derivations. However, it is often not obvious

to choose the right set of generalized coordinates. Also, getting the equations

of motion requires to diﬀerentiate. Most importantly, Newtonian mechanics

requires much less operations to perform inverse dynamics.

The inverse dynamics problem aims to ﬁnd joint torques for an end-

eﬀector trajectory. The Recursive Newton-Euler Algorithm (RNEA) can

be run for each time step of the robot end-eﬀector trajectory to obtain a se-

quence of joint torques that can be applied such that the robot follows the tra-

jectory. The RNEA is done in two successive phases, (1) computing kinemat-

ics from the base, and (2) computing forces and torques from the end-eﬀector.

The kinematics iterations start at the base, and with each iteration the

velocity and acceleration of the COM of a more distal link is computed from

the one of the proximal link. The acceleration of the base of the robot is set

to the opposite of gravity such that the robot compensates. The dynamics

iterations start at the last joint and, for each joint, compute the torque that

must be exerted as a reaction to the wrench exerted by the following joint,

122 APPENDIX A. A SUMMARY’S SUMMARY

and additionally provide the torque necessary to produce the intended motion

of the link between the two joints.

The direct dynamics aims to ﬁnd the motion that would result from

applying a sequence of driving forces. To do so, the equation of motion is

solved for

q and then numerical integration is performed to get

q and q.

The wrench due to C(q,

q + g(q) can be obtained by performing the inverse

dynamics with

q = 0.

Appendix B

Useful Mathematical

Formulas

This section compiles a few formulas that can be handy when performing

derivations. It is in no way exhaustive, and does not cover the mathematics

necessary to understand most of the derivations in this book. For a succint

summary of concepts that are used in this book, the appendices in Peter

Corke’s Robotics, Vision and Control and the Linear Algebra Review in Ap-

pendix E of Modern Robotics (only in the online version) are recommended.

B.1 Exponentials and Logarithms

exp(x + y) = exp(x) exp(y) exp(x − y) =

exp(x)

exp(y)

exp(x)

= exp(xy)

exp(x)

−1

= exp(−x) exp(x) exp(−x) = 1 (a · b)

= a

(a/b)

= a

exp(x) = lim

n→∞



1 +



ln(x) = log

(x)

log

(x) =

ln(x)

ln(a)

ln(m · n) = ln(m) + ln(n) ln(m

) = n ln(m)

jθ

= cos(θ) + j sin(theta) cos(θ) =

jθ

−jθ

sin(θ) =

jθ

−e

−jθ

123

124 APPENDIX B. USEFUL MATHEMATICAL FORMULAS

tan =

sin

cos

cot =

cos

sin

sec =

cos

csc =

sin

+ cos

= 1 sin(2x) = 2 sin(x) cos(x)

1 + tan

= sec

1 + cot = csc

cos(2x) = 1 − 2 sin

(x) cos(2x) = cos

(x) − sin

(x)

sin

(x) =

(1 − cos(2x)) cos

(x) =

(1 + cos(2x))

B.2 Trigonometric Identities

cos(x ± y) = cos(x) cos(y) ∓ sin(x) sin(y) (B.1)

sin(x ± y) = sin(x) cos(y) ± sin(y) cos(x) (B.2)

cos(x) cos(y) =

(cos(x − y) + cos(x + y)) (B.3)

sin(x) sin(y) =

(cos(x − y) − cos(x + y)) (B.4)

B.3 Taylor Series Expansion

When done about a point a, the Taylor series expansion of a function f (x) is

given by

f(x) =

∞

n=0

(n)

(a)

(x − a)

(B.5)

≈ f(a) + f

′

(a)(x − a) +

′′

(a)

(x − a)

′′′

(a)

(x − a)

+ . . . (B.6)

and for a multivariate function f (x), it is given by

f(x) =

∞

n=0

i=1

∂

∂x

(x − a)

(B.7)

≈ f(a) +

i=1

∂f

∂x

(a)(x

− a

) +

i=1

j=1

∂

∂x

(a)(x

− a

)(x

− a

) + . . .

(B.8)

in which a is the point about which the expansion is done.

B.4. CALCULUS 125

f(x) f

′

(x) f(x) f

′

(x) f(x) f

′

(x)

a 0 au adu uv udv + vdu

vdu−udv

n−1

du e

ln(a)du ln(u)

du log

(u)

u ln(a)

sin(u) cos(u)du cos(u) −sin(u)du z(y(x))

y(x)

B.4 Calculus

k = kx + c

k+1

+ c

= ln |x| + c

= e

+ c

ln(a)

+ c

sin(x) = −cos(x) + c

cos(x) = sin(x) + c

sec(x) = ln |(sec x + tan x)| + c

csc x = ln(csc x − cot x) + c

tan x = ln |sec x| + c

cot x = ln |sin x| + c

sec

x = tan x + c

csc

x = −cot x + c

sec x tan x = sec x + c

csc x cot x = −csc x + c

√

1−x

= arcsin x + c

√

−1

= asec |x| + c

1+x

= atan x + c

+ c



x −



+ c



−



+ c

ln(x) = x ln(x) − x + c

x ln(x) =



ln(x) −



+ c

sin(ax) =

− cos(ax)

+ c

sin

(ax) =

−

sin(2ax)

+ c

x sin(ax) =

sin(ax)

−

x cos(ax)

+ c

cos(ax) =

sin(ax)

+ c

cos

(ax) =

sin(2ax)

+ c

x cos(ax) =

cos(ax)

x sin(ax)

+ c

sin(bx) =

(a sin(bx)−b cos(bx))

+ c

cos(bx) =

(a cos(bx)+b sin(bx))

+ c

sin(ax) cos(ax) =

sin

(ax)

+ c

cot(ax) =

ln |sin(ax)| + c

sec(ax) =

ln |sin(ax)| + c

csc(ax) =

ln |csc(ax) − cot(ax)| + c

sec

(ax) =

tan(ax) + c

csc

(ax) =

−1

cot(ax) + c

sec

(ax) tan(ax) =

sec

(ax) + c

csc

(ax) cot(ax) =

−1

csc

(ax) + c

sin

x cos x =

n+1

sin

n+1

(x) + c

cos

x sin x =

−1

n+1

cos

n+1

x + c

tan

x sec

x =

n+1

tan

n+1

x + c

atan





+ c

−a

ln |

a+x

a−x

| + c

−x



a+x

a−x



+ c

√

−x

= arcsin





+ c

√

= ln



x +

+ a



+ c

√

−a

= ln



x +

− a



+ c

126 APPENDIX B. USEFUL MATHEMATICAL FORMULAS

√

−a

asec



+ c

√

−1



√



+ c

√

−x

−1



√

−x



+ c

sin(ax) =

2x sin(ax)

−

cos(ax) + c (B.9)

cos(ax) =

2x cos(ax)

−

sin(ax) + c (B.10)

sin(ax) cos(bx) = −

cos(a − b)x

2(a − b)

−

cos(a + b)x

2(a + b)

+ c (B.11)

tan(ax) =

ln |sec(ax)| + c (B.12)

± a



x +

± a



+ c (B.13)

− x

arcsin

+ c (B.14)

See the Matrix Cookbook by Petersen & Pedersen, from which the following

formulas were taken from, for more formulas and identities useful for matrix

calculus.

∂x

= u (B.15)

∂x

= (A + A

)x (B.16)

∂u

∂X

= uv

(B.17)

∂u

∂X

= vu

(B.18)

∂u

∂X

= A

Xuv

+ AXvu

(B.19)

∂ (Bx + u)

A (Cx + v)

∂x

= B

A (Cx + v) + C

(Bx + u) (B.20)

B.5. NORMS 127

B.5 Norms

∥x∥

i=1

1/p

(B.21)



≤ ∥x∥

∥y∥

(B.22)

∥x + y∥

≤ ∥x∥

+ ∥y∥

(B.23)

∥x∥

≥ 0 (B.24)

∥cx∥

= |c|∥x∥

(B.25)

∥x∥

= 0 ⇐⇒ x = 0 (B.26)

B.6 Matrix Properties

AB = BA A(B + C) = AB + AC

(B + C)A = BA + CA cA =

















= A

(A + B)

= A

+ B

(AB)

= B



−1







−1

(cA)

= cA

det





= det (A)

= det (A) det



−1



= det (A)

−1

det (AB) = det (A) det (B) det (cA) = c

det (A)

det (1) = 1 det (A) =

i=1



−1



−1

= A (cA)

−1

= c

−1

(AB)

−1

= B

−1





−1



−1



−1

adj(A)

det(A)

adj (A) = (cof (A))





−1

↣ A

A = 1 (left) A

= A





−1

↣ AA

= 1 (right)

tr (A) =

i=1

tr (A + B) = tr (A) + tr (B)

tr (cA) = ctr (A) tr (AB) = tr (BA) = tr (A) tr (B)

tr (A) = tr









= tr





tr (ABC) = tr (BCA) = tr (CAB) tr



−1



= tr (A)

tr (A) =

, the eigenvalues u

v = tr





128 APPENDIX B. USEFUL MATHEMATICAL FORMULAS

diag {a

, . . . , a

}

−1

= diag

−1

, . . . , a

−1

(B.27)

diag {a

, . . . , a

}diag {b

, . . . , b

} = diag {a

, . . . , a

} (B.28)

det (diag {a

, . . . , a

}) =

i=1

(B.29)

det



A B

C D



= det (D) det



A − BD

−1



(B.30)

= det (A) det



D − CA

−1



(B.31)

Appendix C

Skew-Symmetric Operator

The skew-symmetric operatior is deﬁned as

[u]

= u × (−u)











−u



(C.1)





× u









0 −u

−u





(C.2)

such that

[u]

v = u × v (C.3)

with identities

[u]

= −[u]

(C.4)

[u]

v = −[v]

u (C.5)

[u + v]

= [u]

+ [v]

(C.6)

[u]

[v]



[v]

[u]



(C.7)

[u]

[v]

= vu

−





1 (C.8)

R [u]

v = [Ru]

Rv (C.9)

R [u]

= [Ru]

(C.10)

R [u]

[v]

= [Ru]

[Rv]

(C.11)



[u]







v + [u]

(C.12)

129

130 APPENDIX C. SKEW-SYMMETRIC OPERATOR

where (C.11) is derived in appendix C.1.

C.1 Similarity Transform over [u]x[v]x

Since we know that

[Ru]

= R [u]

(C.13)

then

[Ru]

[Rv]

= R [u]

R [v]

(C.14)

= R [u]

[v]

(C.15)

since R

= R

−1

and R

−1

R = 1 for R ∈ SO(3).

C.1.1 Longer Derivation

We begin by stating the known identity :

[u]

[v]

= vu

−





1. (C.16)

It follows that with R ∈ SO(3)

[Ru]

[Rv]

= Rv (Ru)

−



(Ru)



1 (C.17)

= Rvu

−





1 (C.18)

= Rvu

−





1 (C.19)

since R

= R

−1

and R

−1

R = 1. Applying a similarity transform to [u]

[v]

and reusing (C.16) we obtain

R [u]

[v]

= Rvu

− R





by linearity (C.20)

= Rvu

−





R1R

since u

v is scalar (C.21)

= Rvu

−





1 since R1R

= 1. (C.22)

Since (C.19) and (C.22) are equal, we can conclude that

R [u]

[v]

= [Ru]

[Rv]

(C.23)

for R ∈ SO(3).

Appendix D

Rotation Time Derivative

Assuming, just for this section that the coordinate system {b} in Fig. 3.1

rotates continuously but does not move linearly relative to {w}, and that {o}

is ﬁxed relative to {b}. At all times, we have that

(D.1)

whose total time derivative is

(

) =

(

)

(

) (D.2)

(

)

(D.3)

where the second term is zero since the time derivative of the position vector

(i.e. the linear velocity) is assumed to be zero. Since the origin of {o} is

ﬁxed relative to {b}, and assuming that

is zero,

(D.4)

(D.5)

gives the velocity of {o} relative to {b} due to the rotation of {b} about {w}

as in (3.5). Since (D.3) and (D.5) are both equal to

(

), we can state

that

(

)

(D.6)

(D.7)

131

132 APPENDIX D. ROTATION TIME DERIVATIVE

such that, by identiﬁcation, we obtain the time derivative of a rotation matrix

(an important result) as

(D.8)

b w

= −

b w

= [

]

(D.9)

b w

= −

b w

(D.10)

where (C.4) was used. The total time derivative of a rotating vector is

(

) =

(D.11)

where

is the instantaneous angular velocity of {b} with respect to {w}

and expressed in the inertial/ﬁxed frame {w}.

Appendix E

Inertia Tensor Derivation

The second moment of a point-mass about an axis n is

J = mp

⊥

(E.1)

where p

⊥

is the distance between the point-mass and the axis, given by the

component of p = [p

, p

]

that is perpendicular to n = [n

, n

]

. By

the additivity of vectors, we know that

p = p

⊥

+ p

∥

. (E.2)

Also, the component of p that is parallel to n is given by the scaled projection

of p onto n with

∥



p · n

n · n



n =





n (E.3)

where the denominator equals 1 in the case that n is a unit vector. By sub-

stituting (E.3) in (E.2), we obtain

⊥

= p −





n (E.4)

that allows us to rewrite (E.1) into

J = mp

⊥

(E.5)

= m



⊥



(E.6)

= m



p −







p −





(E.7)

133

134 APPENDIX E. INERTIA TENSOR DERIVATION

If we assume that n is a unit vector and that n

n = 1 then

J = m





p −







p −







(E.8)

= m



p + p





n − n





p + n







(E.9)

noting that p

n is a scalar, it can be placed in the front

= m



p +





n − n









(E.10)

noting that p

n = n

p, the middle term can be cancelled

= m



p +







(E.11)

noting that n

n = 1, the last term can be simpliﬁed

= m



p +







(E.12)

and expanded into

= m



p + (p

n)(p



(E.13)

using p

n = n

p to slightly reformulate into

= m



p + (n

p)(p



(E.14)

= m



p + n



(E.15)

We can once again rely on n

n = 1 and on the fact that p

p is a scalar to

reformulate the equation into

J = m



p + n



(E.16)

= m



pn + n



(E.17)

= mn



3×3

+ pp



n (E.18)

which we can further simplify into

J = mn



−[p]

[p]



n (E.19)

= mn



[p]



n (E.20)

by using identities (C.8) and (C.4) of skew-symmetric matrices. The value of

J can be interpreted as the moment of inertia about n when the point-mass

rotates around n. The vector about which the moment of inertia is computed

can be diﬀerent from the one around which the point-mass rotates. Since a

Cartesian reference frame F

has three axes, there is a total of 3 × 3 = 9

moments that can be computed for a given point-mass with respect to F

These moments are regrouped into an inertia tensor whose ij-th element is

135

the moment of inertia about axis e

when the point-mass rotates around axis

. For p = [p

, p

]

, we have the inertia tensor

I = m





+ p

−p

+ p

−p

+ p





(E.21)

in which the moment of inertia about axis e

when the point-mass rotates

around axis e

can be extracted with

= e

· I · e

= e

. (E.22)

A change in reference frame can be performed by multiplying vectors e

and

by appropriate extrinsic rotation matrices such that

= (e

i i

)

I(e

j j

) (E.23)

|{z}

(E.24)

b b

(E.25)

where the inertia tensor I

computed in reference frame F

is related to I

that is computed in reference frame F

when F

. Note that (E.25)

assumes that the origin of F

and F

are coincident. For a more general

formula, see appendix F.

Since inertia is additive, the inertia tensor of a rigid body can be computed

as the sum of the inertia tensors of the point-masses that form the rigid body.

Therefore, for a rigid body of shape V ⊂ R

with mass density ρ(p), the inertia

tensor is

I =

[p]

ρ(p)dV (E.26)

whose elements depend on the choice of reference frame used in describing p.

136 APPENDIX E. INERTIA TENSOR DERIVATION

Appendix F

Expressing Inertia Tensors

in Any Frame

The inertia tensor of a rigid body expressed relative to F

can be computed

from the inertia tensor of the same rigid body expressed in another reference

frame F

, as pictured in Fig. F.1, from the equation derived in this section.

Figure F.1: Reference frame F

is ﬁxed on the rigid body with the origin

located at the centre of mass. The inertia tensor expressed relative to F

(green) is computed from the known quantities in blue.

From (4.22) we have that

b b

− m [

]

[

]

(F.1)

b b

− m [

]

[

]

(F.2)

which implies that



+ m [

]

[

]



. (F.3)

137

138 APPENDIX F. EXPRESSING INERTIA TENSORS IN ANY FRAME

By substituting (F.3) in (F.1) we obtain



+ m [

]

[

]



− m [

]

[

]

(F.4)



+ m [

]

[

]



− m [

]

[

]

(F.5)

in which we can substitute

to get



+ m [

]

[

]



− m [

]

[

]

. (F.6)

We then use identity (C.11) on the rightmost term to get



+ m [

]

[

]



− m

[

]

[

]

(F.7)

which is simpliﬁed by the use of rotation’s linearity property into



+ m



[

]

[

]

− [

]

[

]



(F.8)

in which we substitute

−

to get our ﬁnal result



+ m



[

]

[

]

− [

−

]

[

−

]



(F.9)

Bibliography

[1] Cheng Li, Yuanqing Wu, Harald L¨owe, and Zexiang Li. Poe-based robot

kinematic calibration using axis conﬁguration space and the adjoint error

model. IEEE Transactions on Robotics, 32(5):1264–1279, 2016.

139