Vous êtes sur la page 1sur 15


Fundamentals: Modeling & Control

2.1 Kinematics

2.1.1 Introduction, passive and active transformations

The most common task of a robot is to move objects in 3-D space. Therefore, the
geometry of motion, i.e. kinematics is the first topic to be studied in robotics. Forces
will be added later, this will be treated in chapter 2.3 on robot dynamics (Newton-Euler
equations or Lagrange formalism to obtain the differential equations of motion).
Kinematics and dynamics are connected through the concept of the Jacobian matrix
(chapter 2.2). Kinematics and dynamics are needed for model-based optimal control
design of robots (chapter 2.4).
Joint Space Operational Space Work Space
A robot is controlled by set values for joint angles and (in case of linear motions) joint
positions. These values are sent by the control electronics to the joint actuators. These
variables are called joint variables or robot variables, they define the joint space.
The number of variables corresponds to the number of degrees of freedom (d.o.f.,
degrs de libert in French, Freiheitsgrade in German) n of the robot. In this text, we
use two equivalent notations for the joint variables commonly found in literature:
{ q1, q2, qi, . qn } or { 1 , 2 , ... i , ... n }
The task of the robot is not given in joint variables, but in an operational space,
independent of a particular robot. These operational variables or task variables
could be e.g. Cartesian coordinates defined with respect to the workspace, giving the
target point and the orientation of an object to be placed. Sometimes, the operational
variables are also referred to as world coordinates. Orientation and position of a rigid
body in 3-D space is determined by a total of 6 coordinate values.
A third space, essential in robotics, is the workspace. It is the subset of the operational
space, which is reachable by a given robot.
Although seemingly straightforward, it can be a complex time- or configurationdependent construction of sub-workspaces. Think e.g. of an endoscopic telemanipulator
for surgery composed of two tweezers coming out of an endoscope. The tweezers have
partially overlapping workspaces, the intersection defines the workspace for bimanual
operation. The positioning of the endoscope itself will move this workspace within the
operational space. The workspace is modified by the kind of end-effector and by the

requirements as to orientation of the end-effector. Reachable space is not the same than
reachable with arbitrary orientation.
Robot kinematics therefore essentially is concerned with establishing the mathematical
link between these spaces, joint space and operational space. The portion of operational
space reachable by the full joint space will be the workspace.
Two basic mathematical relationships can be defined:
1) Given the joint variables, what are the operational variables? This
mathematical function is called forward kinematics (modle
gomtrique direct in French). Forward kinematics is, in short, the position
and orientation of the robot hand (or end-effector) in function of the joint
2) Given a set of task variables, what joint variables are needed to reach them?
This function (mathematically the inverse function of the previous one) is
called inverse kinematics (modle gomtrique indirect).
Kinematics is the basis for all that follows, the dynamic model of the robot (the equations
of motion) giving accelerations, forces and torques, the specifications of the mechanical
design, control and actuator implementation. Of all these tasks, kinematics is often
mathematically the most demanding, which is also why it occupies relatively large space
in within chapter 2 of this text. The topic is so complex that entire books or lectures could
be devoted to kinematics only.
One of the difficulties in describing kinematics is the need to use different coordinate
frames, moving ones fixed to robot links or to the endeffector, and fixed ones connected
to operational space. Such multiple coordinate frames, some moving and some fixed, are
responsible for the complex notations and calculations when setting up the equations of
motion. An elegant modern way to deal with these complexities is the use of 6-D algebra
(Featherstone) for the setup of rigid body dynamics. In this lecture, time does not allow to
introduce this method, but it is recommended to search the abundant literature on the
topic before solving complex modelling problems of rigid body dynamics.
In the classical methodology used here to introduce the basic mathematical modelling,
coordinate transforms will be necessary to move from one reference frame to the other. In
this context, there are two basic situations, always to be clearly distinguished:
1) Either the coordinate frame is changed, but the situation (the object, the physical
position vector) is unchanged. This is called passive transformation
2) Or, on the opposite, an object is moved with respect to a fixed coordinate frame.
This is called active transformation.
For the rest of chapter 2.1, we will essentially deal with active transformations, i.e. unless
otherwise stated, we will express vectors in a single fixed coordinate frame. This will
help to simplify significantly mathematical treatment, as we can do without all kinds of

indices and superscripts. We will also (in chapter 2.1) leave out the time dimension, i.e.
velocities and accelerations will only be introduced from chapter 2.2. (Dynamics) on.

2.1.2 Translations and rotations in 2-D around origin

There are only two types of motions of a solid: Translation and rotation. Translation of a
solid is the motion characterised by every point of the solid having a single identical
velocity vector t.


P (before)


Fig. 1a) Active translation vector t describing

the change of position of any point C of the
object being moved.

Let the coordinates of a material point P be given by a coordinate vector v1 . After
moving the object by a translation t , the point P will have the new coordinates v2 in the
fixed reference frame:
v2 = v1 + t


This equation thus describes an active transformation, i.e. the object is moving in a
fixed frame. For any point P of the object, the same equation (1) is valid.
Passive transformation is just the opposite: The position of the object is fixed, but it is
described in a new coordinate frame displaced from the original one. In case of a
translation of the coordinate frame, the direction of the axes stays the same, but the origin
is shifted by a translation vector t:







Fig. 1b) Passive translation by vector: The object

stays fix, the coordinate frame is moved by vector t

It can be seen that in this case, the equation of transformation becomes the inverse
operation as compared to equation 1a), i.e. subtraction in place of addition.
v2 = v1 t


Rotation is more complex: A rotation axis can be defined, each of its points
characterized by zero velocity. This rotation axis can run through the solid or it can be
completely disconnected from it.
Finally, the most general motion of a solid is a combination of the two, with a velocity
component in direction of the rotation axis. This is called a screw motion, obviously a
motion in 3-dimensional space, discussed in the next subchapter.
In this sub-chapter, we first analyse only plane motions. In this case, the rotation axis
perpendicular to the plane and no screw motion is possible. The intersection of the plane
and the rotation axis is called the (instantaneous) center of rotation. Mathematically a
translation within the plane is a limit case of a rotation with center of rotation at infinity.
Let us first limit ourselves to rotations about the origin 0 of a Cartesian coordinate
system. Again, let the coordinates of a material point P be given by a coordinate vector v1
After moving the object by a rotation of angle about the origin, the point P will have
the new coordinates v2 in the fixed reference frame. This time, the equations are a found
by a trick, transforming the Cartesian coordinates into polar coordinates:




r cos #
v1 = ! 1 # = !
" y1 $ " r sin $
r cos( + ) #
v2 = ! 2 # = !
" 2 $ " r sin( + ) $




Fig. 2 Active rotation by an angle

In polar coordinates, rotation is trivial. To find formulas converting directly Cartesian

coordinates, the following trigonometric identities are used:
cos(a+b) = cos(a)cos(b) sin(a)sin(b)
sin(a+b) = sin(a)cos(b) + cos(a)sin(b)
Inserting this in (2) gives


! x 2 $ ! r cos( + )$ ! r cos cos r sin sin $ ! cos sin $ ! r cos $

v2 = # & = #
" y2 % " r sin( + ) &% #" r cos sin + r sin cos &% #" sin cos &% #" r sin &% (4)
with the rotation matrix R :

" cos
v2 = Rv1 where R = $
# sin

sin %
cos &


Exercise 1)

R(= 0) = ?


R() = ?


R()1 = ?



v3 = R(2)v2 = R(2)R(1)v1 = R(?)v1


The rotation matrix can be interpreted as the expression of the unit vectors of the rotated
system (x2, y2) in components of the fixed system (x1, y1), in an active transformation



!" x 2 , y #$ = & cos
" sin

sin # & x 2 x1
cos $ & x 2 y1

y 2 x1 #
y 2 y1 '


The decomposition in components is realized by the scalar products of the vectors as

given in (6), which is the cosine of the angles between the vectors. This matrix is
therefore also called the matrix of direction cosines.
Thus the orthogonality of the coordinate frame gives its qualifier orthogonal to the
rotation matrix. Reminder: The column vectors of such a matrix are orthogonal among
each other and of unity norm. It follows from this that RT=R1 .
Exercice 1d) demonstrates that the sequence of two rotations (called composition) can be
calculated by matrix multiplication. This corresponds to a rotation about the sum of the
angles (1+ 2), as can be seen from applying the trigonometric identity (3) to the result
of the matrix product R(2)R(1).
2.1.3 Combination of translations and rotations: Homogenous matrices

A general plane motion is a combination of translation and rotations. It will be necessary

to compose translations and rotations, especially to achieve rotation about an arbitrary
center. This becomes complicated when a sequence of vector additions (for translations)
and matrix multiplications (for rotations) has to be elaborated. In order to facilitate this, a
method known from projective geometry is used, allowing to integrate translation AND
rotation in a single matrix multiplication. This is called the homogenous representation,
with homogenous matrices and homogenous vectors, defined as follows:
A scale factor of value 1 is added at the end of each vector column. 2-d vectors thus
will have three elements, 3-d vectors four elements. This scale factor is not a geometric
dimension, it is only an algebraic element.
In 2-d we will have 3x3 transformation matrices combining rotation and translation, in 3d transformation matrices will be 4x4. The last column of such a rotation matrix contains
the translation vector, with a scale factor 1. The remaining two or three elements below
the rotation matrix are filled with zeros. We thus have:

"R t % $cos
$# 0 1 '& = $ sin
# 0


tx % "c s tx %
ty ' = $ s c ty '
' $
1 & #0 0 1 &


with the 2x2 rotation matrix R as before and the translation vector t augmented by the
scale factor 1. The remaining two elements on the bottom left are filled with zeros.
You will immediately understand how this works by trying a pure translation of a
vector v = [vx , vy , 1]T . Remember, this is a 2-d vector with an additional scale factor 1.
This scale factor is NOT a spatial dimension (such as, later on, the z-coordinate). So, try a
pure translation (=0) by multiplying matrix (8) with vector v.
Example: Composition of translations and rotations:
Let v1 be a vector who is
1) rotated by angle 1
2) translated by t1
and then again
3) rotated bym angle 2
4) translated by t2
with the final result

giving v2 = R(1) v1 + t1

v3 = R(2) v2 + t2 = R(2) R(1) v1 + R(2) t1 + t2


Exercise 2) Do the four operations above by using homogenous representation. Express

the final result in a single matrix, containing all four motions.
Important: Note that the 2x2 rotational matrix (top left block) can be simplified by
applying the trigonometric identities (3). The composed rotation R(2) R(1) is thus
equivalent to R(1+2) while the composed translation is R(2) t1 + t2

Exercise 3a) Give the homogenous matrices for pure translation, pure rotation, identity.
3b) First translation, then rotation.
3c) Inverse motion of 3b), i.e. first rotation by then translation by t .

2.1.4 Rotations in 2-D around arbitrary origin

These preliminaries allow us now to introduce easily rotation about an arbitrary center
point P described by vector p. All we have to do is to compose a translation of P to the
origin, rotation, and then translation back to P . This gives the following composed
"1 0
$0 1
$#0 0

0%"1 0 p x %
0'$0 1 p y ' =
1&'#$0 0 1 '&
" I p%"R 0%" I p% "R p Rp%
1 &
#0 1 &# 0 1&#0 1 & # 0

p x %"cos
p y '$ sin
1 &'#$ 0



From this it can be seen, that a rotation about P can also be achieved by a rotation about
the origin and a subsequent translation by . Conversely, any combination of rotation and
translation can be expressed by a rotation about a certain point P.


4a) Is the above statement entirely correct? (4b) to 4f), see exercises)

Rotations in 3-D around the origin; Axis of rotation

In three-dimensional space, it will not be sufficient just to indicate a center of rotation, as

we did for plane kinematics. Any change of orientation in 3D defines an axis of
rotation. This axis was given in case of plane kinematics: It was always perpendicular to
the plane in question. Now, the axis can have any direction in space. This fact alone
opens up entirely new concepts and techniques in kinematics, especially also as to
intuition and mental representation of the geometry of motion.
How many parameters define a rotation in 3-D?
How many parameters are necessary to characterize a change of orientation (a rotation) in
Of course, as before, we will first have an angle of rotation.
But now the direction of the rotation axis needs also to be specified. A direction of an
axis in 3-D space is fixed by only two parameters. These could be, as an example,
latitude and longitude of a point on the surface of a sphere. The axis is then defined as
going from the centre of the sphere through the point of given latitude and longitude.

Together, this gives three parameters, two for the axis direction and one for the angle of
rotation around this axis.
Alternatively, we could consider the axis given by a vector v = [xa, ya, za]T i.e. three
parameters, defining the rotation axis and the angle of rotation around this axis. This
gives a total of four parameters. So there is an excess of one parameter: The excess
parameter is the length of the vector, which does not have any meaning for the definition
of our rotation. This length could be normalized ( ||v|| = xa2 + ya2 + za2 = 1 ), such an
additional constraint on the four parameters removes one freedom, so we are again at the
minimal number of three for the definition of a change in orientation.
Different ways to express a change of orientation in 3-D
We have just introduced a first kind of specifying change of orientation. Lets call it
In practice many other ways to specify this information are used, so let us introduce them
here. In the context of flight dynamics, the three angles roll, pitch and yaw are used to
define orientation. They are sometimes also called Tait-Bryan angles or Bryan
Angles (George H Bryan, 1864-1928). These angles can also be used in robotics. In
French, the names of these same angles are have familiar names in the navigation
context: roulis, tangage, lacet, in German they are called Roll, Nick & Gier
Winkel. The particularity of this set of angles (again, three parameters to define an
orientation) is, that they are defined with respect to a direction of a vehicle motion, or, in
case of a robot, with respect to the approach direction.
Furthermore, they can either be defined with respect to a fixed direction independent of
the instantaneous vehicle orientation (e.g. in navigation), or with respect to body-fixed
axes (as in flight dynamics or robotics), this should be specified from the context. Be
careful not to confuse the two situations!
This leads us to a further generalization, which is linked to the work of Leonard Euler.
While Newton formulated the laws of motion for point masses, it is Leonard Euler who
clarified the dynamics of rigid bodies by generalizing the equations of motion for rigid
body dynamics. He thus had to describe the orientation of a rigid body in space. He
studied specifically the gyroscope and it is in this context that the three Euler Angles
were first introduced. They are: Precession, nutation and self-rotation. Euler being one
of the most prolific mathematician of all times, there is a large collection of Eulersomething. Orientation in space can also be represented by the Euler parameters,
which are NOT the Euler angles! We will define the Euler parameters in a short moment,
they are equivalent to the quaternions discussed a bit later. Let us first connect all this
to the 2-D rotations and give some examples.
Let us start with a straightforward generalization from 2-D (plane) kinematics to 3-D
kinematics. We have introduced previously rotation as a matrix used in the

v(after) = R() v(before)

where v(before) and v(after) are the vectors indicating a point on an object before resp. after
rotation (active transformation, fixed coordinate frame). The 2x2 orthogonal matrix
R() contains the rotation by angle as seen in section 2.1.2. How do we now apply this
formalism when we have to specify a rotation axis?
We know the answer for rotations about the (vertical) z-axis for rotations in the x-yplane: Such a rotation matrix, now 3x3, has to leave the z-coordinate values unchanged.
The (x-y) coordinates are transformed as before. Thus we obtain:

R z = % sin
%$ 0



Exercise 5: Give the rotation matrices Rz(), Rx() and Ry() for rotations about the z-,
x- and y- axis. Hint: Use the transform of the unit vectors ex , ey , ez as in equation (8)
with the direction cosines. It is more than useful to draw a figure for each case! Use the
direction cosine matrix as in equation (8) above.

Rx() = ?

Ry() = ?

Now for the core problem in this context. Try the following:
Exercise 6: Rotate an object a) 90 around the z-axis, then 90 around the y-axis
b) 90 around the y-axis, then 90 around the z-axis
This is done mathematically by composition of the rotations, i.e. by matrix
multiplications. What do you observe?
Exercise 7: Solve the previous question graphically, by rotating the object (a sheet with a
1 printed, to define orientation):










Can you identify the rotation axis and angle equivalent to the two rotations about the zand y-axis?
How can we find this axis in the matrix products resulting from 6a) and 6b)?
A point on the rotation axis will not be affected by the rotation. We therefore have the
identity Rv = v for any point v on the rotation axis. The rotation axis is therefore the
eigenvector of the rotation matrix corresponding to the eigenvalue 1. The angle can then
be found by projecting a transformed vector on the plane normal to the axis.
Example: Matlab command [V,d]=eig(A) gives eigenvectors in matrix V and eigenvalues
in d:

0 1

0.05 - 0.57i

0.05 + 0.57i


0.5 + .86i

A = 1

0 0

V = 0.47 + 0.32i

0.47 - 0.32i


d= 0

1 0

0.52 + 0.24i

0.52 - 0.24i


0.5 0.86i


These calculation can be simplified and expressed in the following formula:

Transformation from direction cosine matrix to angle-axis representation:
The angle of rotation matrix R is then given by the following expressions:

! a d
R =# b e
# c f

g $
h &
i &

cos( ) = (tr(R) 1)
sin( ) =
( f h)2 + (g c)2 + (b d)2

where the trace operator tr(R) is defined as the sum of the diagonal elements of R.

" f h %
1 $
The rotation axis is given by
$ gc '
2sin( ) $
# bd &


Note that this formula breaks down for rotation angle zero! Worse, this means that
numerical condition will deteriorate as the angle approaches zero, which is physically a
completely normal situation. This behavior is undesirable for the control of robots. It will
be a strong incentive to find other representations for change of orientation than
Exercise 8a): Find the direction cosine matrix (the rotation matrix) for a rotation about an
angle around the axis [0, sin(), cos()]T
Hint: Use composition of rotations around known coordinate axes.

Of course, this could be done more easily by an inverse of formula (10), i.e. by finding a
formula rotating directly a vector when rotation axis and angle are given.
Let us assume that the rotation axis k is given as a vector of length one: k=[x,y,z]T and let
the rotation angle be . Any vector v may then be transformed by decomposing it into a
component parallel to the rotation axis and a component perpendicular to it. While the
parallel component remains unchanged, the rotated perpendicular component can be
found by a combination of vector cross-product and trigonometric functions. The final
result is
! !
! !!
vrot = v cos + (k v)sin + k (kv)(1 cos )
This formula was first published by Benjamin Olinde Rodrigues, a French
mathematician who lived from 1795 to 1851. By applying this formula to the base
vectors of a coordinate system, one obtains the matrix form of the Rodriguez equation,
which gives the rotation matrix (direction cosines) from an angle-axis
Given axis k = [x,y,z]T with || k || = x2+ y2 + z2 = 1 & angle of rotation about k:

" xx
R = (1 cos )$ xy
$ xz

" 0 z y %
" 1 0 0 %
xy xz %
yy yz ' + cos $ 0 1 0 ' + sin $ z 0 x '
$ y x 0 '
$ 0 0 1 '
yz zz '&


with x2+ y2 + z2 = 1.
With rotation matrices we use 9 parameters for the change of orientation which are
interconnected for the three degrees of freedom of orientation. With the angle-axis
representation as introduced above, we run into problems for small rotation angles. This
why mathematicians in early 19th century where searching for a way to represent
orientation in 3-D space by an analogy of complex numbers, after Jean-Robert Argand
(1768-1822) from Geneva had popularized the complex plane allowing a very elegant
representation of plane rotation.
After many unsuccessful attempts to generalize the complex numbers with one real and
two imaginary components, William Rowan Hamilton famously succeeded in 1843 based
on work by Gauss and Euler, by introducing quaternions. It is worth noting that this work
was motivated by the representation of orientation in 3-D space.
Quaternions, Euler Parameters, Olinde-Rodriguez Parameters
While Euler Parameters (also called Rodriguez Parameters) are strictly concerned with
kinematics, quaternions have acquired a far greater significance as generalized complex
numbers. The trick in Hamiltons breakthrough is two-fold.

1) He recognized that a generalization to of complex numbers to three dimensions is

not possible, that he must jump directly to four dimensions, i.e. a real part and
three imaginary parts.
2) Furthermore, he also recognized, that he must abandon the law of commutatitvity
of multiplication. (remember, this was many years before introduction of matrices
and vectors).
Based on this, the new hypercomplex numbers, called quaternions could be
introduced, according on the following algebraic rules :
There is a real part and three imaginary parts, with units i, j, k.
We have

i2 = j2 = k2 = ijk = 1

and the non-commutative


ij = k= ji

jk = i = kj

ki = j = ik


In analogy to complex numbers where a number z is a pair of scalars a and b according to

z = {a,b} = a + bi
we now have for the quaternion Q a quadruplet of real numbers (scalars) 0 , 1, 2, 3
Q = { 0 , 1, 2, 3} = 0 + i 1 + j 2 + k 3


With this, we have all necessary algebraic definitions. In modern notation, the three
imaginary parts are often grouped as a 3x1 vector = [1, 2, 3]:
Q = { 0 , 1, 2, 3} ={ 0 , }
How is spatial orientation coded in quaternions?
This is where the four Euler Parameters introduced in the 18th century by Euler are used:

0 is the cosine of half of the rotation angle :

0 = cos(/2)


The direction of the rotation axis is given by the vector part = [1, 2, 3]
As we use four parameters for three degrees of freedom, we may introduce a constraint. It
is essential. The constraint is that the modulus (the absolute value) of the quaternion is
equal to one (unitary quaternion):
02 + 12+ 22+ 2 = 1


This means that the length of vector = [1, 2, 3] is restricted to sin(/2):

|| || = sin(/2)

leading to

|| Q || = 1


These rules lead to the following composition of quaternions (and therefore of rotations):
QMQL = { 0 , } { 0 , } = { 00 T ,

0 + 0 + }


With this, we can now solve problems such as exercises 6, 7 or 8a) much more easily.
Exercise 8b) Find the rotation axes of 6 & 7 with the help of quaternions.
Finally, it is useful to have equations translating from direction cosines to quaternions
and vice-versa.
Direction cosines given in terms of the quaternion (from Sciavicco & Siciliano,
Modelling and Control of Robot Manipulators, Springer, 2000):
# 2(20 + 21) 1 2(1 2 0 3) 2(1 3 + 02)&
R = %2(12 + 03) 2(20 + 22) 1 2(2 3 01)(
%$2(13 0 2) 2( 2 3 + 01) 2(20 + 23) 1 ('


Quaternion given in terms of direction cosines:

! r r
# 11 12
R = # r21 r22
#" r31 r32

r13 $
r23 &
r33 &%

= $
0 =

sgn(r32 r23 ) r11 r22 r33 +1 '
sgn(r13 r31 ) r22 r11 r33 +1 '
sgn(r21 r12 ) r33 r22 r11 +1 ''
r11 + r22 + r33 +1


With the signs of the vector component as defined above, one has to take the positive
angle when computing it from (11d) = arccos(0)

2.1.6 Rotations in 3-D with homogenous matrices

While quaternions are most useful to describe change of orientation, we still need to work
with rotation matrices (direction cosines) especially when combining of rotations and
translations. In this case, the use of homogenous matrices is particularly helpful. There is
nothing new to introduce at this stage, the expansion of the vectors and matrices can be
done exactly as in the 2-D case: Add a scale factor of 1 to the vectors, now having for
algebraic elements: The three spatial coordinates (x, y, z ) and the scale factor 1. The
homogenous matrix contains as before as a fourth column (to the right of the direction
cosines) the translation vector t and the last row of the homogenous matrices contains
three zeros and the scale factor 1 of vector t :


This corresponds, as before, to a rotation by R and then a translation by t.


A rotation around an axis not through the origin is is given, again as before, by translating
the object from any point P on the axis to the origin (translation by p), then rotating
around the axis now through the origin and finally translating back:

" I p%"R 0%" I p% "R p Rp%

1 &
#0 1 &# 0 1&#0 1 & # 0


The vector p points to any point on the rotation axis.

The most general motion in 3-D is not, as before, rotation around a give center. There can
also be a translational motion in the direction of the rotation axis. Therefore the general
motion in 3-D is a screw (sometimes called a twist).

2.1.7 Forward Kinematics

We are now in a position to introduce the forward kinematics of a robot, as defined in the
introduction to this chapter. It is the position and orientation in operational space where a
certain configuration of the joint variables { 1 , 2 , ... i , ... n } of the robot will bring a
given object. Expressed as an equation, the forward kinematics is the following
functional relationship:
(x, y, z, (orientation))T = f (1 , 2 , ... i , ... n )
As an example, we first derive the forward kinematics for the SCARA robot, which is
essentially 2-D, and then for a 6 degrees-of freedom 3-D robot. SCARA stands for
Selective Compliance Robot Arm, the most common assembly robot with 4 degr. of
freedom, xyz and orientation around the vertical axis. The compliance in the horizontal
plane can be increased (through the control) while it remains low in z-direction, very
useful for insertion and assembly tasks with self-alignement of the inserted piece.

Figure 8a) Industrial SCARA Robot