Rotation Matrices and Quaternion Siamak Faal 1/2 04/20/2016

Orientation and Rotation Move and Rotate The object is now at a new position and orientation.

Orientation and Rotation Rotation Matrices Quaternion

Preliminaries and notations Unit vectors: 𝑖 = 1 0 0 𝑇 ; 𝑗 = 0 1 0 𝑇 ; 𝑘 = 0 0 1 𝑇 Transpose: 𝐴 𝑇 𝑖𝑗 = 𝐴 𝑗𝑖 Inner product: 𝑥,𝑦 =𝑥⋅𝑦= 𝑥 𝑇 𝑦 Notation simplifications: 𝑠 𝜃 = sin 𝜃 𝑐 𝜃 = cos 𝜃 𝑠 12 = sin 𝜃 1 + 𝜃 2 𝑐 12 = cos 𝜃 1 + 𝜃 2

Reference Frames z y z x x y p y y {2} y {1} x x {0} x A point p can have different coordinates in different reference frames. Frames can be fixed in space or attached to moving objects (inertial or non-inertial frames) x y z p x y {2} x y {1} x y {0}

Rotation Matrix A 2D example 2 𝑝 = 2 𝑥 𝑝 ∙ 2 𝑖 + 2 𝑦 𝑝 ∙ 2 𝑗 2 𝑝 = 2 𝑥 𝑝 ∙ 2 𝑖 + 2 𝑦 𝑝 ∙ 2 𝑗 1 𝑥 𝑝 = 2 𝑥 𝑝 2 𝑖 , 1 𝑖 + 2 𝑦 𝑝 2 𝑗 , 1 𝑖 1 𝑦 𝑝 = 2 𝑥 𝑝 2 𝑖 , 1 𝑗 + 2 𝑦 𝑝 2 𝑗 , 1 𝑗 𝑎,𝑏 = 𝑎 𝑏 cos 𝜃 𝑖 = 𝑗 =1 1 𝑥 𝑝 1 𝑦 𝑝 = cos 𝜃 cos 𝜋 2 +𝜃 cos 𝜋 2 −𝜃 cos 𝜃 2 𝑥 𝑝 2 𝑦 𝑝 1 𝑥 𝑝 1 𝑦 𝑝 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 2 𝑥 𝑝 2 𝑦 𝑝 A 2D example y y x p θ {2} x {1}

Rotation Matrix A 2D example 1 𝑥 𝑝 1 𝑦 𝑝 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 2 𝑥 𝑝 2 𝑦 𝑝 1 𝑝 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 2 𝑝 1 𝑝 = 2 1 𝑅 2 𝑝 y y x p The 𝑅 matrix is both and operator and a description! θ {2} x {1}

Rotation Matrix (Direction Cosines) 𝐵 𝐴 𝑅 = 𝐴 𝑖 𝐵 𝐴 𝑗 𝐵 𝐴 𝑘 𝐵 𝐵 𝐴 𝑅 = 𝐵 𝑖 ∙ 𝐴 𝑖 𝐵 𝑗 ∙ 𝐴 𝑖 𝐵 𝑘 ∙ 𝐴 𝑖 𝐵 𝑖 ∙ 𝐴 𝑗 𝐵 𝑗 ∙ 𝐴 𝑗 𝐵 𝑘 ∙ 𝐴 𝑗 𝐵 𝑖 ∙ 𝐴 𝑘 𝐵 𝑗 ∙ 𝐴 𝑘 𝐵 𝑘 ∙ 𝐴 𝑘

Standard Axis Rotations 𝑅 𝑥 𝜃 = 1 0 0 0 cos 𝜃 − sin 𝜃 0 sin 𝜃 cos 𝜃 𝑅 𝑦 𝜃 = cos 𝜃 0 sin 𝜃 0 1 0 − sin 𝜃 0 cos 𝜃 𝑅 𝑧 𝜃 = cos 𝜃 − sin 𝜃 0 sin 𝜃 cos 𝜃 0 0 0 1

Rotation Matrix Identities Claim: Rotation matrix from A to B is equal to the transpose of a rotation matrix from B to A 𝐵 𝐴 𝑅 = 𝐴 𝐵 𝑅 𝑇 Proof: 𝐴 𝐵 𝑅 = 𝐴 𝑖 ∙ 𝐵 𝑖 𝐴 𝑗 ∙ 𝐵 𝑖 𝐴 𝑘 ∙ 𝐵 𝑖 𝐴 𝑖 ∙ 𝐵 𝑗 𝐴 𝑗 ∙ 𝐵 𝑗 𝐴 𝑘 ∙ 𝐵 𝑗 𝐴 𝑖 ∙ 𝐵 𝑘 𝐴 𝑗 ∙ 𝐵 𝑘 𝐴 𝑘 ∙ 𝐵 𝑘 ⇒ 𝐴 𝐵 𝑅 𝑇 = 𝐵 𝑖 ∙ 𝐴 𝑖 𝐵 𝑗 ∙ 𝐴 𝑖 𝐵 𝑘 ∙ 𝐴 𝑖 𝐵 𝑖 ∙ 𝐴 𝑗 𝐵 𝑗 ∙ 𝐴 𝑗 𝐵 𝑘 ∙ 𝐴 𝑗 𝐵 𝑖 ∙ 𝐴 𝑘 𝐵 𝑗 ∙ 𝐴 𝑘 𝐵 𝑘 ∙ 𝐴 𝑘 = 𝐵 𝐴 𝑅

Rotation Matrix Identities Claim: The transpose of a rotation matrix is equal to its inverse 𝐵 𝐴 𝑅 = 𝐴 𝐵 𝑅 −1 = 𝐴 𝐵 𝑅 𝑇 Proof: 𝐵 𝐴 𝑅 𝑇 𝐵 𝐴 𝑅 = 𝐴 𝑖 𝐵 𝑇 𝐴 𝑗 𝐵 𝑇 𝐴 𝑘 𝐵 𝑇 𝐴 𝑖 𝐵 𝐴 𝑗 𝐵 𝐴 𝑘 𝐵 = 𝐼 3 where 𝐼 3 is the 3×3 identity matrix. Hence, 𝐴 𝐵 𝑅 −1 = 𝐴 𝐵 𝑅 𝑇

Rotation Matrix Identities Claim: The determinant of a rotation matrix is 1 det 𝑅 =1 Proof: 𝑅 𝑇 𝑅=𝐼 det 𝐴 = det 𝐴 𝑇 det 𝑅 2 = det 𝐼 =±1 Rotation matrix preserves dimensions, hence: 𝑅𝑣 = 𝑣 𝑅𝑣 ≤ 𝜆 𝑚𝑎𝑥 𝑅 𝑣 ⇒ 𝜆 𝑚𝑎𝑥 𝑅 = +1 𝜆 𝑚𝑖𝑛 𝑅 𝑣 2 2 ≤ 𝑅𝑣,𝑣 ⇒ 𝜆 𝑚𝑖𝑛 𝑅 𝑣 𝑇 𝑣≤ 𝑣 𝑇 𝑅 𝑇 𝑣 Using length perversity of 𝑅 implies: 𝑣 𝑇 𝑣= 𝑅𝑣,𝑣 ; thus: 𝜆 𝑚𝑖𝑛 𝑅 =1 Implies that all the eigenvalues of 𝑅 are 1, consequently: det 𝑅 = 𝜆 1 𝜆 2 𝜆 3 =+1

Rotation Matrix Identities Claim: Multiplication of two rotation matrices is another rotation matrix 𝑅= 𝑅 1 𝑅 2 Proof: Using 𝑅 𝑇 = 𝑅 −1 and consequently 𝑅 𝑇 𝑅=𝐼: 𝑅 1 𝑅 2 𝑇 𝑅 1 𝑅 2 = 𝑅 2 𝑇 𝑅 1 𝑇 𝑅 1 𝑅 2 =𝐼 Also: det 𝑅 1 𝑅 2 = det 𝑅 1 det 𝑅 2 =+1

Rotation Matrix Identities det 𝑅 =1 𝐵 𝐴 𝑅 = 𝐴 𝐵 𝑅 𝑇 𝑅 𝑇 = 𝑅 −1 𝑅 3 = 𝑅 1 𝑅 2 𝑅 1 𝑅 2 ≠ 𝑅 2 𝑅 1 ∀𝑛>2 n is space dimension

Intrinsic vs. Extrinsic Rotations Intrinsic rotations Extrinsic rotations

Euler Angles Leonhard Euler April 15, 1707, Basel, Switzerland September 18, 1783, Saint Petersburg, Russia

Intrinsic rotations (Euler) Rotations about “new” axes z z' z' z'' 𝜓 z'' 𝜃 zf yf y'' y' y' y'' y x' x' x 𝜙 xf x'' x'' For intrinsic rotations, rotation matrices are post-multiplied: 𝑅= 𝑅 𝑧 𝜓 𝑅 𝑦 𝜃 𝑅 𝑥 𝜙

Extrinsic rotations Rotations about “fixed” axes z z z yf 𝜓 zf y y xf 𝜙 𝜃 For extrinsic rotations, rotation matrices are pre-multiplied: 𝑅= 𝑅 𝑥 𝜙 𝑅 𝑦 𝜃 𝑅 𝑧 𝜓

More on Rotation Matrix 𝑅 −1 = 𝑅 𝑇 ⇒𝑅 is an orthogonal matrix, thus for: 𝑅= 𝑥 𝑦 𝑧 The following statements hold: 𝑥 ⋅ 𝑦 = 𝑥 ⋅ 𝑧 = 𝑦 ⋅ 𝑧 =0 𝑥 ⋅ 𝑥 = 𝑦 ⋅ 𝑦 = 𝑧 ⋅ 𝑧 =1 This implies only 3 of the 9 components of 𝑅 are independent! Consequently, any orientation is achievable through 3 successive rotations about linearly independent axes.

Euler Angles Proper Euler angles: z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y Tait-Bryan angles: x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z

z-y-x Euler Angles 𝐵 𝐴 𝑅 𝑧𝑦𝑥 = 𝑅 𝑧 𝜓 𝑅 𝑦 𝜃 𝑅 𝑥 𝜙 𝐵 𝐴 𝑅 𝑧𝑦𝑥 = 𝑐 𝜓 − 𝑠 𝜓 0 𝑠 𝜓 𝑐 𝜓 0 0 0 1 𝑐 𝜃 0 𝑠 𝜃 0 1 0 − 𝑠 𝜃 0 𝑐 𝜃 1 0 0 0 𝑐 𝜙 − 𝑠 𝜙 0 𝑠 𝜙 𝑐 𝜙 𝐵 𝐴 𝑅 𝑧𝑦𝑥 = 𝑐 𝜓 𝑐 𝜃 𝑐 𝜓 𝑠 𝜃 𝑠 𝜙 − 𝑠 𝜓 𝑐 𝜙 𝑐 𝜓 𝑠 𝜃 𝑐 𝜙 + 𝑠 𝜓 𝑠 𝜙 𝑠 𝜓 𝑐 𝜃 𝑠 𝜓 𝑠 𝜃 𝑠 𝜙 + 𝑐 𝜓 𝑐 𝜙 𝑠 𝜓 𝑠 𝜃 𝑐 𝜙 − 𝑐 𝜓 𝑠 𝜙 − 𝑠 𝜃 𝑐 𝜃 𝑠 𝜙 𝑐 𝜃 𝑐 𝜙

z-y-x Euler Angles 𝐵 𝐴 𝑅 𝑧𝑦𝑥 = 𝑐 𝜓 𝑐 𝜃 𝑐 𝜓 𝑠 𝜃 𝑠 𝜙 − 𝑠 𝜓 𝑐 𝜙 𝑐 𝜓 𝑠 𝜃 𝑐 𝜙 + 𝑠 𝜓 𝑠 𝜙 𝑠 𝜓 𝑐 𝜃 𝑠 𝜓 𝑠 𝜃 𝑠 𝜙 + 𝑐 𝜓 𝑐 𝜙 𝑠 𝜓 𝑠 𝜃 𝑐 𝜙 − 𝑐 𝜓 𝑠 𝜙 − 𝑠 𝜃 𝑐 𝜃 𝑠 𝜙 𝑐 𝜃 𝑐 𝜙 𝐵 𝐴 𝑅 = 𝑟 𝑖𝑗 if 𝑐 𝜃 ≠0 𝜃=atan2 − 𝑟 31 , 𝑟 11 2 + 𝑟 21 2 𝜓=atan2 𝑟 21 𝑐 𝜃 , 𝑟 11 𝑐 𝜃 𝜙=atan2 𝑟 32 𝑐 𝜃 , 𝑟 33 𝑐 𝜃 if 𝜃=90° 𝜓=0 𝜙=atan2 𝑟 12 , 𝑟 22 if 𝜃=−90° 𝜓=0 𝜙=−atan2 𝑟 12 , 𝑟 22

Rotations in Constrained Objects Exercise: find 2 0 𝑅 1 0 𝑅 = 𝑐 𝜃 1 − 𝑠 𝜃 1 0 𝑠 𝜃 1 𝑐 𝜃 1 0 0 0 1 2 1 𝑅 = 𝑐 𝜃 2 − 𝑠 𝜃 2 0 𝑠 𝜃 2 𝑐 𝜃 2 0 0 0 1 2 0 𝑅 = 1 0 𝑅 2 1 𝑅 2 0 𝑅 = 𝑐 12 − 𝑠 12 0 𝑠 12 𝑐 12 0 0 0 1 y x 𝜃 2 y {2} y x 𝜃 1 {1} x {0}

Rotations in Constrained Objects Example: 1 0 𝑅 = 𝑅 𝑧 𝜃 1 45° 2 1 𝑅 = 𝑅 𝑥 𝜃 2 {2} x y z y z {3} x y {4} x z 𝜃 3 3 2 𝑅 = 𝑅 𝑧 45° 𝜃 2 4 3 𝑅 = 𝑅 𝑥 𝜃 3 {1} x y z 4 2 𝑅 = 3 2 𝑅 4 3 𝑅 4 1 𝑅 = 2 1 𝑅 4 2 𝑅 = 2 1 𝑅 3 2 𝑅 4 3 𝑅 x y z {0} 𝜃 1 4 0 𝑅 = 1 0 𝑅 4 1 𝑅 = 1 0 𝑅 2 1 𝑅 3 2 𝑅 4 3 𝑅

Roll-Picth-Yaw x-y-z or z-y-x Euler Angles 𝑅 1 = 𝑅 𝑥 𝜙 𝑅 𝑦 𝜃 𝑅 𝑧 𝜓 𝑅 2 = 𝑅 𝑥 𝜙 𝑅 𝑦 𝜃 𝑅 𝑧 𝜓 x y z 𝜙 𝜓 𝜃

Rotations in free bodies Question: If we assume any type of Euler angles, how can we guarantee that rotations are preserved? z 𝜔= 𝜔 𝑥 𝑖 + 𝜔 𝑦 𝑗 + 𝜔 𝑘 𝑘 𝜔 𝑧 y 𝜔 𝑦 𝜔 𝑥 Answer: We can not! x

Rotations in free bodies 𝜔 x y z x' y' z' z'' y'' x'' xf yf zf 𝜓 𝜃 𝜙 𝜓 𝜃 𝜙 𝜔= 𝜙 𝑖 + 𝑅 𝑥 𝜙 𝑇 𝜃 𝑗 + 𝑅 𝑦 𝜃 𝑅 𝑥 𝜙 𝑇 𝜓 𝑘

Singularity! Singularities at: 𝜔= 𝜙 𝑖 + 𝑅 𝑥 𝜙 𝑇 𝜃 𝑗 + 𝑅 𝑦 𝜃 𝑅 𝑥 𝜙 𝑇 𝜓 𝑘 𝜔 𝑥 = 𝜙 − 𝜓 sin 𝜃 𝜔 𝑦 = 𝜓 cos 𝜃 sin 𝜙 + 𝜃 cos 𝜙 𝜔 𝑧 = 𝜓 cos 𝜃 cos 𝜙 − 𝜃 sin 𝜙 𝜙 = 𝜔 𝑥 + 𝜔 𝑦 sin 𝜙 tan 𝜃 + 𝜔 𝑧 cos 𝜙 tan 𝜃 𝜃 = 𝜔 𝑦 cos 𝜙 − 𝜔 𝑧 sin 𝜙 𝜓 = 𝜔 𝑦 sin 𝜙 + 𝜔 𝑧 cos 𝜙 / cos 𝜃 𝜃= (2𝑘−1)𝜋 2 Singularities at:

Time derivative of a Rotation Matrix y z 𝜔 𝑥 𝜔 𝑦 𝜔 𝑧 For angular velocity vector 𝜔 defined in body frame: 𝜔= 𝜔 𝑥 𝑖 + 𝜔 𝑦 𝑗 + 𝜔 𝑘 𝑘 And a rotation matrix 1 0 𝑅 that maps {1} into {0}. The time derivative of the rotation matrix is defined as: 𝑑 𝑑𝑡 𝑅 𝑡 =𝑆 𝜔 𝑅 where: 𝑆 𝜔 = 0 − 𝜔 𝑧 𝜔 𝑦 𝜔 𝑧 0 − 𝜔 𝑥 − 𝜔 𝑦 𝜔 𝑥 0 {1} {0} x y z

Quaternion Sir William Rowan Hamilton August 4, 1805, Dublin, Republic of Ireland September 2, 1865, Dublin, Republic of Ireland

Quaternion Broom Bridge Dublin, Irland 𝑖 2 + 𝑗 2 + 𝑘 2 =𝑖𝑗𝑘=−1

End of Session 1 of 2 To be continued…

References: A good reference on Rotation matrices and some of the underlying theories is presented in chapter 2 of: Craig, John J. “Introduction to robotics: mechanics and control.” Vol. 3. Upper Saddle River: Pearson Prentice Hall, 2005. Proof of the time derivative of a rotation matrix: Hamano, Fumio. “Derivative of Rotation Matrix Direct Matrix Derivation of Well Known Formula.” arXiv preprint arXiv:1311.6010 (2013).