Ridig Body Motion – Homogeneous Transformations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Università di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
1 / 80
Summary
1
Ridig Body Motion
Rotations
Translations
2
Homogeneous transformations
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
2 / 80
Ridig Body Motion
Ridig Body Motion – Homogeneous Transformations
Ridig Body Motion
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Università di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
3 / 80
Ridig Body Motion
Rigid body motions - Homogeneous transformations
Description of the manipulator kinematic properties:
Description of the geometric characteristics of the robot’s motion (position,
velocity, acceleration), without considering the forces applied to it
The solution of the kinematic problem is based on:
definition of a reference frame associated to each link of the manipulator
a procedure for the computation of the relative motion (position, velocity,
acceleration) of these frames due to joints’ movements.
It is necessary to introduce some conventions to describe the position/orientation
of rigid bodies and their motion in the space:
kinematic properties of a rigid body and how to describe them
homogenous transformations
description of position and velocity (force) vectors in different reference
frames.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
4 / 80
Ridig Body Motion
Rigid body and its representation
A manipulator is composed by a series of rigid
bodies, the links, connected by joints that allow a
relative motion.
RIGID BODY: idealization of a solid body of finite size in which deformation is neglected: the
distance between any two given points of a rigid
body remains constant in time regardless of external forces exerted on it.
||p(t) − q(t)|| = d(p(t), q(t)) = cost
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
5 / 80
Ridig Body Motion
Rigid body and its representation
Some assumptions:
The 3D operational space is represented by the vector space IR3 ,
In the 3D space, are defined the inner product
T
u v=
n
X
i =1
u, v ∈ IRn
ui vi = ||u||||v|| cos θ
and the Euclidean norm
||u|| = uT u =
n
X
i =1
ui2
u ∈ IRn
We often use Cartesian (right-handed) reference frames, with homogenous
dimensions along the axes
The base frame is an inertial frame.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
6 / 80
Ridig Body Motion
Rigid body and its representation
General definition of norm:
kuk = uT W u
being W a matrix:
symmetric
positive definite
Often, W is a diagonal matrix.
Since
W = VT V, then:
kuk = uT W u = (uT VT )(Vu) = xT x
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
7 / 80
Ridig Body Motion
Rigid body and its representation
In IR3 , a rigid body has 6 degrees of freedom (dof):
3 for the position, x, y , z;
3 for the orientation, α, β, γ.
In general, a rigid body in IRn has
n dof for the position
n(n − 1)/2 dof for the orientation
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
8 / 80
Ridig Body Motion
Rigid body and its representation
Roto-translation motion: most general motion of a rigid body in the space,
composed by a rotation about an axis (instantaneous axis of rotation) and a
translation along the same axis.
Problem of describing the instantaneous position/orientation of a rigid body with
respect to a fixed base frame
Let 0 o1 be the origin of the frame F1 fixed to the
rigid body, expressed in F0 . Each point of the
body has coordinates
1
p = [1 p x 1 p y 1 p z ]T
constant with respect to F1 .
Since the body moves, the same point has coordinates 0 p, expressed in F0 , variable in time
0
C. Melchiorri (DEIS)
p = [0 p x 0 p y 0 p z ]T
Ridig Body Motion – Homogeneous Transformations
9 / 80
Ridig Body Motion
Rigid body and its representation
First problem: if point p is known in
F1 , compute the equivalent representation in F0 .
1
p =⇒
0
p
The problem is solved by using the Homogeneous Transformation Matrix 0 T1 :


nx s x ax o x
0
0
 ny s y ay o y 
o1
R1
0

=
T1 =
 nz s z az o z 
0
0
0 1
0 0 0 1
defining the transformation (roto-translation) between F1 and F0 .
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
10 / 80
Ridig Body Motion
Rigid body and its representation
The problem is decomposed into two parts:
1
2
F0 and F1 share the same origin, and have a different orientation in space
F0 and F1 have parallel axes but a different origin (translation).
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
11 / 80
Ridig Body Motion
Ridig Body Motion – Homogeneous Transformations
Rotations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Università di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
12 / 80
Ridig Body Motion
Rotations
Rotations
Consider two reference frames F0 and F1 with the same origin, i.e. o1 ≡ o0 .
Given a vector 0 v in F0 , its components vx , vy , vz are the orthogonal projections
of 0 v on the coordinate axes.
vx
vy
=
=
0 T
v i = ||0 v|| cos α1
0 T
v j = ||0 v|| cos α2
vz
=
0 T
v k = ||0 v|| cos α3
i, j, k: unit vectors defining the directions of x0 , y0 , z0
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
13 / 80
Ridig Body Motion
Rotations
Rotations
If 0 v indicates an axis of F1 , e.g. 0 i1 , then 0 i1 = [0 ix 0 ij 0 iz ]T , where
0
ix
iy
0
iz
0
=
=
=
0 T
i1 i
0 T
i1 j
0 T
i1 k
= cos α1
= cos α2
= cos α3
This is a well known result:
0
i1 = [cos α1 , cos α2 , cos α3 ]T
the components of a unit vector with respect to a reference frame are its direction
cosines.
A similar results holds for the other directions 0 j1 and 0 k1 .
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
14 / 80
Ridig Body Motion
Rotations
Rotations
Once the direction cosines of the three axes of F1 with respect to F0 are known,
the matrix R may be defined:

 0T
0 T
i1 i 0 jT
k1 i
1i

 0T
0 T

j1 j 0 kT
R=
1j 
 i1 j
0 T
0 T
i1 k 0 jT
k1 k
1k
0
i1 , 0 j1 , 0 k1 : axes of F1 expressed in F0
i, j, k:
C. Melchiorri (DEIS)
axes of F0 .
Ridig Body Motion – Homogeneous Transformations
15 / 80
Ridig Body Motion
Rotations
Rotations
EXAMPLE
By projecting the unit vectors i1 , j1 , k1 on i0 , j0 , k0 , the components of the
principal axes of F1 in F0 are obtained:

√
√
i1 = [0, 1/ 2, −1/ 2]T




√
√
j1 = [0, 1/ 2, 1/ 2]T




k1 = [1, 0, 0]T
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
16 / 80
Ridig Body Motion
Rotations
Rotations
The rotation matrix between F0 and F1 is obtained from these three vectors:

R=
0
0
√1
2
− √12
√1
2
√1
2

1
0 
0
In general
x0
y0
z0
C. Melchiorri (DEIS)
x1
y1
z1
r11
 r21
r31
r12
r22
r32

r13
r23 
r33

Ridig Body Motion – Homogeneous Transformations
17 / 80
Ridig Body Motion
Rotations
Rotations
Usually, in robotics the symbols n, s, a, are used to indicate the axes x1 , y1 , z1 ,
then


nx s x ax
0
R1 = [0 n 0 s 0 a] =  ny sy ay 
nz s z az
defining the relative orientation between F0 and F1 .
Symbols n, s, a refer to a frame fixed on the endeffector (e.g. gripper) with
z axis (a) along the approach direction
y axis (s) in the sliding plane of the fingers
x axis (n) in the normal direction with respect to
y, z.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
18 / 80
Ridig Body Motion
Rotations
Rotations
Rotation matrix:
From the conditions:

nx
R = [n s a] =  ny
nz
(
sx
sy
sz

ax
ay 
az
n T a = sT a = sT n = 0
||n|| = ||s|| = ||a|| = 1
it follows that R is an orthonormal matrix, i.e.
R RT = RT R = I3
I3 : 3 × 3 identity matrix
A rotation matrix is always invertible. By pre-multiplying by R−1 we have
R−1 = RT
0
i.e.
C. Melchiorri (DEIS)
R−1
1 =
1
R0 =
0
RT
1
Ridig Body Motion – Homogeneous Transformations
19 / 80
Ridig Body Motion
Rotations
Elementary rotations
Consider two frames F0 and F1 with coincident origins.
Rotations of θ about the x0 , y0 , and z0 axes
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
20 / 80
Ridig Body Motion
Rotations
Elementary rotations
In the first case, F1 is obtained with a rotation of an angle θ about the x0 axis of
F0 .
From

we have
C. Melchiorri (DEIS)
R=
0 T
i1 i
0 T
i1 j
0 T
i1 k
0 T
j1 i
0 T
j1 j
0 T
j1 k

0 T
k1 i
0 T
k1 j
0 T
k1 k
1
0
0
R1 = Rot(x, θ) =  0 cos θ
0 sin θ



0
− sin θ 
cos θ
Ridig Body Motion – Homogeneous Transformations
21 / 80
Ridig Body Motion
Rotations
Elementary rotations
Similarly, considering rotations about y0 and z0 :

cos θ 0 sin θ
0
0
1
0
R1 = Rot(y, θ) = 
− sin θ 0 cos θ

cos θ − sin θ 0
0
cos θ 0
R1 = Rot(z, θ) =  sin θ
0
0
1




Rotation matrices R relate different reference frames.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
22 / 80
Ridig Body Motion
Rotations
Rotations
Another interpretation for rotation matrices.
Let us consider a rotation of point 0 p1 = [7, 3, 2]T by 90o about z0 .
The matrix expressing the rotation is

cos 90o − sin 90o
o
cos 90o
R1 = Rot(z, 90 ) =  sin 90o
0
0


0 −1 0
0
0 
=  1
0
0
1
Therefore:
C. Melchiorri (DEIS)

0
0 
1

 


−3
0 −1 0
7
0
p2 =  7  =  1 0 0   3 
2
0 0 1
2
Ridig Body Motion – Homogeneous Transformations
23 / 80
Ridig Body Motion
Rotations
Rotations
Consider now a second rotation of 90o about y0 :



2
0 0
0
p3 =  7  = R2 0 p2 =  0 1
3
−1 0
C. Melchiorri (DEIS)


1
−3
0  7 
0
2
Ridig Body Motion – Homogeneous Transformations
24 / 80
Ridig Body Motion
Rotations
Rotations
By combining the two rotations one obtains


0 0 1
0
R = R2 R1 =  0 1 0   1
−1 0 0
0



from which
2
0
p3 =  7  = R 0 p1 = 
3
 
−1 0
0 0
0 0 = 1 0
0 1
0 1
 
0 0 1
7
1 0 0  3 
0 1 0
2

1
0 
0
Rotation matrices “rotates” vectors
with respect to a fixed reference frame.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
25 / 80
Ridig Body Motion
Rotations
Axis/angle rotations
Rotation θ about a generic unit vector w = [wx wy wz ]T .
The rotation of the angle θ about w is equivalent
to the following procedure:
Aligne w with z0
Rotate by θ about w ≡ z0
Restore w in its original position.
Each rotation is performed with respect to F0 , then:
Rot(w, θ) = Rot(z0 , α)Rot(y0 , β)Rot(z0 , θ)Rot(y0 , −β)Rot(z0 , −α)
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
26 / 80
Ridig Body Motion
Rotations
Axis/angle rotations
Moreover, since ||w|| = 1, we have:
wy
sin α = q
wx2 + wy2
q
sin β = wx2 + wy2
cos α = q
wx
wx2 + wy2
cos β = wz
The matrix R representing the rotation is therefore given by


wx wx Vθ + Cθ
wy wx Vθ − wz Sθ wz wx Vθ + wy Sθ
wy wy Vθ + Cθ
wz wy Vθ − wx Sθ 
R(w, θ) =  wx wy Vθ + wz Sθ
wx wz Vθ − wy Sθ wy wz Vθ + wx Sθ
wz wz Vθ + Cθ
being Cθ = cos θ, Sθ = sin θ, e Vθ = vers θ = 1 − cos θ.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
27 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
1. Not all the orthogonal matrices (RT R = I) for which the following conditions
T
n a = sT a = sT n = 0
||n|| = ||s|| = ||a|| = 1
are satisfied represent rotations. For example,

1 0

S = 0 −1
0 0
matrix

0
0 
1
does not represent a rotation, but rather a “specular” transformation.
It is not possible, starting from F0 , to obtain frame F1 with a rotation. F1 may be
obtained only by means of a specular reflection.
This is not physically feasible for a rigid
body.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
28 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
If matrix R represents a rigid body rotation, then
det(R) = 1
Because of their properties, the rotation matrices in IR3 belong to a “special set”,
the Special Orthogonal group of order 3, i.e. So(3).
More in general, the set of n × n matrices R satisfying the two conditions
(
RRT = RT R = I
det(R) = +1
is called So(n): Special Orthogonal group in IRn
=⇒
Special: det(R) = +1
=⇒
Orthogonal: RRT = RT R = I
So(n) = {R ∈ IRn×n : RRT = RT R = I, det(R) = +1}
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
29 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
2. The equations

nx
0
R1 = [ 0 n 0 s 0 a ] =  ny
nz
sx
sy
sz

ax
ay 
az
0
R−1
1 =
1
R0 =
0
RT
1
allow to consider the relative rotation of two frames, and to transform in
F0 vectors defined in F1 . The expression of 1 p in F0 is given by:


nx s x ax
0
p = 0 R1 1 p =  ny sy ay  1 p
nz s z az
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
30 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
The composition of more rotations is expressed by a simple matrix multiplication:
Given n + 1 reference frames F0 , ..., Fn with coincident origins and relative
orientation expressed by i −1 Ri , i = 1, . . . n, and given the vector n p in Fn , then
0
p=
0
R1 1 R2 ...n−1 Rn n p
A note about computational complexity:
0
0
1
2
3
p=
( R1 R2 R3 ) p =
0
p=
(0 R1 (1 R2 (2 R3 3 p)))
| {z }
2p
|
{z
1p
0
3
R3 p
63
42
27
→
18
→
products
summations
products
summations
}
3. From 0 p = 0 R1 1 p it follows that a rotation applied to vector 1 p is a linear
0
function:
p = r(1 p) = 0 R1 1 p.
Given two vectors p, q and two scalar
quantities a, b, we have
r(ap + bq) = ar(p) + br(q)
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
31 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
4. Rotations do not change the amplitude of a vector:
kRak = kak
As a matter of fact:
kRak = aT RT Ra = aT a = kak
5. The inner product, and then the angle between two vectors, is invariant with
respect to rotations:
aT b = (Ra)T (Rb)
As a matter of fact:
(Ra)T (Rb) = aT RT Rb = aT b = kak kbk cos θ
6. Since R is an orthogonal matrix, the following property holds
R(a × b) = Ra × Rb
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
32 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
7. In general, the product of rotation matrices does not commute:
Ra Rb 6= Rb Ra
Except the trivial case of the identity matrix (i.e. when R = I3 ), rotations
commute only if the rotation axis is the same!
Consider the two rotations

1
Ra = Rot(x, 900) =  0
0
C. Melchiorri (DEIS)
by a 90o angle about the x0 and y0 axes:


0 0
0 0
0 −1 
Rb = Rot(y, 90o ) =  0 1
1 0
−1 0
Ridig Body Motion – Homogeneous Transformations

1
0 
0
33 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
1)
2)
Case 1) Rb followed by Ra :

0
1
0
R = Rb Ra =  0
−1 0
C. Melchiorri (DEIS)

0
−1 
0
Case 2) Ra followed by Rb :

0 0
R = Ra Rb =  1 0
0 1
Ridig Body Motion – Homogeneous Transformations

1
0 
0
34 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
8. It may be of interest to define a sequence of rotations with respect to F0 , and
not with respect to the current frame Fi as assumed until now.
Consider two rotations R1 = Rot(y, φ) and R2 = Rot(z, θ)
about the axes y0 and z0 of F0 .
What is the result of applying first R1 and then R2 ?
Consider the vector 0 p in F0 . After the first rotation R1 , the new expression of the
vector (still wrt F0 ) is
0
p1 = R 1 0 p
Since also the second rotation is about an axis of F0 , we have
0
p2 = R 2 0 p1 = R 2 R 1 0 p
More in general, given n consecutive rotations Ri , i = 1, . . . , n defined with respect to
the same reference frame F0 , then
0
C. Melchiorri (DEIS)
pn = Rn Rn−1 . . . R1 0 p
Ridig Body Motion – Homogeneous Transformations
35 / 80
Ridig Body Motion
Rotations
Proprieties of rotations
Then, there are two different possibilities to define a sequence of consecutive
rotations:
1
If each rotation is expressed wrt the current frame Fn , F n−1 , . . . , F0 , then
the equivalent rotation matrix 0 Rn is obtained by post-multiplication of the
matrices i −1 Ri .
0
p = 0 R1 1 R2 . . . n−1 Rn n p
2
If matrices Ri , i = 1, . . . , n describe rotations about an axis of the base frame
F0 , the equivalent matrix is obtained by pre-multiplication of the matrices.
0
C. Melchiorri (DEIS)
pn = Rn Rn−1 . . . R1 0 p
Ridig Body Motion – Homogeneous Transformations
36 / 80
Ridig Body Motion
Rotations
Interpretations of a rotation matrix
In summary, a rotation matrix 0 R1 has three equivalent interpretations:
0
1.
R1 describes the mutual orientation of two
reference frames F0 and F1 ; the columns of
0
R1 are the direction cosines of the axes of
F1 expressed in F0
2.
0
3.
0
R1 defines the coordinate transformation
between the coordinates of a point
expressed in F0 and in F1 (common origin)
R1 rotates a vector 0 va to 0 vb in a given
reference frame F0
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
37 / 80
Ridig Body Motion
Rotations
Representations of rotations
A rotation is described with a 3 × 3 matrix with 9
elements:

nx
R =  ny
nz
sx
sy
sz

ax
ay 
az
On the other hand, a rigid body in IR3 → Three parameters should be sufficient
to describe its orientation
has 3 rotational dof
A 3 × 3 matrix, although computationally efficient, is redundant. Among the 9
elements of R one can define the following relations:
( T
n a = sT a = sT n = 0
||n|| = ||s|| = ||a|| = 1
Note that it is sufficient to know 6 elements of R to define completely the matrix.
If only 5 (or less) elements are known, R cannot be determined univocally.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
38 / 80
Ridig Body Motion
Rotations
Representations of rotations
Theoretically, only 3 parameters are sufficient to describe the orientation of a rigid
body in the 3D space.
There are representations based on 3 parameters only (minimal representations),
more “compact” than rotation matrices, although computationally less convenient.
Among these representations, we have:
Euler angles: three consecutive rotations about axes z, y′ , z′′
Roll, Pitch and Yaw angles: three consecutive rotations about axes z0 , y0 , x0
Axis/Angle representation: a unitary rotation axis r and the angle θ
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
39 / 80
Ridig Body Motion
Rotations
Euler angles
Euler angles (φ, θ, ψ) represents three rotations, applied sequentially about the
axes z0 , y1 , z2 of the current frame F0 , F1 , F2 .
Consider a base frame F0 . By applying the three rotations we have
- A frame F1 obtained with the rotation φ about z0
- A frame F2 obtained from F1 with the rotation θ about y1
- A frame F3 obtaine from F2 with the rotation ψ about z2
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
40 / 80
Ridig Body Motion
Rotations
Euler angles
By composing the three rotations, the total rotation from F0 to F3 is
0
R3 = REuler (φ, θ, ψ)
=
=
C. Melchiorri (DEIS)
Rot(z0 , φ)Rot(y1 , θ)Rot(z2 , ψ)

Cφ Cθ Cψ − Sφ Sψ −Cφ Cθ Sψ − Sφ Cψ
 Sφ Cθ Cψ + Cφ Sψ −Sφ Cθ Sψ + Cφ Cψ
−Sθ Cψ
Sθ Sψ
Ridig Body Motion – Homogeneous Transformations

Cφ Sθ
Sφ Sθ 
Cθ
41 / 80
Ridig Body Motion
Rotations
Euler angles
Rotation matrix corresponding to the Euler angles:

Cφ Cθ Cψ − Sφ Sψ −Cφ Cθ Sψ − Sφ Cψ
REuler (φ, θ, ψ) =  Sφ Cθ Cψ + Cφ Sψ −Sφ Cθ Sψ + Cφ Cψ
−Sθ Cψ
Sθ Sψ

Cφ Sθ
Sφ Sθ 
Cθ
Inverse problem: compute the Euler angles corresponding to a given rotation
matrix R:


r11 r12 r13
R =  r21 r22 r23 
→
(φ, θ, ψ) ?
r31 r32 r33
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
42 / 80
Ridig Body Motion
Rotations
Atan2 function
arctan( yx ) = arctan( −y
−x ), gives results in two quadrants (in [−π/2, +π/2])
atan2 is the arctangent with output values in the four quadrants:
two input arguments
gives values in [−π, +π]
undefined only for (0, 0)
uses the sign of both arguments to define the output quadrant
based on arctan function with output values in [−π/2, +π/2]
available in main languages (C++, Matlab, . . . )

arctan(y /x)




π + arctan(y /x)



−π + arctan(y /x)
atan2 (y , x) =
π/2




−π/2



undefined
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
x
y
y
y
y
y
>0
≥ 0, x
< 0, x
> 0, x
< 0, x
= 0, x
<0
<0
=0
=0
=0
43 / 80
Ridig Body Motion
Rotations
Euler angles
Two cases are possible:
1
2
2
r13
+ r23
6= 0 → sin θ 6= 0. By assuming 0 < θ < π (sin θ > 0), one obtains:


23 , r13 );
 φ = atan2 (rp
2 + r 2 , r );
θ = atan2 ( r13
23 33


ψ = atan2 (r32 , −r31 )
or, with −π < θ < 0, (sin θ < 0):


23 , −r13 );
 φ = atan2 (−rp
2 + r 2 , r );
θ = atan2 (− r13
23 33


ψ = atan2 (−r32 , r31 )
Two possible sets of solutions depending on the sign of sin θ.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
44 / 80
Ridig Body Motion
Rotations
Euler angles
2
2
2. r13
+ r23
= 0 (θ = 0, π and cos θ = ±1). By choosing θ = 0 (cos θ = 1)
one obtains
θ=0
φ + ψ = atan2 (r21 , r11 ) = atan2 (−r12 , r11 );
On the other hand, if θ = π (cos θ = −1)
θ=π
φ − ψ = atan2 (−r21 , −r11 ) = atan2 (−r12 , −r11 );
In both cases, infinite solutions are obtained (only the sum or difference of φ
and θ is known).
Being θ = 0, π, the rotations by the angles φ and ψ occur about parallel (the
same) axes, i.e. the z axis.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
45 / 80
Ridig Body Motion
Rotations
Roll, Pitch, Yaw
Consider three consecutive rotations about the axes of the base frame F0 :
A rotation ψ about x0 , (yaw),
A rotation θ about y0 , (pitch)
A rotation φ about z0 , (roll).
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
46 / 80
Ridig Body Motion
Rotations
Roll, Pitch, Yaw
By properly composing the three rotations:
0
R3 = RRPY (φ, θ, ψ)
=
=
C. Melchiorri (DEIS)
Rot(z0 , φ)Rot(y0 , θ)Rot(x0 , ψ)


Cφ Cθ −Sφ Cψ + Cφ Sθ Sψ Sφ Sψ + Cφ Sθ Cψ
 Sφ Cθ Cφ Cψ + Sφ Sθ Sψ −Cφ Sψ + Sφ Sθ Cψ 
−Sθ
Cθ Sψ
Cθ Cψ
Ridig Body Motion – Homogeneous Transformations
47 / 80
Ridig Body Motion
Rotations
Roll, Pitch, Yaw
Rotation matrix corresponding to the RPY angles:

Cφ Cθ −Sφ Cψ + Cφ Sθ Sψ
RRPY (φ, θ, ψ) =  Sφ Cθ Cφ Cψ + Sφ Sθ Sψ
−Sθ
Cθ Sψ

Sφ Sψ + Cφ Sθ Cψ
−Cφ Sψ + Sφ Sθ Cψ 
Cθ Cψ
Inverse problem: compute the RPY angles corresponding to a given rotation
matrix R:


r11 r12 r13
→
(φ, θ, ψ) (?)
R =  r21 r22 r23 
r31 r32 r33
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
48 / 80
Ridig Body Motion
Rotations
Roll, Pitch, Yaw
Two cases are possible:
1
2
2
r11
+ r21
6= 0 → cos θ 6= 0. By choosing θ ∈ [−π/2, π/2], one obtains:

);

 φ = atan2 (r21 , r11p
2 + r 2 );
θ = atan2 (−r31 , r32
33


ψ = atan2 (r32 , r33 );
Otherwise, if θ ∈ [π/2, 3π/2]:


11 );
 φ = atan2 (−r21 , −rp
2 + r 2 );
θ = atan2 (−r31 , − r32
33


ψ = atan2 (−r32 , −r33 );
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
49 / 80
Ridig Body Motion
Rotations
Roll, Pitch, Yaw
2
2
2. r11
+ r21
= 0 → cos θ = 0: θ = ±π/2 and infinite solutions are possible
(sum or difference of ψ and φ).
It may be convenient to (arbitrarily) assign a value (e.g. ±90o ) to one of the
two angles (φ or ψ) a ±90o and then compute the remaining one:
θ = ±π/2;
φ − ψ = atan2 (r23 , r13 ) = atan2 (−r12 , r22 );
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
50 / 80
Ridig Body Motion
Rotations
Angle/Axis representation
It is possible to describe any rotation in 3D by means of the rotation angle θ and
the corresponding rotation axis w
wx2 (1 − Cθ ) + Cθ
R =  wx wy (1 − Cθ ) + wz Sθ
wx wz (1 − Cθ ) − wy Sθ

wx wy (1 − Cθ ) − wz Sθ
wy2 (1 − Cθ ) + Cθ
wy wz (1 − Cθ ) + wx Sθ

wx wz (1 − Cθ ) + wy Sθ
wy wz (1 − Cθ ) − wx Sθ 
wz2 (1 − Cθ ) + Cθ
4 parameters:
wx , wy , wz , θ
with the condition:
wx2 + wy2 + wz2 = 1
=⇒ 3 dof
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
51 / 80
Ridig Body Motion
Rotations
Angle/Axis representation
Inverse problem: compute the axis w and the angle θ corresponding to a given
rotation matrix R:
r11 + r22 + r33 − 1
2


r32 − r23
1 
r13 − r31 
2 sin θ
r21 − r12
θ
= acos
w
=
The trace of a rotation matrix depends only on the (cosine of) rotation angle.
This representation suffers of some drawbacks:
It is not unique:
Rot(w, θ) = Rot(−w, −θ)
(we can arbitrarily assume 0 ≤ θ ≤ π)
If θ = 0 then Rot(w, 0) = I3 and w is indefinite
There are numerical problems if θ ≈ 0: in this case sin θ ≈ 0 and problems
may arise in computing w
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
52 / 80
Ridig Body Motion
Rotations
Example
Compute the rotation matrix corresponding to the RPY angles 0o , 45o , 90o , i.e.:
1
2
3
A rotation of 90o about x0 , (yaw)
A rotation of 45o about y0 , (pitch)
A rotation of 0o about z0 , (roll)
RPY rotations: roll = 0o , pitch = 45o , yaw = 90o
One obtains:

R=
C. Melchiorri (DEIS)
√1
2
√1
2
0
− √12
√1
2
0

0
−1 
0
Ridig Body Motion – Homogeneous Transformations
53 / 80
Ridig Body Motion
Rotations
Example

R=
√1
2
√1
2
0
− √12
√1
2
0

0
−1 
0
RPY rotations: roll = 0o , pitch = 45o , yaw = 90o
• The Euler angles corresponding to this rotation matrix are
φ = −90o ;
θ = 90o ;
ψ = 45o
• Considering the Angle/Axis representation, one obtains:
rotation of the angle θ = 98.42o about the axis w = [0.863, 0.357, −0.357]T
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
54 / 80
Ridig Body Motion
Rotations
Ridig Body Motion – Homogeneous Transformations
Translations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Università di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
55 / 80
Ridig Body Motion
Translations
Translations
Rotations between two coordinate frames can be expressed in matrix form:
0
p = R 1p
This is not possible for translations! → It is not possible to define a 3 × 3 matrix
P so that a translation can be expressed as a matrix/vector multiplication
0
p = P 1 p =⇒ not possible!!
A translation of a vector 0 p by 0 o corresponds to a
vectorial summation
0
q=
0
p + 0o
0
qy = 0 py + 0 oy
Then
0
qx = 0 px + 0 ox
C. Melchiorri (DEIS)
0
qz = 0 pz + 0 oz
Ridig Body Motion – Homogeneous Transformations
56 / 80
Ridig Body Motion
Translations
Translations
Then, a translation is expressed as a function
In general:
t(p) = p + o
t(ap + bq) = ap + bq + o 6= at(p) + bt(q)
Then, translations are not linear transformations!
The most general transformation between two coordinate frames cannot be
represented by a 3 × 3 matrix.
The composition of a rotation and a
translation is obtained from
0
q=
0
p + 0o
by considering vector p defined in F1 , rotated and translated with respect to F0 .
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
57 / 80
Ridig Body Motion
Translations
Translations
Since vectors can be added only if they are defined with respect to the same
coordinate system:
0
p=
0
R1 1 p + 0 o1
being 0 o1 the translation from F0 to F1 and 0 R1 the mutual rotation.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
58 / 80
Ridig Body Motion
Ridig Body Motion – Homogeneous Transformations
Homogeneous transformations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Università di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
59 / 80
Homogeneous transformations
Homogeneous transformations
It is of interest to put in matrix form the equation
0
p=
0
R1 1 p + 0 o1
since, in case of successive transformations, one could obtain expressions similar to
0
p=
0
R1 1 R2 ...n−1 Rn n p
For this purpose, it is possible to add to matrix R the vector 0 o as fourth column;
in this manner a 3 × 4 matrix is obtained
M = [n s a 0 o1 ] = [R 0 o1 ]
A square matrix is obtained by adding to M the row [0 0 0 1].
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
60 / 80
Homogeneous transformations
Homogeneous transformations
The Homogeneous Transformation Matrix 0 T1 is obtained

nx s x ax
0
0
 ny s y ay
o1
R1
0

=
T1 =
nz s z az
0
0
0 1
0 0 0

ox
oy 

oz 
1
This matrix represents the transformation (rotation-translation) between F0 and
F1 .
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
61 / 80
Homogeneous transformations
Homogeneous transformations
By defining the homogeneous 4-dimensional (in IR4 ) vector:
[pT 1]T = [px py pz 1]T
one obtains
0
p
1
=
0
T1
1
p
1
=
0
R1 1 p + 0 o1
1
The subspace IR3 defined by the first three components represents the
transformation 0 p = 0 R1 1 p + 0 o1 on vectors in IR3 , and physically represents
the rigid body motion from F1 to F0 ; the fourth component is not affected by
the matrix multiplication.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
62 / 80
Homogeneous transformations
Homogeneous transformations
Given the homogeneous transformation matrices 0 T1 , from F1 to F0 , and 1 T2 ,
from F2 to F1 , the composition
0
T2 =
0
T1 1 T2
can be applied to vectors [ 2 pT 1]T defined in F2 , and the result is
0
T1 (1 T2
2
p
1
) =
=
C. Melchiorri (DEIS)
1
R2 2 p + 1 o2
T1
1
0
R1 1 R2 2 p + 0 R1 1 o2 + 0 o
1
0
Ridig Body Motion – Homogeneous Transformations
63 / 80
Homogeneous transformations
Homogeneous transformations
This is equivalent to the product of the homogeneous matrix
0
T2 =
0
0
R1 1 R2
0
0
0
R1 1 o2 + 0 o
1
=
0
T1 1 T2
with the vector [ 2 pT 1]T .
In general we have
0
C. Melchiorri (DEIS)
Tn =
0
T1 1 T2 . . .
Ridig Body Motion – Homogeneous Transformations
n−1
Tn
64 / 80
Homogeneous transformations
Homogeneous transformation of coordinates
The coordinate transformation bewteen two reference frames F0 and F1 may be
expressed by a 4 × 4 matrix 0 T1 :
0
p=
0
T1 1 p
Of particular interest are the elementary transformations, i.e. simple rotations or
translations along the coordinate axes.
All the coordinate transformations may be obtained by combinations of these
elementary transformations.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
65 / 80
Homogeneous transformations
Elementary rotations and translations
The homogeneous transformation matrices corresponding to rotations of an angle
θ about the axes x, y, z of F0 are:
1
 0
Rot(x, θ)=
0
0

0
Cθ
Sθ
0
0
−Sθ
Cθ
0


Cθ
0
0 
 0
, Rot(y, θ)=
−Sθ
0 
0
1
0
1
0
0
Sθ
0
Cθ
0


0
0 

, Rot(z, θ)=
0 
1
Cθ
Sθ
0
0
−Sθ
Cθ
0
0
0
0
1
0

0
0 
0 
1
being Cθ = cos θ, Sθ = sin θ.
The homogeneous transformation T corresponding to the translation of vector
v = [vx vy vz ]T is

1
 0
T = Trasl(v) = 
 0
0
C. Melchiorri (DEIS)
0
1
0
0
0
0
1
0

vx
vy 

vz 
1
Ridig Body Motion – Homogeneous Transformations
66 / 80
Homogeneous transformations
Example
The frame F1 , with respect to F0 , is translated of
1 along x0 and of 3 along y0 , moreover, it is rotated
by 30o about z0 .
The transformation matrix from F1 to F0 is

0.866 −0.500 0 1
 0.500 0.866 0 3 
0

T1 = 
 0
0
1 0 
0
0
0 1

C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
67 / 80
Homogeneous transformations
Example
Consider a point defined in F1 by
 
2
1
p= 1 
0
Its coordinates in F0 are

0.866 −0.500 0

0.500 0.866 0
0
p = 0 T1 1 p = 
 0
0
1
0
0
0
C. Melchiorri (DEIS)

1
2
 1
3 

0  0
1
1
Ridig Body Motion – Homogeneous Transformations



2.232
  4.866 
=

  0 
1
68 / 80
Homogeneous transformations
Example
Consider the point in F0


2
0
pa =  1 
0
Let us apply to 0 p a translation of 1 along x0 , of 3 along y0 ; then rotate the vector
by 30o about z0 . The result is obtained by multiplying vector 0 p by the matrix


0.866 −0.500 0 1
 0.500 0.866 0 3 

T=
 0
0
1 0 
0
0
0 1
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
69 / 80
Homogeneous transformations
Example
Then

0.866 −0.500 0

0.500 0.866 0
0
pb = T 0 pa = T = 
 0
0
1
0
0
0

1
2
 1
3 

0  0
1
1



2.232
  4.866 
=

  0 
1
The same numerical result is obtained, although the physical interpretation is
different.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
70 / 80
Homogeneous transformations
Interpretations of a homogeneous transformation matrix
Similarly to rotation matrices, also an homogeneous transformation matrix 0 T1
0
R1 0 v
0
T1 =
000 1
has three possible physical interpretations:
1
2
3
Description of F1 in F0 : in particular 0 v represents the origin of F1 with
respect to F0 , and the elements of 0 R1 give the direction of the axes of F1
Coordinate transformation of vectors between F1 and F0 , 1 p →
0
p;
Translates and rotates a generic vector 0 pa to 0 pb in a given reference frame
F0
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
71 / 80
Homogeneous transformations
Inverse transformation
Once the position/orientation of F1 with respect to F0 are known, defined by the
homogeneous transformation matrix 0 T1 , it is simple to compute the inverse
transformation 1 T0 = (0 T1 )−1 , defining the position/orientation of F0 with
respect to F1 .
From
0
T1 1 T0 = I4
it follows that
1
T0 =
C. Melchiorri (DEIS)
0
0
RT
1
0
−0 RT
1v
0
1

nx
 sx
=
 ax
0
ny
sy
ay
0
Ridig Body Motion – Homogeneous Transformations
nz
sz
az
0

−vT n
−vT s 

−vT a 
1
72 / 80
Homogeneous transformations
Inverse transformation
As a matter of fact, if
1
M
000
M
000
T0 =
then
R
000
I3
0
=
=
v
1
and therefore
C. Melchiorri (DEIS)
RM
Rx + v
=⇒
x
1
x
1

1
 0

=
0
0
0
1
0
0
M =
x =
RT
−RT v
Ridig Body Motion – Homogeneous Transformations
0
0
1
0

0
0 

0 
1
73 / 80
Homogeneous transformations
Example
Given

0
 1
T=
 0
0
0
0
1
0
1
0
0
0

2
1 

0 
1
compute its inverse transformation.
Solution:
T−1 =
T
0
R
0
C. Melchiorri (DEIS)
T
0
−R v
1
nx

=  sx
ax
0

ny
sy
ay
0
nz
sz
az
0
 
−vT n
0
T
−v s  =  0
  1
−vT a
0
1
Ridig Body Motion – Homogeneous Transformations
1
0
0
0
0
1
0
0

−1
0 
−2 
1
74 / 80
Homogeneous transformations
Homogeneous transformations
The term homogeneous derives from projective geometry.
Equations describing lines and planes in projective geometry in IR3 are
homogeneous in the four variables x1 , x2 , x3 , x4 in IR4 .
In IR3 these equations, affine transformations, are non homogeneous in
x1 , x2 , x3 , (lines or planes not passing through the origin present a constant
term - non function of x1 , x2 , x3 - in their expression).
In general, computations of an affine transformations in IRn−1 may be
expressed as homogeneous linear transformations in IRn :
0
p =
0 ′
p
=
0
R1 1 p + 0 o1
0 0
R1
p
=
1
0
C. Melchiorri (DEIS)
affine transformation
0
o1
1
1
p
1
homogeneous transformation
Ridig Body Motion – Homogeneous Transformations
75 / 80
Homogeneous transformations
Homogeneous transformations
The most general expression for an homogeneous transformation is
D3×3 p3×1
Deformation Translation
T=
=
f1×3
s
Perspective
Scale
Note the terms
D, f1×3 and s.
These quantities, in robotics, are alway assumed as:
A rotation matrix
(D = R),
The null vector [0 0 0]
The unit gain
(f1×3 = [0 0 0]),
(s = 1)
In other cases (e.g. computer graphics), these quantities are used to obtain
deformations, perspective distortions, change of scaling factors, and so on (in
general: effects non applicable to rigid bodies).
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
76 / 80
Homogeneous transformations
Example
Consider the transformation matrix

a
 0
T=
 0
0
0
b
0
0
0
0
c
0

0
0 

0 
1
This transformation applies a different “gain” along the three reference axes
x, y, z. A deformation of the body is obtained.
Point p = [1, 1, 1]T is transformed to

a
 0

p1 = T p = 
0
0
0
b
0
0
0
0
c
0

1
0

0 
 1
0  1
1
1

a
  b 

=
  c 
1


With this matrix, a cube is transformed in a parallelepiped.
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
77 / 80
Homogeneous transformations
Example
Similarly, the transformation

1
 0
T=
 0
0
0
1
0
− f1

0
0 

0 
1
0
0
1
0
performs a perspective transformation along y. The coordinates x, y , z of a point
p are transformed in
80
70
x
′
y
′
=
x
1 − y /f
=
y
1 − y /f
60
50
40
f = −20
30
z′
=
z
1 − y /f
z
20
y
10
0
C. Melchiorri (DEIS)
x
0
10
20
30
40
Ridig Body Motion – Homogeneous Transformations
50
60
70
80
78 / 80
Homogeneous transformations
Equations with homogeneous transformations
Usually, in robotics it is necessary to specify the position/orientation of an object
with respect to different reference frames (e.g. wrt to the end-effector, to the base
frame, to other machines/tools, . . . ).
The following equation must be verified if the robot has to grasp the object:
B B T6 E = OG
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
79 / 80
Homogeneous transformations
Equations with homogeneous transformations
B B T6 E = OG
Usually, matrices B, O, G, E are known (and constant). Therefore, the equation
can be solved in terms of B T6
B
T6 = B−1 OGE−1
The robot configuration is then obtained.
Otherwise, the object position O (if not known) can be computed as
O = B B T6 EG−1
C. Melchiorri (DEIS)
Ridig Body Motion – Homogeneous Transformations
80 / 80