Ridig Body Motion Homogeneous Transformations - unibo.it · Ridig Body Motion...

Ridig Body Motion – Homogeneous Transformations

Claudio Melchiorri

Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)

Universita di Bologna

email: [email protected]

C. Melchiorri (DEIS) Ridig Body Motion – Homogeneous Transformations 1 / 80

Summary

1 Ridig Body MotionRotationsTranslations

2 Homogeneous transformations


Ridig Body Motion

Ridig Body Motion – Homogeneous TransformationsRidig Body Motion

Claudio Melchiorri





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.


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


Ridig Body Motion


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.


Ridig Body Motion


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


Ridig Body Motion


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


Ridig Body Motion


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


Ridig Body Motion


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 .


Ridig Body Motion


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).


Ridig Body Motion

Ridig Body Motion – Homogeneous TransformationsRotations

Claudio Melchiorri





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



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.



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 .



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



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



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.



Rotations

Rotation matrix:R = [n s a] =


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



Elementary rotations

Consider two frames F0 and F1 with coincident origins.

Rotations of θ about the x0, y0, and z0 axes




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 θ




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.



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



Rotations

Consider now a second rotation of 90o about y0:

0p3 =

273

= R20p2 =

0 0 10 1 0−1 0 0

−372



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.



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


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 θ.



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.




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}




2. The equations

0R1 = [ 0n 0s 0a ] =


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 =


1p




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



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




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




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




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




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



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



Representations of rotations

A rotation is described with a 3× 3 matrix with 9elements:

R =


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.



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 θ



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



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θ



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

→ (φ, θ, ψ) ?



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



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 θ.



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.



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).



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ψ



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

→ (φ, θ, ψ) (?)



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);



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);



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



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



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



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



Ridig Body Motion – Homogeneous TransformationsTranslations

Claudio Melchiorri





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



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 .



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.


Ridig Body Motion

Ridig Body Motion – Homogeneous TransformationsHomogeneous transformations

Claudio Melchiorri





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].




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 .




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.




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

]




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



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.



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



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



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



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



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.



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



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



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



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




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




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).



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.



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



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



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


Ridig Body Motion Homogeneous Transformations - unibo.it · Ridig Body Motion...

Documents

Transcript of Ridig Body Motion Homogeneous Transformations - unibo.it · Ridig Body Motion...