Post on 08-Jun-2018
Ridig Body Motion – Homogeneous Transformations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universita di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 1 / 80
Summary
1 Ridig Body MotionRotationsTranslations
2 Homogeneous transformations
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 2 / 80
Ridig Body Motion
Ridig Body Motion – Homogeneous TransformationsRidig Body Motion
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universita 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/orientationof 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 referenceframes.
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 rigidbodies, the links, connected by joints that allow arelative motion.
RIGID BODY: idealization of a solid body of fi-nite size in which deformation is neglected: thedistance between any two given points of a rigidbody remains constant in time regardless of exter-nal 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
uTv =n∑
i=1
uivi = ||u||||v|| cos θ u, v ∈ IRn
and the Euclidean norm
||u|| = uTu =
n∑
i=1
u2i u ∈ IRn
We often use Cartesian (right-handed) reference frames, with homogenousdimensions 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:
‖u‖ = uTW u
being W a matrix:
symmetric
positive definite
Often, W is a diagonal matrix.
Since W = VTV, then:
‖u‖ = uTW u = (uTVT )(Vu) = xTx
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 atranslation along the same axis.
Problem of describing the instantaneous position/orientation of a rigid body withrespect to a fixed base frame
Let 0o1 be the origin of the frame F1 fixed to therigid body, expressed in F0 . Each point of thebody has coordinates
1p = [1px1py
1pz ]T
constant with respect to F1 .
Since the body moves, the same point has coor-dinates 0p, expressed in F0 , variable in time
0p = [0px0py
0pz ]T
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 9 / 80
Ridig Body Motion
Rigid body and its representation
First problem: if point p is known inF1 , compute the equivalent represen-tation in F0 .
1p =⇒ 0p
The problem is solved by using the Homogeneous Transformation Matrix 0T1:
0T1 =
[0R1
0o10 0 0 1
]
=
nx sx ax oxny sy ay oynz sz az oz0 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 F0 and F1 share the same origin, and have a different orientation in space
2 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 TransformationsRotations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universita 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 0v in F0 , its components vx , vy , vz are the orthogonal projectionsof 0v on the coordinate axes.
vx = 0vT i = ||0v|| cosα1
vy = 0vT j = ||0v|| cosα2
vz = 0vTk = ||0v|| 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 0v indicates an axis of F1 , e.g. 0i1, then0i1 = [0ix
0ij0iz ]
T , where
0ix = 0iT1 i = cosα10iy = 0iT1 j = cosα20iz = 0iT1 k = cosα3
This is a well known result:
0i1 = [cosα1, cosα2, cosα3]T
the components of a unit vector with respect to a reference frame are its directioncosines.A similar results holds for the other directions 0j1 and 0k1.
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:
R =
0iT1 i0jT1 i
0kT1 i
0iT1 j0jT1 j
0kT1 j
0iT1 k0jT1 k
0kT1 k
0i1,0j1,
0k1: axes of F1 expressed in F0
i, j, k: axes of F0 .
C. Melchiorri (DEIS) 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 theprincipal 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 11√2
1√2
0
− 1√2
1√2
0
In generalx1 y1 z1
x0y0z0
r11 r12 r13r21 r22 r23r31 r32 r33
C. Melchiorri (DEIS) 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
0R1 = [0n 0s 0a] =
nx sx axny sy aynz sz az
defining the relative orientation between F0 and F1 .
Symbols n, s, a refer to a frame fixed on the end-effector (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 toy, z.
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 18 / 80
Ridig Body Motion Rotations
Rotations
Rotation matrix:R = [n s a] =
nx sx axny sy aynz sz az
From the conditions:{
nT 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
i.e. 0R−11 = 1R0 =
0RT1
C. Melchiorri (DEIS) 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 ofF0 .
From
R =
0iT1 i0jT1 i
0kT1 i0iT1 j
0jT1 j0kT1 j
0iT1 k0jT1 k
0kT1 k
we have
0R1 = Rot(x, θ) =
1 0 00 cos θ − sin θ0 sin θ cos θ
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 21 / 80
Ridig Body Motion Rotations
Elementary rotations
Similarly, considering rotations about y0 and z0:
0R1 = Rot(y, θ) =
cos θ 0 sin θ0 1 0
− sin θ 0 cos θ
0R1 = Rot(z, θ) =
cos θ − sin θ 0sin θ cos θ 00 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 0p1 = [7, 3, 2]T by 90o about z0.
The matrix expressing the rotation is
R1 = Rot(z, 90o) =
cos 90o − sin 90o 0sin 90o cos 90o 0
0 0 1
=
0 −1 01 0 00 0 1
Therefore:
0p2 =
−372
=
0 −1 01 0 00 0 1
732
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 23 / 80
Ridig Body Motion Rotations
Rotations
Consider now a second rotation of 90o about y0:
0p3 =
273
= R20p2 =
0 0 10 1 0−1 0 0
−372
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 24 / 80
Ridig Body Motion Rotations
Rotations
By combining the two rotations one obtains
R = R2R1 =
0 0 10 1 0−1 0 0
0 −1 01 0 00 0 1
=
0 0 11 0 00 1 0
from which0p3 =
273
= R 0p1 =
0 0 11 0 00 1 0
732
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 equivalentto 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:
sinα =wy
√
w 2x + w 2
y
cosα =wx
√
w 2x + w 2
y
sin β =√
w 2x + w 2
y cosβ = wz
The matrix R representing the rotation is therefore given by
R(w, θ) =
wxwxVθ + Cθ wywxVθ − wzSθ wzwxVθ + wySθwxwyVθ + wzSθ wywyVθ + Cθ wzwyVθ − wxSθwxwzVθ − wySθ wywzVθ + wxSθ wzwzVθ + 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{
nT a = sT a = sT n = 0||n|| = ||s|| = ||a|| = 1
are satisfied represent rotations. For example, matrix
S =
1 0 00 −1 00 0 1
does not represent a rotation, but rather a “specular” transformation.
It is not possible, starting from F0 , to ob-tain frame F1 with a rotation. F1 may beobtained only by means of a specular reflec-tion.This is not physically feasible for a rigidbody.
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 = RTR = I
det(R) = +1
is called So(n): Special Orthogonal group in IRn
=⇒ Special: det(R) = +1
=⇒ Orthogonal: RRT = RTR = I
So(n) = {R ∈ IRn×n : RRT = RTR = I, det(R) = +1}
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 29 / 80
Ridig Body Motion Rotations
Proprieties of rotations
2. The equations
0R1 = [ 0n 0s 0a ] =
nx sx axny sy aynz sz az
0R−11 = 1R0 =
0RT1
allow to consider the relative rotation of two frames, and to transform inF0 vectors defined in F1 . The expression of 1p in F0 is given by:
0p = 0R11p =
nx sx axny sy aynz sz az
1p
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 relativeorientation expressed by i−1Ri , i = 1, . . . n, and given the vector np in Fn , then
0p = 0R11R2...
n−1Rnnp
A note about computational complexity:
0p = (0R11R2
2R3)3p = 0R3
3p →{
63 products42 summations
0p = (0R1 (1R2 (2R33p)
︸ ︷︷ ︸2p
)
︸ ︷︷ ︸1p
) →{
27 products18 summations
3. From 0p = 0R11p it follows that a rotation applied to vector 1p is a linear
function: 0p = r(1p) = 0R11p. 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:
‖Ra‖ = ‖a‖
As a matter of fact:
‖Ra‖ = aTRTRa = aTa = ‖a‖
5. The inner product, and then the angle between two vectors, is invariant withrespect to rotations:
aTb = (Ra)T (Rb)
As a matter of fact:
(Ra)T (Rb) = aTRTRb = aTb = ‖a‖ ‖b‖ 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:
RaRb 6= RbRa
Except the trivial case of the identity matrix (i.e. when R = I3), rotationscommute only if the rotation axis is the same!
Consider the two rotations by a 90o angle about the x0 and y0 axes:
Ra = Rot(x, 900) =
1 0 00 0 −10 1 0
Rb = Rot(y, 90o) =
0 0 10 1 0−1 0 0
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 33 / 80
Ridig Body Motion Rotations
Proprieties of rotations
1)
2)
Case 1) Rb followed by Ra:
R = RbRa =
0 1 00 0 −1−1 0 0
Case 2) Ra followed by Rb:
R = RaRb =
0 0 11 0 00 1 0
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 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 , andnot 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 0p in F0 . After the first rotation R1, the new expression of thevector (still wrt F0 ) is
0p1 = R1
0p
Since also the second rotation is about an axis of F0 , we have0p2 = R2
0p1 = R2 R1
0p
More in general, given n consecutive rotations Ri , i = 1, . . . , n defined with respect tothe same reference frame F0 , then
0pn = Rn Rn−1 . . .R1
0p
C. Melchiorri (DEIS) 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 consecutiverotations:
1 If each rotation is expressed wrt the current frame Fn , Fn−1, . . . , F0 , thenthe equivalent rotation matrix 0Rn is obtained by post-multiplication of thematrices i−1Ri .
0p = 0R11R2 . . . n−1Rn
np
2 If matrices Ri , i = 1, . . . , n describe rotations about an axis of the base frameF0 , the equivalent matrix is obtained by pre-multiplication of the matrices.
0pn = Rn Rn−1 . . . R10p
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 36 / 80
Ridig Body Motion Rotations
Interpretations of a rotation matrix
In summary, a rotation matrix 0R1 has three equivalent interpretations:
1. 0R1 describes the mutual orientation of tworeference frames F0 and F1 ; the columns of0R1 are the direction cosines of the axes ofF1 expressed in F0
2. 0R1 defines the coordinate transformationbetween the coordinates of a pointexpressed in F0 and in F1 (common origin)
3. 0R1 rotates a vector 0va to 0vb in a givenreference 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 9elements:
R =
nx sx axny sy aynz sz az
On the other hand, a rigid body in IR3
has 3 rotational dof→ Three parameters should be sufficient
to describe its orientation
A 3× 3 matrix, although computationally efficient, is redundant. Among the 9elements of R one can define the following relations:
{nT 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 rigidbody 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 theaxes 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
0R3 = REuler (φ, θ, ψ) = Rot(z0, φ)Rot(y1, θ)Rot(z2, ψ)
=
CφCθCψ − SφSψ −CφCθSψ − SφCψ CφSθSφCθCψ + CφSψ −SφCθSψ + CφCψ SφSθ
−SθCψ SθSψ Cθ
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 41 / 80
Ridig Body Motion Rotations
Euler angles
Rotation matrix corresponding to the Euler angles:
REuler (φ, θ, ψ) =
CφCθCψ − SφSψ −CφCθSψ − SφCψ CφSθSφCθCψ + CφSψ −SφCθSψ + CφCψ SφSθ
−SθCψ SθSψ Cθ
Inverse problem: compute the Euler angles corresponding to a given rotationmatrix R:
R =
r11 r12 r13r21 r22 r23r31 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 argumentsgives 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, . . . )
atan2 (y , x) =
arctan(y/x) x > 0π + arctan(y/x) y ≥ 0, x < 0−π + arctan(y/x) y < 0, x < 0π/2 y > 0, x = 0−π/2 y < 0, x = 0undefined y = 0, x = 0
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 43 / 80
Ridig Body Motion Rotations
Euler angles
Two cases are possible:
1 r213+ r223 6= 0 → sin θ 6= 0. By assuming 0 < θ < π (sin θ > 0), one obtains:
φ = atan2 (r23, r13);
θ = atan2 (√
r213 + r223, r33);
ψ = atan2 (r32,−r31)
or, with −π < θ < 0, (sin θ < 0):
φ = atan2 (−r23,−r13);
θ = atan2 (−√
r213 + r223, r33);
ψ = 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. r213 + r223 = 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 (thesame) 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:
0R3 = RRPY (φ, θ, ψ) = 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ψ
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 47 / 80
Ridig Body Motion Rotations
Roll, Pitch, Yaw
Rotation matrix corresponding to the RPY angles:
RRPY (φ, θ, ψ) =
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ψ
Inverse problem: compute the RPY angles corresponding to a given rotationmatrix R:
R =
r11 r12 r13r21 r22 r23r31 r32 r33
→ (φ, θ, ψ) (?)
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 48 / 80
Ridig Body Motion Rotations
Roll, Pitch, Yaw
Two cases are possible:
1 r211 + r221 6= 0 → cos θ 6= 0. By choosing θ ∈ [−π/2, π/2], one obtains:
φ = atan2 (r21, r11);
θ = atan2 (−r31,√
r232 + r233);
ψ = atan2 (r32, r33);
Otherwise, if θ ∈ [π/2, 3π/2]:
φ = atan2 (−r21,−r11);
θ = atan2 (−r31,−√
r232 + r233);
ψ = atan2 (−r32,−r33);
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 49 / 80
Ridig Body Motion Rotations
Roll, Pitch, Yaw
2. r211 + r221 = 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 thetwo 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 θ andthe corresponding rotation axis w
R =
w2x (1− Cθ) + Cθ wxwy (1− Cθ)− wzSθ wxwz (1− Cθ) + wySθ
wxwy (1− Cθ) + wzSθ w2y (1− Cθ) + Cθ wywz (1− Cθ)− wxSθ
wxwz (1− Cθ)− wySθ wywz (1− Cθ) + wxSθ w2z (1− Cθ) + Cθ
4 parameters: wx ,wy ,wz , θ
with the condition: w 2x + w 2
y + w 2z = 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 givenrotation matrix R:
θ = acosr11 + r22 + r33 − 1
2
w =1
2 sin θ
r32 − r23r13 − r31r21 − r12
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 problemsmay 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 A rotation of 90o about x0, (yaw)
2 A rotation of 45oabout y0, (pitch)
3 A rotation of 0o about z0, (roll)
RPY rotations: roll = 0o , pitch = 45o , yaw = 90o
One obtains:
R =
1√2
1√2
0
0 0 −1− 1√
21√2
0
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 53 / 80
Ridig Body Motion Rotations
Example
R =
1√2
1√2
0
0 0 −1− 1√
21√2
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 TransformationsTranslations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universita 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:
0p = R 1p
This is not possible for translations! → It is not possible to define a 3× 3 matrixP so that a translation can be expressed as a matrix/vector multiplication
0p = P 1p =⇒ not possible!!
A translation of a vector 0p by 0o corresponds to avectorial summation
0q = 0p + 0o
Then
0qx = 0
px+0ox
0qy = 0
py +0oy
0qz = 0
pz+0oz
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 56 / 80
Ridig Body Motion Translations
Translations
Then, a translation is expressed as a function t(p) = p+ o
In general: 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 berepresented by a 3× 3 matrix.
The composition of a rotation and atranslation is obtained from
0q = 0p+ 0o
by considering vector p defined in F1 , ro-tated 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 samecoordinate system:
0p = 0R11p+ 0o1
being 0o1 the translation from F0 to F1 and 0R1 the mutual rotation.
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 58 / 80
Ridig Body Motion
Ridig Body Motion – Homogeneous TransformationsHomogeneous transformations
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universita 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
0p = 0R11p+ 0o1
since, in case of successive transformations, one could obtain expressions similar to
0p = 0R11R2...
n−1Rnnp
For this purpose, it is possible to add to matrix R the vector 0o as fourth column;in this manner a 3× 4 matrix is obtained
M = [n s a 0o1] = [R 0o1]
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 0T1 is obtained
0T1 =
[0R1
0o10 0 0 1
]
=
nx sx ax oxny sy ay oynz sz az oz0 0 0 1
This matrix represents the transformation (rotation-translation) between F0 andF1 .
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
[0p
1
]
= 0T1
[1p
1
]
=
[0R1
1p+ 0o11
]
The subspace IR3 defined by the first three components represents the
transformation 0p = 0R11p+ 0o1 on vectors in IR
3, and physically representsthe rigid body motion from F1 to F0 ; the fourth component is not affected bythe matrix multiplication.
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 62 / 80
Homogeneous transformations
Homogeneous transformations
Given the homogeneous transformation matrices 0T1, from F1 to F0 , and 1T2,from F2 to F1 , the composition
0T2 =0T1
1T2
can be applied to vectors [ 2pT 1]T defined in F2 , and the result is
0T1(1T2
[2p
1
]
) = 0T1
[1R2
2p+ 1o21
]
=
[0R1
1R22p+ 0R1
1o2 +0o
1
]
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 63 / 80
Homogeneous transformations
Homogeneous transformations
This is equivalent to the product of the homogeneous matrix
0T2 =
[0R1
1R20R1
1o2 +0o
0 0 0 1
]
= 0T11T2
with the vector [ 2pT 1]T .
In general we have
0Tn = 0T11T2 . . . n−1Tn
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 64 / 80
Homogeneous transformations
Homogeneous transformation of coordinates
The coordinate transformation bewteen two reference frames F0 and F1 may beexpressed by a 4× 4 matrix 0T1:
0p = 0T11p
Of particular interest are the elementary transformations, i.e. simple rotations ortranslations along the coordinate axes.All the coordinate transformations may be obtained by combinations of theseelementary 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:
Rot(x, θ)=
1 0 0 00 Cθ −Sθ 00 Sθ Cθ 00 0 0 1
,Rot(y, θ)=
Cθ 0 Sθ 00 1 0 0
−Sθ 0 Cθ 00 0 0 1
,Rot(z, θ)=
Cθ −Sθ 0 0Sθ Cθ 0 00 0 1 00 0 0 1
being Cθ = cos θ, Sθ = sin θ.
The homogeneous transformation T corresponding to the translation of vectorv = [vx vy vz ]
T is
T = Trasl(v) =
1 0 0 vx0 1 0 vy0 0 1 vz0 0 0 1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 66 / 80
Homogeneous transformations
Example
The frame F1 , with respect to F0 , is translated of1 along x0 and of 3 along y0 , moreover, it is rotatedby 30o about z0.
The transformation matrix from F1 to F0 is
0T1 =
0.866 −0.500 0 10.500 0.866 0 30 0 1 00 0 0 1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 67 / 80
Homogeneous transformations
Example
Consider a point defined in F1 by
1p =
210
Its coordinates in F0 are
0p = 0T11p =
0.866 −0.500 0 10.500 0.866 0 30 0 1 00 0 0 1
2101
=
2.2324.86601
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 68 / 80
Homogeneous transformations
Example
Consider the point in F0
0pa =
210
Let us apply to 0p a translation of 1 along x0, of 3 along y0; then rotate the vectorby 30o about z0. The result is obtained by multiplying vector 0p by the matrix
T =
0.866 −0.500 0 10.500 0.866 0 30 0 1 00 0 0 1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 69 / 80
Homogeneous transformations
Example
Then
0pb = T 0pa = T =
0.866 −0.500 0 10.500 0.866 0 30 0 1 00 0 0 1
2101
=
2.2324.86601
The same numerical result is obtained, although the physical interpretation isdifferent.
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 0T1
0T1 =
[0R1
0v
0 0 0 1
]
has three possible physical interpretations:
1 Description of F1 in F0 : in particular 0v represents the origin of F1 withrespect to F0 , and the elements of 0R1 give the direction of the axes of F1
2 Coordinate transformation of vectors between F1 and F0 , 1p → 0p;
3 Translates and rotates a generic vector 0pa to 0pb in a given reference frameF0
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 thehomogeneous transformation matrix 0T1, it is simple to compute the inversetransformation 1T0 = (0T1)
−1, defining the position/orientation of F0 withrespect to F1 .
From0T1
1T0 = I4
it follows that
1T0 =
[0RT
1 −0RT1 v
0 0 0 1
]
=
nx ny nz −vTn
sx sy sz −vT s
ax ay az −vTa
0 0 0 1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 72 / 80
Homogeneous transformations
Inverse transformation
As a matter of fact, if
1T0 =
[M x
0 0 0 1
]
then
[R v
0 0 0 1
] [M x
0 0 0 1
]
=
1 0 0 00 1 0 00 0 1 00 0 0 1
and therefore
I3 = RM
0 = Rx+ v=⇒ M = RT
x = −RTv
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 73 / 80
Homogeneous transformations
Example
Given
T =
0 0 1 21 0 0 10 1 0 00 0 0 1
compute its inverse transformation.
Solution:
T−1 =
[
RT−RTv
0 0 0 1
]
=
nx ny nz −vTn
sx sy sz −vT s
ax ay az −vT a0 0 0 1
=
0 1 0 −10 0 1 01 0 0 −20 0 0 1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 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 constantterm - 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:
0p = 0R11p+ 0o1 affine transformation
0p′ =
[0p
1
]
=
[0R1
0o10 1
] [1p
1
]
homogeneous transformation
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 75 / 80
Homogeneous transformations
Homogeneous transformations
The most general expression for an homogeneous transformation is
T =
[D3×3 p3×1
f1×3 s
]
=
[Deformation TranslationPerspective 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] (f1×3 = [0 0 0]),
The unit gain (s = 1)
In other cases (e.g. computer graphics), these quantities are used to obtaindeformations, perspective distortions, change of scaling factors, and so on (ingeneral: effects non applicable to rigid bodies).
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 76 / 80
Homogeneous transformations
Example
Consider the transformation matrix
T =
a 0 0 00 b 0 00 0 c 00 0 0 1
This transformation applies a different “gain” along the three reference axesx, y, z. A deformation of the body is obtained.
Point p = [1, 1, 1]T is transformed to
p1 = T p =
a 0 0 00 b 0 00 0 c 00 0 0 1
1111
=
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
T =
1 0 0 00 1 0 00 0 1 00 −
1f
0 1
performs a perspective transformation along y. The coordinates x , y , z of a pointp are transformed in
x′ =
x
1− y/f
y′ =
y
1− y/f
z′ =
z
1− y/f
0 10 20 30 40 50 60 70 800
10
20
30
40
50
60
70
80
x
y
z
f = −20
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 78 / 80
Homogeneous transformations
Equations with homogeneous transformations
Usually, in robotics it is necessary to specify the position/orientation of an objectwith respect to different reference frames (e.g. wrt to the end-effector, to the baseframe, to other machines/tools, . . . ).
The following equation must be verified if the robot has to grasp the object:
B BT6E = OG
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 79 / 80
Homogeneous transformations
Equations with homogeneous transformations
B BT6E = OG
Usually, matrices B,O,G,E are known (and constant). Therefore, the equationcan be solved in terms of BT6
BT6 = B−1OGE−1
The robot configuration is then obtained.Otherwise, the object position O (if not known) can be computed as
O = B BT6EG−1
C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 80 / 80