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

Post on 30-Jun-2020

15 views 0 download

Transcript of 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

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

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

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

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

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

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

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

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

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

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)

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

Linear Transformations (Cont’d)

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

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

Linear Transformations (Cont’d)

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)

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

Linear Transformations (Cont’d)

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 .

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

Linear Transformations (Cont’d)

If Le = λe,

then e : eigenvector of L,

λ : eigenvalue of L

Characteristic polynomial of L :

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

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

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

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

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)

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

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

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

Rigid-Body Rotations (Cont’d)

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.

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

Rigid-Body Rotations (Cont’d)

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)

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

Rigid-Body Rotations (Cont’d)

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.

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

Rigid-Body Rotations (Cont’d)

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

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

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)

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

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)

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

Cross-Product Matrix (Cont’d)

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)

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

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)

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

Cross-Product Matrix (Cont’d)

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.

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

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

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

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

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

Rotation Matrix (Cont’d)

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.

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

Rotation Matrix (Cont’d)

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)

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

Rotation Matrix (Cont’d)

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)

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

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)

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

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 .

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

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)

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

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

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)

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

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

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)

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

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

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.

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

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)

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

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)

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

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)

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

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‖

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

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

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

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

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

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

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

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◦

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

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

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

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◦

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

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

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

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.

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

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

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

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

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

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

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

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.

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

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

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

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

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

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

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

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

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

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.

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

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

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

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)

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

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.

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

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

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

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)

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

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.

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

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)

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

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

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

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

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

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.

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

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

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

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

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

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.

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

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)

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

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

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)

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

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

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)

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

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

TheoremThe inverse relation of Theorem 1 is given by

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

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

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.

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

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

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

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

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

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

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

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.

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

Coordinate Transformation with Origin Shift (Cont’d)

FIG.: Coordinate frames with different origins.

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

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.

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

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

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

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)

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

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

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

Homogeneous Coordinates (Cont’d)

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

]

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

Homogeneous Coordinates (Cont’d)

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)

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

Homogeneous Coordinates (Cont’d)

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

]

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

Homogeneous Coordinates (Cont’d)

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)

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

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

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

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

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

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

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

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

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

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

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

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

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

Similarity Transformations (Cont’d)

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

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

Similarity Transformations (Cont’d)

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

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

Similarity Transformations (Cont’d)

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

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

Similarity Transformations (Cont’d)

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)

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

Similarity Transformations (Cont’d)

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)

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

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

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

Similarity Transformations (Cont’d)

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)

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

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.

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

Example (Cont’d)

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

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

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

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

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

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

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

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

Invariance Concepts (Cont’d)

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

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

Invariance Concepts (Cont’d)

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.

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

Invariance Concepts (Cont’d)

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.

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

Invariance Concepts (Cont’d)

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

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

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 ?

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

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

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

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

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

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

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

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.

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

Applications to Redundant Sensing (Cont’d)

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.

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

Applications to Redundant Sensing (Cont’d)

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.

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

Applications to Redundant Sensing (Cont’d)

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)

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

Applications to Redundant Sensing (Cont’d)

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 ]

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

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.

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

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

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

Example (Cont’d)

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

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

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

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

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

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