More Rigid Transforms, 3D Rotations ECE 383/ ME 442 More Rigid Transforms, 3D Rotations 1

Agenda Working with transforms 3D rotations and representations

Operations on Transforms Application Composition Inversion Interpolation Differentiation Accomplish: changes of coordinate frame Accomplish: movement

Composing 2D transformations Using the representation q=( 𝑡 ,𝜃) with 𝑡 =( 𝑡 𝑥 , 𝑡 𝑦 ) 𝑇 𝑞 𝑥 =𝑅 𝜃 𝑥 + 𝑡 Suppose this is followed by another transformation 𝑇 𝑞 ′ with 𝑞 ′ = 𝑡 ′ , 𝜃 ′ . Then… 𝑇 𝑞 ′ 𝑇 𝑞 𝑥 =𝑅 𝜃 ′ 𝑅 𝜃 𝑥 + 𝑡 + 𝑡 ′ =𝑅 𝜃 ′ 𝑅 𝜃 𝑥 +𝑅 𝜃 ′ 𝑡 + 𝑡 ′ Which is equivalent to the transform 𝑇 𝑞′′ represented by 𝑞 ′′ = 𝑅 𝜃 ′ 𝑡 + 𝑡 ′ ,𝜃+ 𝜃 ′

Rigid Transformation in 3D q = (R,tx,ty,tz) with R a 3x3 rotation matrix Robot R0R3 given in reference frame T0 What’s the new robot Rq? {Tq(x,y,z) | (x,y,z)  R0} Define rigid transformation Tq(x,y,z) : R3 R3 R x y z tx ty tz Tq(x,y,z) = + 3D rotation matrix Affine translation 5

3-D Rigid Rotations r11 r12 r13 r21 r22 r23 r31 r32 r33 det(A) = +1 Orthogonal rows and columns: ATA=I, AAT=I ||Ax|| = ||x|| for any x, L2 norm ||.|| Product of two rotations is a rotation (0,1,0) (r11,r21,r31) (Right-handed coordinate system) (r13,r23,r33) (1,0,0) (1,0,0) (r12,r22,r32) (0,0,1) (0,0,1) 6

Coordinate Frames R R-1 = RT Local frame World frame 𝑟 13 𝑟 23 𝑟 33 𝑟 13 𝑟 23 𝑟 33 (0,1,0) R 𝑥 𝑦 𝑧 𝑟 11 𝑟 21 𝑟 31 (1,0,0) R 𝑥 𝑦 𝑧 𝑟 12 𝑟 22 𝑟 32 (0,0,1) R-1 = RT

Composing 3D transformations Using the representation q=(𝑅, 𝑡 ) with 𝑡 =( 𝑡 𝑥 , 𝑡 𝑦 , 𝑡 𝑧 ) 𝑇 𝑞 𝑥 =𝑅⋅ 𝑥 + 𝑡 Suppose this is followed by another transformation 𝑇 𝑞 ′ with 𝑞 ′ =(𝑅′, 𝑡 ′). Then… 𝑇 𝑞 ′ 𝑇 𝑞 𝑥 = 𝑅 ′ ⋅ 𝑅⋅ 𝑥 + 𝑡 + 𝑡 ′ = 𝑅 ′ ⋅𝑅 𝑥 + 𝑅 ′ ⋅ 𝑡 + 𝑡 ′ Which is equivalent to the transform 𝑇 𝑞′′ where 𝑞 ′′ = 𝑅 ′ ⋅𝑅, 𝑅 ′ ⋅ 𝑡 + 𝑡 ′

Interpolating 2D rotations Say we want to blend smoothly from 𝑅 𝜃 to 𝑅 𝜃 ′ with a parameter 𝑢∈[0,1] (𝑢=0 gives 𝑅 𝜃 , 𝑢=1 gives 𝑅(𝜃’)) Blending matrices doesn’t work: (1−𝑢)𝑅(𝜃)+𝑢 𝑅(𝜃’) isn’t in general a rotation matrix Blending angles 𝑅 1−𝑢 𝜃+𝑢 𝜃 ′ … sort of works. What about the boundary at 0 and 2𝜋? Naïve blend 𝜃 Geodesic blend: pick minimum of CW and CCW displacement 𝜃 𝜃′ 𝜃′

Topological spaces Three representations of 2D rotations: Representation 1: Angle 𝜃∈[0,2𝜋) Representation 2: Matrix 𝑅= 𝑟 11 𝑟 12 𝑟 21 𝑟 22 with constraints 𝑅 𝑇 𝑅=𝐼 (in other words, 𝑟 11 2 + 𝑟 12 2 = 𝑟 21 2 + 𝑟 22 2 =1 and 𝑟 11 𝑟 21 + 𝑟 12 𝑟 22 =0) det(R)=+1 Representation 3: unit vector 𝑥 = 𝑥 1 , 𝑥 2 𝑇 with 𝑥 =1 And a way to convert between them: 𝑅 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝜃 𝑅 =atan2( 𝑟 21 , 𝑟 11 ) (here atan2 gives a range of [0,2𝜋) ) 𝑥 𝜃 = cos 𝜃 , sin 𝜃 𝑇 , 𝑥 𝑅 = 𝑟 11 , 𝑟 21 𝑇 , 𝑅 𝑥 = 𝑥 1 − 𝑥 2 𝑥 2 𝑥 1 But rotations don’t “blend” in the way they would on either 0,2𝜋 , 𝑅 2 , or in 𝑀 2,2 … Fundamentally different topology

Some Important Topological Spaces R: real number line Rn: N-dimensional Cartesian space S1: boundary of circle in 2D S2: surface of sphere in 3D SO(2), SO(3): set of 2D, 3D orientations (special orthogonal group) SE(2), SE(3): set of rigid 2D, 3D transformations (special Euclidean group) Cartesian product A x B, power notation An = A x A … x A Homeomorphism ~ denotes topological equivalence Continuous mapping with continuous inverse (bijective) Cube ~ S2 SO(2) ~ S1 SE(3) ~ SO(3) x R3

Topology of SO(3) r11 r12 r13 r21 r22 r23 r31 r32 r33 9 entries, constrained by: Orthogonal rows and columns: ATA=I, AAT=I det(A) = +1 (+/- 1 is implied by the first condition) Six (Row-wise) orthogonality constraints: 1 = 𝑟 11 2 + 𝑟 12 2 + 𝑟 13 2 1 = 𝑟 21 2 + 𝑟 22 2 + 𝑟 23 2 1 = 𝑟 31 2 + 𝑟 32 2 + 𝑟 33 2 0 = 𝑟 11 𝑟 21 + 𝑟 12 𝑟 22 + 𝑟 13 𝑟 23 0 = 𝑟 11 𝑟 31 + 𝑟 12 𝑟 32 + 𝑟 13 𝑟 33 0 = 𝑟 21 𝑟 31 + 𝑟 22 𝑟 32 + 𝑟 23 𝑟 33 9 variables - 6 non-redundant constraints => SO(3) has 3 degrees of freedom 12

4 representations of 3D rotations 3x3 rotation matrix R Euler angles REuler(f,q,y) Axis angle RAA(v,q) Quaternion RQuat(q) All representations are “equivalent” but may have certain mathematical or computational advantages

Axis-aligned rotations Rotate about: Z axis Y axis X axis cos θ -sin θ 0 sin θ cos θ 0 0 0 1 cos θ 0 sin θ 0 1 0 -sin θ 0 cos θ 1 0 0 0 cos θ -sin θ 0 sin θ cos θ 14

Parameterization of SO(3) Euler angles: (f,q,y) x y z q x y z x y z f x y z 1  2  3  4 15

Euler Angles Which axes to pick, and what order? Convention A,B,C REuler(f,q,y) = RA(f)RB(q)RC(y) E.g., ZYZ, ZYX (roll-pitch-yaw), etc 16

Euler Angles Which axes to pick, and what order? Disadvantages Convention A,B,C REuler(f,q,y) = RA(f)RB(q)RC(y) E.g., ZYZ, ZYX (roll-pitch-yaw), etc Disadvantages Must constrain to range of values Singularities, e.g. ZYZ when q=0 or p (Gimbal lock) Interpolation? 17

Axis-Angle Representation Every rotation matrix R can be obtained by rotating the identity matrix by some angle θ about some axis v! R v = v

Axis-Angle Representation Axis v (||v||=1), angle θ Rodrigues’ formula: rotate x about v -> x’ x’ = x cos θ + (v x x) sin θ + v (vT x) (1 - cos θ) Or in matrix form… RAA(θ,v) = cos θ I + sin θ [v] + (1 - cos θ) v vT Cross product matrix 0 -vz vy vz 0 -vx -vy vx 0

Recovering Axis and Angle from the Rotation Matrix θ = Angle(R) = cos-1((r11+r22+r33-1)/2) = cos-1((tr(R)-1)/2) v = Axis(R) = 1/(2 sin θ) r32-r23 r13-r31 r21-r12

Properties of Axis-Angle Disadvantages: Non unique: RAA(θ,v)=RAA(-θ,-v) (can constrain θ to range [0,p]) 4 parameters + one unit length constraint ||v||=1; dealing with this constraint is sometimes annoying, for example, in interpolation, optimization, or sampling Unique encoding: vector w = θv θ = ||w||, v = w/||w|| “Exponential map” REM(w) = RAA( ||w|| , w/||w|| )

Quaternion representation Generalization of complex numbers Complex z=z0+i z1, with |z|=1 can represent a 2D rotation. What’s the 3D analogue? q = q0+q1i + q2j +q3k, where i2 = j2 = k2 = -1 i = jk = -kj j = ki = -ik k = ij = -ji Quaternions were a forerunner of vectors and were once mandatory of all students of physics and math!

Unit quaternion representation q = (q0,q1,q2,q3), ||q||=1 Related to axis angle: q = (cos q/2,vx sin q/2, vy sin q/2, vz sin q/2) 2(q02+q12)-1 2(q1q2-q0q3) 2(q1q3+q0q2) 2(q1q2+q0q3) 2(q02+q22)-1 2(q2q3-q0q1) 2(q1q3-q0q2) 2(q2q3+q0q1) 2(q02+q32)-1 RQ(q) =

Properties of Unit Quaternions 4-sphere: 3D manifold in 4D space Non-unique: Double cover of SO(3) Advantages (widely used in computer animation): Quaternion multiplication = rotation composition (slightly faster than matrix *) Curve interpolation formulas (Shoemake, ‘85)

Summary Rotation matrix Euler angles Axis-angle 9 parameters rij in range [-1,1] Constraint: determinant +1 Advantages: composition, inversion are easy Euler angles 3 parameters (f,q,y) in range [0,2p) Axes picked by convention (e.g., Roll-Pitch-Yaw) Advantages: no constraints, simple interpolation “works” Disadvantages: multiple representation, singularities, composition & inversion not easy Axis-angle 4 parameters (q,v), q in [0,p] Constraint: |v|=1 Advantages: intuitive, inversion is easy Disadvantages: constraint, composition & interpolation not easy , multiple representation at p Moment representation (aka exponential map) 3 parameters w (= q*v), ||w||<=p Advantages: no constraints, inversion easy, relation to angular velocity (as we shall see soon) Disadvantages: composition & interpolation not easy, multiple representation at p Quaternion 4 parameters q=(q0,q1,q2,q3) Constraint: |q|=1 Advantages: composition & interpolation formulas, inversion easy Disadvantages: constraint, multiple representation R(q) = R(-q)

Notion of Distance θ(R1TR2) = cos-1 ((tr(R1TR2)-1)/2) measures the minimum angle needed to rotate from frame R1 to R2 Makes a good distance metric

Actually a geodesic in SO(3)! Rotation in Motion Interpolating between two rotation matrices A and B We want a path X(s) : [0,1] -> SO(3) with R(0) = A and R(1) = B Let v = Axis(ATB), θ = Angle(ATB) Verify that X(s) = A RAA(sθ,v) satisfies the desired properties Actually a geodesic in SO(3)!

Differentiating 2D rotations 𝑅 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝑅’ 𝜃 = − sin 𝜃 − cos 𝜃 cos 𝜃 −sin 𝜃 Interpretation: Suppose the angle moves as a function of time 𝜃 𝑡 , and we wish to observe the movement of a point 𝑦 𝑡 =𝑅 𝜃(𝑡) 𝑥 where the original coordinates 𝑥 are not time-varying By the chain rule, 𝑦 ′ 𝑡 = 𝜃 ′ (𝑡) 𝑑 𝑑𝜃 𝑅 𝜃 𝑡 ⋅ 𝑥 (Convince yourself by writing out the components of 𝑦(𝑡)) So, the product 𝑅 ′ 𝜃 ⋅ 𝑥 gives the velocity the point 𝑦 would rotate if moved at a constant velocity 𝑅 ′ 𝜃 ⋅ 𝑥 is perpendicular to 𝑦 and of the same magnitude

Angular Velocities in 2D For a rotation trajectory 𝑅 𝜃 𝑡 , we can show: 𝑑 𝑑𝑡 𝑅 𝜃 𝑡 = 𝑑 𝑑𝑡 cos 𝜃 𝑡 − sin 𝜃 𝑡 sin 𝜃 𝑡 cos 𝜃 𝑡 = − sin 𝜃 𝑡 𝜃′(𝑡) − cos 𝜃 𝑡 𝜃′(𝑡) cos 𝜃 𝑡 𝜃′(𝑡) − sin 𝜃 𝑡 𝜃′(𝑡) = 𝜃 ′ 𝑡 0 −1 1 0 𝑅 𝜃 𝑡 => 𝜃 ′ 𝑡 is the angular velocity For a point in world coordinates 𝑝 𝑡 =𝑅 𝜃 𝑡 𝑝 0 , we have 𝑝 ′ (𝑡)= 𝜃 ′ 𝑡 0 −1 1 0 𝑝 𝑡

Angular Velocities in 3D For parameterized rotation trajectory REM(wt), we can show: d/dt REM(wt) = [w] REM(wt) => |w| is the speed of rotation [w] is the cross product matrix => w x x is the velocity of some point x, specified in world coordinates, attached to the frame as it rotates => w is the angular velocity

Recap 5 operations on transforms: Four common representations of SO(3) Application to points / directions Composition Inversion Interpolation Differentiation Four common representations of SO(3) All four implemented robustly in Python klampt.so3 module, C++ in KrisLibrary/math3d Notion of distance, geodesic, velocity in SO(3)

Next Week Lab: Intro to Python and Klamp’t