Fundamentals of Robotic Mechanical Systemsrmsl/Index/Documents/ch2_slds_En.pdfFundamentals of...

Fundamentals of Robotic MechanicalSystems

Chapter 2: Mathematical Background

Jorge Angeles

Department of Mechanical Engineering &Centre for Intelligent Machines

McGill University

2009

Jorge Angeles (McGill University ) Chapter 2: Mathematical Background 2009 1 / 122

Outline

1 Preambule

2 Linear Transformations

3 Rigid-Body RotationsThe Cross-Product MatrixThe Rotation MatrixThe Linear Invariants of a 3× 3 MatrixLinear Invariants of a RotationExamplesEuler-Rodrigues Parameters

4 Composition of Reflections and Rotations

5 Coordinate Transformations and Homogeneous Coordinates


Preamble

Links are regarded as rigid bodies

General motion : rotation and translation

Invariant concepts : items that do not change upon a change of coordinateframe

Examples : distances between points and angles between lines

Vectors vs. components

Tensors vs. matricesWe will use “matrix” whenever we mean “tensor”.


Linear Transformations

Vector space : Set of objects called vectorsthat follow certain algebraic rules

The physical 3-dimensional space is a particular case of a vector space

vectors : in boldface lower case

tensors and matrices : in boldface upper case


Algebraic Rules for Vectors

Let v, v1, v2, v3 and w be elements of a vector space V over the real fieldwhile α and β are real numbers :

(i) v1 + v2 = v2 + v1

(ii) v + 0 = v where 0 : the zero vector

(iii) v1 + (v2 + v3) = (v1 + v2) + v3

(iv) v + w = 0 ⇒ w = −v

(v) α(βv) = (αβ)v(vi) α = 1 ⇒ αv = v

(vii) (α+ β)v = αv + βv

α(v1 + v2) = αv1 + αv2


Linear Transformations (Cont’d)

L : linear transformation operator over the real field

v = Lu

Properties of linear transformations :(i) homogeneity : L(αu) = αv

(ii) additivity : L(u1 + u2) = v1 + v2

Range of L : v = Lu for every u in UKernel of L : uN of U mapped by L into 0 ∈ V

Examples of transformations in 3-dimensional Euclidean space E3 :

projections, reflections, rotations, affine transformations


Linear Transformations (Cont’d)Orthogonal projection P of E3 onto itself : linear transformation of E3 onto aplane Π passing through the origin with unit normal n. Properties :

P2 = P : idempotent; Pn = 0 : singular (2.1a)

The projection p′ of p onto a plane Π of unit vector n :

FIG.: Projection.

p′ = p− n(nTp) = (1− nnT )p = Pp (2.1b)

P = 1− nnT (2.4)



Reflection R of E3 onto a plane Π passing through the origin with a unitnormal n :

FIG.: reflection.

p′ = p− 2nnTp ≡ (1− 2nnT )p = Rp

⇒ R = 1− 2nnT (2.5)

Properties of reflections : R2 = 1, R−1 = R



Orthogonal decompositionv = v‖ + v⊥

axial component (“v-par”) :v‖ ≡ eeTv (2.6a)

normal component (“v-perp”)v⊥ ≡ v − v‖ ≡ (1− eeT )v (2.6b)

Vector space

Basis of a vector space V : set of linearly independent vectors {vi}n1 where nis the dimension of V

v = α1v1 + α2v2 + · · ·+ αnvn (2.7)



A linear transformation of U into V with bases BU = {uj}m1 andBV = {vi}n1 :

Luj = l1jv1 + l2jv2 + · · ·+ lnjvnj = 1, . . . ,m

⇒ [L]BVBU ≡

l11 l12 · · · l1ml21 l22 · · · l2m...

.... . .

...ln1 ln2 · · · lnm

(2.10)

DefinitionThe jth column of L with respect to BU and BV is composed of the n realcoefficients lij of the representation of the image of the jth vector of BU interms of BV .



If Le = λe,

then e : eigenvector of L,

λ : eigenvalue of L

Characteristic polynomial of L :

det(λ1− L) = 0 (2.11)


Example

Example

What is the representation of the reflection R of E3 into itself, with respect tothe x-y plane, in terms of unit vectors parallel to the X,Y, Z axes that formthe coordinate frame F ?

Ri = i, Rj = j, Rk = −k

[Ri]F =

100

, [Rj]F =

010

, [Rk]F =

00−1

⇒ [R]F =

1 0 00 1 00 0 −1


Rigid-Body Rotations

Linear isomorphism : one-to-one linear transformation mapping space V ontoitself

Isometry : distance between any two points is preserved :

d ≡√

(u− v)T (u− v) (2.12)

where u and v are position vectors of two points.

Volume of tetrahedron formed by triad {u, v, w} :

V ≡ 16|u× v ·w| = 1

6

∣∣det([

u v w])∣∣ (2.13)


Rigid-Body Rotations (Cont’d)

Magnitudes of vectors {u, v, w} :

‖u‖ ≡√

uTu, ‖v‖ ≡√

vTv, ‖w‖ ≡√

wTw (2.14)

Under isotropy mapping {u, v, w} into {u′, v′, w′} :

‖u′‖ = ‖u‖, ‖v′‖ = ‖v‖, ‖w′‖ = ‖w‖ (2.15a)

det[u′ v′ w′] = ±det[u v w] (2.15b)

If “+”, then rotation ; otherwise, reflection



A position vector p of a point in E3 undergoes a rotation Q :

p′ = Qp , pTp = p′T p′

⇒ QTQ = 1 and Q is an orthogonal matrix.

Moreover, for T ≡ [u v w] and T′ ≡ [u′ v′ w′],

T′ = QT (2.20)

⇒ det(T) = det(T′) (2.21a)

det(Q) = +1 (2.21b)

⇒ Q is a proper orthogonal matrix.



TheoremThe eigenvalues of a proper orthogonal matrix Q lie on the unit circlecentered at the origin of the complex plane.

Proof :Qe = λe , e∗Q∗ = λe∗

λ : eigenvalue of Q, e : eigenvector of Q

λ : complex conjugate of λ, e∗ : complex conjugate of e

For real Q : Q∗ = QT , e∗ = eT

eTQ∗Qe = λλeTe ⇒ eTQTQe = λλeTe (2.25)

Q is orthogonal ⇒ eTe =| λ |2 eTe ⇒ | λ |2= 1 (2.27)



Corollary

A proper orthogonal 3× 3 matrix has at least one eigenvalue that is +1.

Qe = e (2.28)

Theorem(Euler, 1776)

A rigid-body motion about a point O leaves fixed a set of points lying on aline L that passes through O and is parallel to eigenvector e of Q associatedwith the eigenvalue +1.



Theorem(Cayley-Hamilton) If P (λ) is the characteristic polynomial of n× n matrixA, i.e.,

P (λ) = det(λ1−A)= λn + an−1λ

n−1 + · · ·+ a1λ+ a0 (2.29)

then,An + an−1An−1 + · · ·+ a1A + a01 = O (2.30)

O : n× n zero matrix

Any power p ≥ n of the n× n matrix A can be expressed as linearcombination of the first n powers of A :

1,A, · · · ,An−1 where 1 = A0


The Cross-Product Matrix

If u = u(t) ∈ U and v = v(u(t)) ∈ V are m- and n-dimensional vectors,respectively, while t is a real variable, and f = f(u,v) is a real-valuedfunction, then

u̇(t) =

u̇1(t)u̇2(t)

...u̇m(t)

, v̇(t) =

v̇1(t)v̇2(t)

...v̇n(t)


Cross-Product Matrix (Cont’d)

∂f

∂u≡

∂f/∂u1

∂f/∂u2

...∂f/∂um

,∂f

∂v≡

∂f/∂v1∂f/∂v2

...∂f/∂vn

(2.31)

∂v∂u≡

∂v1/∂u1 ∂v1/∂u2 · · · ∂v1/∂um

∂v2/∂u1 ∂v2/∂u2 · · · ∂v2/∂um

......

. . ....

∂vn/∂u1 ∂vn/∂u2 · · · ∂vn/∂um

(2.32)

df

du=∂f

∂u+(∂v∂u

)T∂f

∂v(2.33)



If f = f(u,v, t) and v = v(u, t), then

df

dt=

∂f

∂t+(∂f

∂u

)T dudt

+(∂f

∂v

)T ∂v∂t

+(∂f

∂v

)T ∂v∂u

dudt

(2.34)

and

dvdt

=∂v∂t

+∂v∂u

dudt

(2.35)


Example

ExampleLet the components of v and x in certain reference frame F be given as

[v]F =

v1v2v3

, [x]F =

x1

x2

x3

(2.36a)

what are the components of [v × x] and[

∂(v×x)∂x

]in the same frame F ?

Solution :

[v × x]F =

v2x3 − v3x2

v3x1 − v1x3

v1x2 − v2x1

(2.36b)

⇒[∂(v × x)

∂x

]F

=

0 −v3 v2v3 0 −v1−v2 v1 0

(2.36c)



v × x = Vx (2.37)

TheoremThe cross-product matrix A of any 3-dimensional vector a is skew-symmetric,i.e.,

AT = −A ⇒ a× (a× b) = A2b (2.38)

withA2 = −‖a‖21 + aaT (2.39)

and ‖ · ‖ being the Euclidean norm of the vector.

The cross-product matrix (CPM) A of any 3-dimensional vector a is uniquelydefined.


The Rotation Matrix

FIG.: Rotation of a rigid body about a line.

p′ =−−→OQ +

−−−→QP ′ (2.40)

−−→OQ : the axial component of p along

−→e

−−−→QP : normal component of p with respect to e


Rotation Matrix (Cont’d)

−−→OQ= eeTp (2.41)

−−−→QP ′= (cosφ)

−−−→QP +(sinφ)

−−−−→QP ′′ (2.42)

−−→QP= (1− eeT )p,

−−−−→QP ′′= e× p ≡ Ep

⇒−−−→QP ′= cosφ(1− eeT )p + sinφEp (2.45)

p′ = [eeT + cosφ(1− eeT ) + sinφE]p (2.47)

p′ is a linear transformation of p with a rotation matrix

Q = eeT + cosφ(1− eeT ) + sinφE (2.48)

e, φ : The natural invariants of Q



Special cases :

φ = π ⇒ Q = −1 + 2eeT (2.49)

φ = 0 ⇒ Q = 1

leading to a symmetric Q.

Under no circumstance does the rotation matrix become skew-symmetric, fora 3× 3 skew-symmetric matrix is by necessity singular.



The series expansion of Q for a fixed value of e :

Q(φ) = Q(0) + Q′(0)φ+12!

Q′′(0)φ2 + · · ·+ 1k!

Q(k)(0)φk + · · ·

where (k) is the kth derivative of Q with respect to φ.

Since Q(k)(0) = Ek, E(2k+1) = (−1)kE,E2k = (−1)k(1− eeT ),

Q(φ) = 1 + Eφ+12!

E2φ2 + · · ·+ 1k!

Ekφk + · · ·

Q(φ) = eEφ (2.53)



Q(φ) = 1+

[− 12!φ2 +

14!φ4 − · · ·+ 1

(2k)!(−1)kφ2k + · · ·︸︷︷︸

cosφ−1

](1− eeT )

+[φ− 13!φ3 + · · ·+ 1

(2k + 1)!(−1)kφ2k+1 + · · ·︸︷︷︸

sinφ

]E

Q = 1 + sinφE + (1− cosφ)E2 (2.54)


Rotation Matrix (Cont’d)Canonical Forms of the Rotation Matrix

If X axis is parallel to the axis of rotation, i.e., to [e]X =[

1 0 0]T , then

[E]X =

0 0 00 0 −10 1 0

, [E2]X =

0 0 00 −1 00 0 −1

[Q]X =

1 0 00 cosφ − sinφ0 sinφ cosφ

(2.55a)

[Q]Y =

cosφ 0 sinφ0 1 0

− sinφ 0 cosφ

(2.55b)

[ Q ]Z =

cosφ − sinφ 0sinφ cosφ 0

0 0 1

(2.55c)


The Linear Invariants of a 3× 3 Matrix

For any 3× 3 matrix A :Cartesian decomposition

AS ≡12

(A + AT ), ASS ≡12

(A−AT ) (2.56)

AS : symmetric part, ASS : skew-symmetric part.

The axial vector is a vector a with the property

a× v ≡ ASSv (2.57)

for any vector v.

The trace of A is the sum of the eigenvalues of AS .


Linear Invariants of a 3× 3 Matrix (Cont’d)

Let entries of matrix A be aij , for i, j = 1, 2, 3. Then,

vect(A) ≡ a ≡ 12

a32 − a23

a13 − a31

a21 − a12

tr(A) ≡ a11 + a22 + a33 (2.58)



TheoremThe vector of a 3× 3 matrix vanishes if and only if it is symmetric, whereasthe trace of an n× n matrix vanishes if the matrix is skew symmetric.

For 3-dimensional vectors a and b :

vect(abT ) = −12a× b (2.59)

tr(abT ) = aTb (2.60)



Proof of relation (2.59) :Let w denote vect(abT )

⇒ w × v = Wv (2.61)

W : skew-symmetric component of abT

W ≡ 12

(abT − baT ) (2.62)

⇒ Wv = w × v =12

[(bTv)a− (aTv)b] (2.63)



Compare relation (2.63) with the double-cross product :

(b× a)× v = (bTv)a− (aTv)b (2.64)

⇒ w =12b× a (2.65)

⇒ The trace of an n× n matrix can vanish without the matrix necessarilybeing skew-symmetric, while the trace of a skew-symmetric matrixnecessarily vanishes.


Linear Invariants of a Rotation

Matrices 1, eeT and cosφ(1− eeT ) are symmetric ;

matrix sinφE is skew-symmetric. Hence,

vect(Q) = vect(sinφE) = sinφ e (2.66)

tr(Q) = tr[eeT + cosφ(1− eeT )]≡ eTe + cosφ(3− eTe)= 1 + 2 cosφ (2.67)

cosφ =tr(Q)− 1

2(2.68)


Linear Invariants of a Rotation (Cont’d)

vect(Q) ≡ q =

q1q2q3

in a given frame.

Introducing q0 ≡ cosφ as a linear invariant of the rotation matrix, the rotationmatrix can be fully defined by four scalar parameters :

λ ≡ [q1, q2, q3, q0]T (2.69)

These components of λ are not independent since

‖q‖2 + q20 ≡ sin2 φ+ cos2 φ = 1 (2.70)


Linear Invariants of a Rotation (Cont’d)

⇒ ‖λ‖2 ≡ q21 + q22 + q23 + q20 = 1 (2.71)

sinφ = ±‖q‖, e = q/ sinφ (2.73)

Q =qqT

‖q‖2+ q0

(1− qqT

‖q‖2

)+ Q (2.74a)

Q ≡ ∂(q× x)∂x

Q = q01 + Q +qqT

1 + q0(2.74b)


Linear Invariants of a Rotation

⇒ Linear invariants are not suitable to represent a rotation when φ is eitherπ or close to it.

Changing the sign of e does not change the rotation matrix, provided that thesign of φ is also changed.

Choose 0 ≤ φ ≤ π

⇒ sinφ = ‖q‖, e =q‖q‖


Examples

Example

If [e]F = [√

3/3, −√

3/3,√

3/3 ]T in a given coordinate frame F andφ = 120◦, what is Q in F ?

Solution : From the data,

cosφ = −12, sinφ =

√3

2

[eeT ]F =13

1−11

[ 1 −1 1]

=13

1 −1 1−1 1 −11 −1 1


Examples (Cont’d)

[ 1− eeT ]F =13

2 1 −11 2 1−1 1 2

, [ E ]F ≡√

33

0 −1 −11 0 −11 1 0

[Q]F =

13

1 −1 1−1 1 −11 −1 1

− 16

2 1 −11 2 1−1 1 2

+36

0 −1 −11 0 −11 1 0

[Q]F =

0 −1 00 0 −11 0 0


Examples (Cont’d)

ExampleIs the linear transformation below a rotation ?

[ Q ]F =

0 1 00 0 11 0 0

If so, find its natural invariants.

Solution : Test for orthogonality :

[ Q ]F [ QT ]F =

0 1 00 0 11 0 0

0 0 11 0 00 1 0

=

1 0 00 1 00 0 1


Examples (Cont’d)

det( Q ) = +1 ⇒ a rotation

Find e and φ : vect(Q) = (sinφ)e

⇒ [ q ]F ≡ sinφ [e]F = −12[

1 1 1]T

sinφ ≡ ‖q‖ =√

32

⇒ φ = 60◦ or 120◦

[e]F =[q]F‖q‖

= −√

33[

1 1 1]T

1 + 2 cosφ ≡ tr(Q) = 0, cosφ = −12⇒ φ = 120◦


Examples (Cont’d)

ExampleA coordinate frame X1, Y1, Z1 is rotated into a configuration X2, Y2, Z2 insuch a way that

X2 = −Y1, Y2 = Z1, Z2 = −X1

Find the matrix representation of the rotation in X1, Y1, Z1 coordinates.Compute the direction of the axis and the angle of rotation.

Solution : Let i1, j1, k1 be unit vectors parallel to X1, Y1, Z1, respectively,i2, j2, k2 being defined correspondingly.

⇒ i2 = −j1, j2 = k1, k2 = −i1


Examples (Cont’d)

[ Q ]1 =

0 0 −1−1 0 00 1 0

⇒ [q]1 ≡ [ vect(Q) ]1 = sinφ [ e ]1 = 1

2

1−1−1

cosφ = 1

2 [ tr(Q)− 1 ] = −12

Assume sinφ ≥ 0 ⇒ sinφ = ‖q‖ =√

32

[ e ]1 =[q]1sinφ

=√

33

1−1−1

⇒ φ = 120◦


Examples (Cont’d)

Example

Show that the matrix P in eq. (2.4) satisfies (2.1a).

Solution : Is P idempotent ?

P2 = (1− nnT )(1− nnT )= 1− 2nnT + nnTnnT = 1− nnT = P

Moreover,

Pn = (1− nnT )n = n− nnTn = n− n = 0

⇒ n is eigenvector of P with eigenvalue 0 and, hence, spans the nullspaceof P


Examples (Cont’d)

ExampleThe representations of three linear transformations in a given coordinateframe F :

[A ]F =13

2 1 2−2 2 1−1 −2 2

, [ B ]F =13

2 1 11 2 −11 −1 2

,[ C ]F =

13

1 2 22 1 −22 −2 1

Identify the orthogonal projection, the reflection and the rotation and find itsinvariants.


Examples (Cont’d)

Solution : B and C are symmetric. Rotation matrix is symmetric if and only ifsinφ = 0 i.e., φ = 0 or π, with trace 3 or −1. But

tr(B) = 2, tr(C) = 1

⇒ B and C are not rotations

[ AAT ]F =19

9 0 00 9 00 0 9

, det(A) = +1

⇒ A is a rotation


Examples (Cont’d)

Natural invariants of A :

sinφ [e]F = [vect(A)]F = 12

−11−1

cosφ = 1

2 [tr(A)− 1] = 12(2− 1) = 1

2

For sinφ ≥ 0 sinφ = ‖vect(A)‖ =√

32 ⇒ φ = 60◦

[e]F =[vect(A)]F‖vect(A)‖

=√

33

−11−1


Examples (Cont’d)

Test B and C for orthogonality :

[ BBT ]F =13

2 1 11 2 −11 −1 2

= [ B2 ]F = [ B ]F

⇒ B is not orthogonal but idempotent

[ CCT ]F =19

9 0 00 9 00 0 9

⇒ C is orthogonal


Examples (Cont’d)

det(C) = −1 ⇒ C is a reflection

det(B) = 0 ⇒ B is singular : a projection

The unit vector [ n ]F = [n1, n2, n3 ]T spanning the nullspace of B can bedetermined from :

Bn = 0 ⇒ nnT = 1−B

Produces two solutions, one the negative of the other.


Examples (Cont’d)

nnT =

n21 n1n2 n1n3

n1n2 n22 n2n3

n1n3 n2n3 n23

=13

1 −1 −1−1 1 1−1 1 1

⇒ n = ±√

33

1−1−1

From C = 1− 2nnT follows the same solution for n


Examples (Cont’d)

ExampleThe vector and the trace of a rotation matrix Q in a reference frame F are

[vect(Q)]F =12

−11−1

, tr(Q) = 2

Find the matrix representation of Q in the given coordinate frame and in aframe with its Z-axis parallel to vect(Q).


Examples (Cont’d)

Solution :

Use eq.(2.74a) to determine rotation matrix Q

[ qqT ]F =14

1 −1 1−1 1 −11 −1 1

, ‖q‖2 =34

[ Q ]F =12

0 1 1−1 0 1−1 −1 0

⇒ [ Q ]F =13

2 1 2−2 2 1−1 −2 2


Examples (Cont’d)

Denote by Z a coordinate frame with Z axis parallel to q

⇒ [ q ]Z =√

32

001

, [ qqT ]Z =34

0 0 00 0 00 0 1

[ Q ]Z =√

32

0 −1 01 0 00 0 0

⇒ [ Q ]Z =

1/2 −√

3/2 0√3/2 1/2 00 0 1


Examples (Cont’d)

ExampleA procedure for trajectory planning produced a matrix

[ Q ] =

0.433 −0.500 zx 0.866 −0.433

0.866 y 0.500

representing a rotation for a certain pick-and-place operation where x, y, andz are entries that are unrecognizable due to failures in the printing hardware.Find the missing entries.


Examples (Cont’d)

Solution : Product QTQ must be 1 :

QT Q =

0.437 + z2 0.433(x− z − 1) 0.5(−y + z) + 0.375∗ 0.937 + x2 0.866(x+ y)− 0.216∗ ∗ 1 + y2

Upon equating the diagonal elements to unity :

x = ±0.250, y = 0, z = ±0.750

while the vanishing of the off-diagonal entries leads to :x = 0.250, y = 0, z = −0.750

⇒ det(Q) = +1


Euler-Rodrigues Parameters

r ≡ sin(φ

2

)e, r0 = cos

(φ

2

)(2.75)

Q = (r02 − r · r)1 + 2rrT + 2r0R (2.76)

where, for arbitrary x,

R ≡ ∂(r× x)∂x

φ 6= π : r0 = ±√

1 + q02

, r =q

2r0(2.77)

φ = π : r0 = 0, r = e (2.78)


The Matrix Square Root

The square root for a 3× 3 matrix is a linear combination of 1 (the identitymatrix), the matrix itself and its square, the coefficients being found using theeigenvalues of the matrix (from Cayley-Hamil- ton’s Theorem).Square root of a rotation matrix : a rotation through angle φ/2 about axisparallel to e.


The Matrix Square Root (Cont’d)

A diagonalizable n× n matrix admits as many square roots as there arecombinations of the two possible roots of individual eigenvalues. Example :the eight square roots of the 3× 3 identity matrix :1 0 0

0 1 00 0 1

,1 0 0

0 1 00 0 −1

,1 0 0

0 −1 00 0 1

,−1 0 0

0 1 00 0 1

1 0 0

0 −1 00 0 −1

,−1 0 0

0 1 00 0 −1

,−1 0 0

0 −1 00 0 1

,−1 0 0

0 −1 00 0 −1


The Matrix Square Root (Cont’d)

Not all 23 = 8 square roots of a 3× 3 proper orthogonal matrix are properorthogonal but there is one and only one that represents a rotation of φ/2denoted

√Q :

r = vect(√

Q), r0 =tr(√

Q)− 12

(2.79)


Euler-Rodrigues Parameters (Cont’d)

Linear invariants can be derived from the rotation matrix by simple additionsand subtractions, but fail to produce information on the axis of rotation whenφ = π.

Euler-Rodrigues parameters require square roots and entail sign ambiguities,but produce information on the axis of rotation for any value of the rotationangle φ.

Note : Not to be confused with Euler angles, which are not invariant andadmit multiple definitions.

No single set of Euler angles exists for a given rotation ; set depends on howthe rotation is decomposed into three simpler rotations.


Example

ExampleFind the Euler-Rodrigues parameters of the proper orthogonal matrix

Q =13

−1 2 22 −1 22 2 −1

Solution :Q is symmetric ⇒ φ = π. Impossible to determine the direction of theaxis of rotation from the linear invariants. However, note that

eeT =12

(1 + Q)


Example (Cont’d)

e21 e1e2 e1e3e1e2 e22 e2e3e1e3 e2e3 e23

=13

1 1 11 1 11 1 1

⇒ e = ±

√3

3[1 1 1

]Tr = e sin

(φ

2

)= ±√

33

111

, r0 = cos(φ

2

)= 0


Composition of Reflections and Rotations

Let R be a pure reflection & Q a rotation⇒ T = QR and T′ = RQare neither symmetric nor self-inverse, yet both are reflections !

⇒ An improper orthogonal transformation (not symmetric) can always bedecomposed into the product of a rotation and a pure reflection. Thisdecomposition is not unique.

For arbitrary n : R ≡ 1− 2nnT

⇒ Q = TR−1 ≡ TR = T− 2(Tn)nT


Example

ExamplePlace both palms in the diving position. Hold a sheet of paper between them. Thesheet defines a hand plane in each hand , its unit normal, hand normal, pointingoutside of the hand, nR and nL for the right and left hand, respectively.oR and oL : unit vectors pointing in the direction of the finger axes of each of thehands

⇒ nL = −nR, oL = oR

Now, without moving your right hand, let the left hand attain a position whereby theleft-hand normal lies at right angles with the right-hand normal, the palm pointingdownwards and the finger axes of the two hands remaining parallel.Find the representation of the transformation carrying the right hand to the finalconfiguration of the left hand, in terms of the unit vectors nR and oR.


Example (Cont’d)

Solution : Desired transformation :

T = QR

Reflection mapping the right hand into the left hand :

R = 1− 2nRnTRLeft hand rotates from diver position about an axis || oR through an angle of90◦ cw. From eq.(2.48) and above information,

Q = oRoTR + OR

where OR is the cross-product matrix of oR

⇒ T = oRoTR + 2OR − 2(oR × nR)nTR


Coordinate Transformations andHomogeneous Coordinates

Coordinate Transformations Between Frameswith a Common Origin

A = {X, Y, Z} , B = {X , Y, Z},

Q : A → B (2.80)

[ p ]A =[x y z

]T (2.81)

Find [ p ]B in terms of [ p ]A and Q


Coordinate Transformations BetweenFrames with a Common Origin

FIG.: Coordinate transformation : (a) coordinates of point P in the A-frame ; and (b)relative orientation of frame B with respect to A.


Coordinate Transformations BetweenFrames with a Common Origin (Cont’d)

Point P is attached to frame A, while frame A undergoes a rotation Q aboutits origin and is carried to frame B :

π = Qp (2.82)

where π is the position vector of point Π : rotated position of P

[ π ]B =[x y z

]T (2.83)

[ π ]A =[ξ η ζ

]T (2.84)



TheoremThe representations of the position vector π of any point in two frames A andB, denoted by [ π ]A and [ π ]B respectively, are related by

[ π ]A = [ Q ]A[ π ]B (2.85)

Proof :[ π ]A = [ Q ]A[ p ]A (2.86)

[ π ]B = [ p ]A (2.87)

⇒ [ π ]A = [ Q ]A[ π ]B (2.88)



TheoremThe representations of Q carrying A into B in these two frames are identical,i.e.,

[ Q ]A = [ Q ]B (2.89)

Proof :

[ Qp ]A = [ Q ]A[ Qp ]B, [ Q ]A[ p ]A = [ Q ]A[ Qp ]B

⇒ [ p ]A = [ Q ]B[ p ]B (2.90)

[ p ]A = [ Q ]A[ p ]B (2.91)



TheoremThe inverse relation of Theorem 1 is given by

[ π ]B = [ QT ]B[ π ]A (2.92)


Example

Example

FIG.: Coordinate frames A and B with a common origin.

Find the representations of Q rotating A into B in these two frames and showthat they are identical. Moreover, if [ p ]A = [ 1, 1, 1 ]T , find [ p ]B.


Example (Cont’d)

Solution : Let i, j, and k be unit vectors in the directions of the X-, Y -, andZ-axes, respectively,

unit vectors ι, γ, and κ being defined likewise, as parallel to the X -, Y-, andZ-axes of Fig. 2.2

Qi ≡ ι = −k, Qj ≡ γ = −i, Qk ≡ κ = j

[ Q ]A =

0 −1 00 0 1−1 0 0


Example (Cont’d)

Qι =

0 −1 00 0 1−1 0 0

00−1

=

0−10

= −j = −κ

Likewise,Qγ = −ι, Qκ = γ

From Definition 1

[ Q ]B =

0 −1 00 0 1−1 0 0

= [ Q ]A


Example (Cont’d)

If we represent this matrix in a frame whose X-axis is directed along the axisof rotation of Q,

[ Q ]X =

1 0 00 cosφ − sinφ0 sinφ cosφ

with φ = 120◦

⇒ [ Q ]X =

1 0 00 −1/2 −

√3/2

0√

3/2 −1/2

[ p ]B =

0 0 −1−1 0 00 1 0

111

=

−1−11


Coordinate Transformation with Origin Shift

TheoremThe representations of the position vector p of a point P of the Euclidean3-dimensional space in two frames A and B are related by

[ p ]A = [ b ]A + [ Q ]A[ π ]B (2.93a)

[ π ]B = [ QT ]B([−b ]A + [ p ]A) (2.93b)

with b defined as the vector directed from the origin of A to that of B, and πthe vector directed from the origin of B to P , as depicted in Fig. 2.3.


Coordinate Transformation with Origin Shift (Cont’d)

FIG.: Coordinate frames with different origins.


Coordinate Transformation with Origin Shift (Cont’d)

Proof :p = b + π (2.94)

[ p ]A = [ b ]A + [ π ]A

π is assumed to be readily available in B

[ p ]A = [ b ]A + [ Q ]A[ π ]B

To prove eq.(2.93b), we simply solve eq.(2.94) for π and apply eq.(2.92) tothe equation thus resulting.


Example

Example

If [ b ]A = [−1,−1, −1 ]T and A & B have the relative orientations as inExample 4, find the position vector, in B, of a point P of position vector [ p ]Agiven as in the same example.

Solution : Find [ π ]B : [−b ]A + [ p ]A =[2 2 2

]TBy virtue of Theorem 2, Q ]B = [ QT ]A :

[ Q ]B =

0 0 −1−1 0 00 1 0

⇒ [ π ]B =

0 0 −1−1 0 00 1 0

222

=

−2−22


Homogeneous Coordinates

General coordinate transformation involving a shift of the origin is not linear,e.g., the first terms of the RHS of eq.(2.93a) is nonhomogeneous. It can berepresented in homogeneous form by introducing homogeneous coordinates :

{p}M ≡[[ p ]TM 1

]T (2.95)

Then, the affine transformation of eq.(2.93a) becomes {p}A = {T}A{π}Bwith :

{T }A ≡[[ Q ]A [ b ]A0T 1

], (2.97)

{T−1 }B =[[ QT ]B [−b ]B

0T 1

](2.98)


Homogeneous Coordinates (Cont’d)

Fk, for k = i− 1, i, i+ 1 : coordinate frames with origins at Ok

Qi−1 : the rotation of Fi−1 into FiQi : the rotation of Fi into Fi+1

If three origins coincide :

[ p ]i = [ QTi−1 ]i−1[ p ]i−1

[ p ]i+1 = [ QTi ]i[ p ]i = [ QT

i ]i[ QTi−1 ]i−1[ p ]i−1

with inverse relation

[ p ]i−1 = [ Qi−1 ]i−1[ Qi ]i[ p ]i+1



If origins do not coincide, ai−1 =−−−−→Oi−1Oi, ai =

−−−−→OiOi+1

{Ti−1}i−1 =[[ Qi−1 ]i−1 [ ai−1 ]i−1

0T 1

],

{Ti}i =[[ Qi ]i [ ai ]i0T 1

]Inverse transformation :

{Ti−1}−1i =

[[ QT

i−1 ]i [ QTi−1 ]i[−ai−1 ]i−1

0T 1

]

{Ti}−1i+1 =

[[ QT

i ]i+1 [QTi ]i+1[−ai ]i

0T 1

]



Therefore, the coordinate transformations are

{pi−1}i−1 = {Ti−1}i−1{pi}i (2.105)

{pi−1}i−1 = {Ti−1}i−1{Ti}i{pi+1}i+1 (2.106)

with the corresponding inverse transformations

{pi }i = {Ti−1}−1i−1{pi−1 }i−1 (2.107)

{pi+1 }i+1 = {Ti}−1i {pi }i = {Ti}−1

i {Ti−1}−1i−1{pi−1 }i−1 (2.108)



If P lies at infinity :

{p}M ≡ ‖p‖[

[ e ]M1/‖p‖

]⇒

lim‖p‖→∞

{p}M =(

lim‖p‖→∞

‖p‖)(

lim‖p‖→∞

[[ e ]M1/‖p‖

])

=(

lim‖p‖→∞

‖p‖)[

[ e ]M0

]



We define the homogeneous coordinates of a point P lying at infinity as

{p∞ }M ≡[[ e ]M

0

](2.109)

Let Q =[e1 e2 e3

]and triad { ek }31 be orthonormal

⇒ {T}A of eq.(2.97) becomes

{T }A =[e1 e2 e3 b0 0 0 1

](2.110)


Example

ExampleFind the equation of an ellipsoid in A with semiaxes of a = 1, b = 2, c = 3and with its centre OB of position [ b ]A = [ 1, 2, 3 ]T , its axes X , Y , Zdefining a coordinate frame B. The direction cosines of X are(0.933, 0.067, −0.354), whereas Y ⊥ b and Y ⊥ u while u||X -axis.

u,v, w are unit vectors parallel to the X -, Y-, and Z-axes, respectively :

[ u ]A =

0.9330.067−0.354

, v =u× b‖u× b‖

, w = u× v


Example (Cont’d)

⇒ [ v ]A =

0.243−0.8430.481

, [ w ]A =

−0.266−0.535−0.803

[ Q ]A = [ u, v, w ]A =

0.933 0.243 −0.2660.067 −0.843 −0.535−0.354 0.481 −0.803


Example (Cont’d)

If [ p ]A = [ p1, p2, p3 ]T and [ π ]B = [π1, π2, π3 ]T ,

B :π2

1

12+π2

2

22+π2

3

32= 1

[ QT ]B = [ QT ]A =

0.933 0.067 −0.3540.243 −0.843 0.481−0.266 −0.535 −0.803

[ π ]B =

0.933 0.067 −0.3540.243 −0.843 0.481−0.266 −0.535 −0.803

−1−2−3

+

p1

p2

p3


Example (Cont’d)

Therefore,

π1 = 0.933p1 + 0.067p2 − 0.354p3 − 0.005π2 = 0.243p1 − 0.843p2 + 0.481p3

π3 = −0.266p1 − 0.535p2 − 0.803p3 + 3.745

Upon substitution of the foregoing relations into the ellipsoid equation in B,

A : 32.1521p12 + 7.70235p2

2 + 9.17286p32 − 8.30524p1

−16.0527p2 − 23.9304p3 + 9.32655p1p2 + 9.02784p2p3

−19.9676p1p3 + 20.101 = 0


Similarity Transformations

v = α1a1 + α2a2 + · · ·+ αnanv = β1b1 + β2b2 + · · ·+ βnbn

[ v ]A =

α1

α2...αn

, [ v ]B =

β1

β2...βn

(2.113)

bj = a1ja1 + a2ja2 + · · ·+ anjan, j = 1, . . . , n


Similarity Transformations (Cont’d)

v = β1(a11a1 + a21a2 + · · ·+ an1an)+β2(a12a1 + a22a2 + · · ·+ an2an)

...

+βn(a1na1 + a2na2 + · · ·+ annan)

v = (a11β1 + a12β2 + · · ·+ a1nβn)a1

+ (a21β1 + a22β2 + · · ·+ a2nβn)a2

...

+ (an1β1 + an2β2 + · · ·+ annβn)an



[ v ]A = [ A ]A[ v ]B (2.117)

where

[ A ]A ≡

a11 a12 · · · a1n

a21 a22 · · · a2n...

.... . .

...an1 an2 · · · ann

(2.118)

Inverse relationship :[ v ]B = [ A−1 ]A[ v ]A (2.119)



[ L ]A =

l11 l12 · · · l1nl21 l22 · · · l2n...

.... . .

...ln1 ln2 · · · lnn

(2.120)

We want to find relation between [L]A and [L]B. To this end, let

Lv = w (2.121)

whose representations in A and B are

[ L ]A[ v ]A = [ w ]A (2.122)

[ L ]B[ v ]B = [ w ]B (2.123)



We assume that the image vector w is identical to vector v in the range of L(not always the case).

[ w ]A = [ A ]A[ w ]B (2.124)

Substitution of eq.(2.124) into eq.(2.122) yields

[ A ]A[ w ]B = [ L ]A[ A ]A[ v ]B (2.125)

⇒ [ w ]B = [ A−1 ]A[ L ]A[ A ]A[ v ]B (2.126)

Hence,[ L ]B = [ A−1 ]A[ L ]A[ A ]A (2.127)

[ L ]A = [ A]A[ L ]B[ A−1 ]A (2.128)

Equations (2.117), (2.119), (2.127) & (2.128) are the similaritytransformations sought. They preserve invariant quantities



TheoremThe characteristic polynomial of a given n× n matrix remains unchangedunder a similarity transformation. Moreover, the eigenvalues of two matrixrepresentations of the same n× n linear transformation are identical, and if[ e ]B is an eigenvector of [ L ]B, then under the similarity transformation(2.128), the corresponding eigenvector of [ L ]A is [ e ]A = [ A ]A[ e ]B.

Proof : The characteristic polynomial of [ L ]B is

P (λ) = det(λ[ 1 ]B − [ L ]B) (2.129)



P (λ) ≡ det(λ[ A−1 ]A[ 1 ]A[ A ]A − [ A−1 ]A[ L ]A[ A ]A)= det([ A−1 ]A(λ[ 1 ]A − [ L ]A)[ A ]A)= det([ A−1 ]A)det(λ[ 1 ]A − [ L ]A)det([ A ]A)

det([ A−1 ]A)det([ A ]A) = 1

The characteristic polynomials of [ L ]A and [ L ]B are identical ⇒ [ L ]A& [ L ]B have same eigenvalues.

Let [ L ]B[ e ]B = λ[ e ]B :

[ A−1 ]A[ L ]A[ A ]A[ e ]B = λ[ e ]B

[ L ]A[ A ]A[ e ]B = λ[ A ]A[ e ]B (2.130)


Similarity Transformations (Cont’d)TheoremIf [ L ]A and [ L ]B are related by the similarity transformation (2.127), then

[ Lk ]B = [ A−1 ]A[ Lk ]A[ A ]A (2.131)

for any integer k.

Proof : For k = 2

[ L2 ]B ≡ [ A−1 ]A[ L ]A[ A ]A[ A−1 ]A[ L ]A[ A ]A= [ A−1 ]A[ L2 ]A[ A ]A

For k = n

[ Ln+1 ]B ≡ [ A−1 ]A[ Ln ]A[ A ]A[ A−1 ]A[ L ]A[ A ]A= [ A−1 ]A[ Ln+1 ]A[ A ]A



TheoremThe trace of an n× n matrix does not change under a similaritytransformation.

Proof : Let [ A ], [ B ] and [ C ] be three different n× n matrix arrays, in agiven reference frameLet aij , bij , and cij be the components of the said arrays, with indices rangingfrom 1 to n

tr([ A ] [ B ] [ C ]) ≡ aijbjkcki = bjkckiaij

≡ tr([ B ] [ C ] [ A ])tr([ L ]B) = tr([ A−1 ]A[ L ]A[ A ]A)

= tr([ A ]A[ A−1 ]A[ L ]A) = tr([ L ]A)


Example

ExampleWe consider the equilateral triangle Fig. 2.4 of side 2, with verticesP1, P2, P3, and coordinate frames A(X ,Y) and B(X ′,Y ′), both with originat the centroid of the triangle. Let P be a 2× 2 matrix :

P =[p1 p2

]with pi being the position vector of Pi in a given coordinate frame. Show thatmatrix P does not obey a similarity transformation upon a change of frame,and compute its trace in frames A and B to make it apparent that this matrixdoes not comply with the conditions of Theorem 3.


Example (Cont’d)

FIG.: Two coordinate frames used to represent the position vectors of the corners ofan equilateral triangle.


Example (Cont’d)

[ P ]A =[

1 0−√

3/3 2√

3/3

], [ P ]B =

[0 1

−2√

3/3√

3/3

]

tr([ P ]A) = 1 + 2√

33 6= tr([ P ]B) =

√3

3

[ pi ]A = [ Q ]A[ pi ]B ⇒ [ P ]A = [ Q ]A[ P ]B

Let R ≡ PPT then

[ R ]A =[

1 −√

3/3−√

3/3 5/3

], [ R ]B =

[1

√3/3√

3/3 5/3

]

⇒ tr([ R ]B) =83

[ R ]A = [ PPT ]A = [ Q ]A[ P ]B([ Q ]A[ P ]B)T = [ Q ]A[ PP ]TB [ QT ]A

which is a similarity transformation


Invariance Concepts

The scalar function f( · ) of the position vector p is

invariant if : f([p]B) = f([p]A)

vector f is invariant if : [f ]A = [Q]A[f ]B

matrix F is invariant if : [F]A = [Q]A[F]B[QT ]A

Moreover,[ a ]TA [ b ]A = [ a ]TB [ b ]B

[ a× b ]A = [ Q ]A [ a× b ]B


Invariance Concepts (Cont’d)

The kth moment of an n× n matrix T :

Ik ≡ tr(Tk), k = 0, 1, . . .with I0 = tr(1) = n.

TheoremThe moments of a n× n matrix are invariant under a similaritytransformation.

Proof :[ Tk ]B = [ A−1 ]A[ Tk ]A[ A ]A (2.140)

[ Ik ]B = tr([A−1

]A

[Tk]A [ A ]A) ≡ tr([ A ]A

[A−1

]A

[Tk]A)

= tr([Tk]A) ≡ [ Ik ]A



TheoremAn n× n matrix has only n linearly independent moments.

Proof : Let the characteristic polynomial of T beP (λ) = a0 + a1λ+ · · ·+ an−1λ

n−1 + λn = 0

Applying the Cayley-Hamilton Theorem :

a01 + a1T + · · ·+ an−1Tn−1 + Tn = 0

Take the trace of both sides of the above equation :

a0I0 + a1I1 + · · ·+ an−1In−1 + In = 0



The vector invariants of an n× n matrix are its eigenvectors.

The eigenvectors of symmetric matrices are real and mutually orthogonal, itseigenvalues being real as well.

This is not so for skew-symmetric matrices but for 3× 3 skew-symmetricmatrices, one eigenvalue is real, namely, 0, and its associated vector is theaxial vector of this matrix.

Two n× n matrices related by a similarity transformation have the same setof moments. However, if two n× n symmetric matrices share their first nmoments { Ik }n−1

0 , they are not necessarily related by a similaritytransformation.



For example :

A =[1 00 1

], B =

[1 22 1

]Both matrices share the two moments : I0 = 2 & I1 = 2. The secondmoments are different :

tr(A2) = 2, tr(B2) = tr[5 44 5

]= 10

⇒ To test whether two different matrices represent the same lineartransformation, we must verify that they share the same set of nmoments { Ik }n1 since all n× n matrices share the same I0 = n.



The foregoing discussion does not apply, in general, to nonsymmetricmatrices : they are not fully characterized by their eigenvalues. For example :

A =[1 10 1

]Moments I0 = 2, I1 = tr(A) = 2 are equal to those of the 2× 2 identitymatrix but the transformations are different.

If two symmetric matrices, A and B, represent the same transformation, theyare related by a similarity transformation :

B = T−1AT


Examples

ExampleFind out whether two symmetric matrices

A =

1 0 10 1 01 0 2

, B =

1 0 00 2 −10 −1 1

are related by a similarity transformation.

tr(A) = tr(B) = 4. What about I2 and I3 ?


Examples (Cont’d)

A2 =

2 0 30 1 03 0 5

, B2 =

1 0 00 5 −30 −3 2

tr(A2) = tr(B2) = 8

A3 =

5 0 80 1 08 0 13

, B3 =

1 0 00 13 −80 −8 5

tr(A3) = tr(B3) = 19


Examples (Cont’d)

ExampleSame as the previous example, for :

A =

1 0 20 1 02 0 0

, B =

1 1 11 1 01 0 0

tr(A) = tr(B) = 2, but tr(A2) = 10 while tr(B2) = 6

Therefore, A and B are not related by a similarity transformation


Applications to Redundant Sensing

If redundant sensors are introduced, and we attach frames A and B to each ofthese, then each sensor can be used to determine the orientation of theend-effect- or with respect to a reference configuration.

For this task it is needed to measure rotation R that each frame underwentfrom the reference configuration.

Assume that the measurements produce the orthogonal matrices A and B,representing R in A and B, respectively. Determine the relative orientation Qof frame B with respect to frame A (instrument calibration).


Applications to Redundant Sensing (Cont’d)

We have : A ≡ [ R ]A and B ≡ [ R ]BNeed to determine [ Q ]A or [ Q ]B, which can be obtained from

A = [ Q ]AB[ QT ]A or A [ Q ]A = [ Q ]AB

This problem can be solved with three invariant vectors associated with A andB.Since A and B are orthogonal, they admit one real invariant vector : their axialvector.



To determine Q, we need two more invariant vectors, represented in A and B.To this end, take two measurements of the orientation of A0 and B0 withrespect to A and B.Let the matrices representing these orientations be given by Ai and Bi, withai and bi being the corresponding axial vectors, for i = 1, 2.

There are two possibilities :

(i) Neither of a1 and a2 and, consequently, neither of b1 and b2, is zero ;

(ii) at least one of a1 and a2, and consequently, the corresponding vector ofthe {b1, b2 } pair, vanishes.



In the first case compute a 3rd vector for each set

a3 = a1 × a2, b3 = b1 × b2 (2.143)

In the second case, two more possibilities : angle of rotation of orthogonalmatrix, A1 or A2, whose axial vector vanishes, is either 0 or π.

If angle is 0, then A, as well as B, underwent a pure translation from A0 andB0, correspondingly. Then, a new measurement is needed, involving arotation.



If the angle is π, then associated rotation is symmetric and the unit vector eparallel to its axis can be determined from eq.(2.49) in both A and B.Then, we end up with two pairs of nonzero vectors {ai }21 and {bi }21.

ai = [ Q ]A bi, for i = 1, 2, 3 (2.144)

E = [ Q ]AF (2.145)

E ≡[a1 a2 a3

], F ≡

[b1 b2 b3

](2.146)

⇒ [ Q ]A = EF−1 (2.147)



F−1 =1∆

(b2 × b3)T

(b3 × b1)T

(b1 × b2)T

, ∆ ≡ b1 × b2 · b3 (2.148)

Therefore,

[ Q ]A =1∆

[a1(b2 × b3)T + a2(b3 × b1)T + a3(b1 × b2)T ]


Example

Example (Hand-Eye Calibration)Determine the relative orientation of a frame B attached to a camera mounted on arobot end-effector, with respect to a frame A fixed to the latter, as shown in Fig. 2.5.It is assumed that two measurements of the orientation of the two frames with respectto frames A0 and B0 in the reference configuration of the end-effector are available.These measurements produce the orientation matrices Ai of the frame fixed to thecamera and Bi of the frame fixed to the end-effector, for i = 1, 2.


Example (Cont’d)

The numerical data :

A1 =

− 0.92592593 −0.37037037 −0.074074070.28148148 −0.80740741 0.51851852−0.25185185 0.45925926 0.85185185

A2 =

−0.83134406 0.02335236 −0.55526725−0.52153607 0.31240270 0.793980280.19200830 0.94969269 −0.24753503

B1 =

−0.90268482 0.10343126 −0.417686590.38511568 0.62720266 −0.676980600.19195318 −0.77195777 −0.60599932

B2 =

−0.73851280 −0.54317226 0.39945305−0.45524951 0.83872293 0.29881721−0.49733966 0.03882952 −0.86668653


Example (Cont’d)

FIG.: Measuring the orientation of a camera-fixed coordinate frame with respect to aframe fixed to a robotic end-effector.


Example (Cont’d)

a1 =

−0.029629630.088888890.32592593

, b1 =

−0.04748859−0.304819890.14084221

,a2 =

0.07784121−0.37363778−0.27244422

, b2 =

−0.129993850.448696360.04396138

a3 =

0.097560970.017302930.00415020

, b3 =

−0.07655343−0.01622096−0.06091842

∆ = 0.00983460


Example (Cont’d)

a1(b2 × b3)T =

0.00078822 0.00033435 −0.00107955−0.00236467 −0.00100306 0.00323866−0.00867044 −0.00367788 0.01187508

a2(b3 × b1)T =

−0.00162359 0.00106467 0.001756800.00779175 −0.00510945 −0.008431020.00568148 −0.00372564 −0.00614762

a3(b1 × b2)T =

−0.00746863 −0.00158253 −0.00594326−0.00132460 −0.00028067 −0.00105407−0.00031771 −0.00006732 −0.00025282

[ Q ]A =

−0.84436553 −0.01865909 −0.535457500.41714750 −0.65007032 −0.63514856−0.33622873 −0.75964911 0.55667078


Fundamentals of Robotic Mechanical Systemsrmsl/Index/Documents/ch2_slds_En.pdfFundamentals of...

Documents

Transcript of Fundamentals of Robotic Mechanical Systemsrmsl/Index/Documents/ch2_slds_En.pdfFundamentals of...