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
Top Related