CS B659: Principles of Intelligent Robot Motion Rigid Transformations

Agenda Principles, Ch

Rigid Objects

Biological systems, virtual characters

Articulated Robot Robot: usually a rigid articulated structure Geometric CAD models, relative to reference frames A configuration specifies the placement of those frames q1q1q1q1 q2q2q2q2

reference point Rigid Transformation in 2D q = (t x,t y,  ) with   [0,2  ) Robot R 0  R 2 given in reference frame T 0 What’s the new robot R q ? txtx tyty  robot reference direction workspace

Rigid Transformation in 2D q = (t x,t y,  ) with   [0,2  ) Robot R 0  R 2 given in reference frame T 0 What’s the new robot R q ? {T q (x,y) | (x,y)  R 0 } Define rigid transformation T q (x,y) : R 2  R 2 T q (x,y) = cos θ -sin θ sin θ cos θ xyxy txtytxty + 2D rotation matrixAffine translation

2D Rigid Rotations A rotation matrix A has: det(A) = +1 Orthogonal rows and columns: A T A=I, AA T =I ||Ax|| = ||x|| for any x, L 2 norm ||.|| Product of two rotations is a rotation cos θ -sin θ sin θ cos θ θ (cos θ, sin θ) (-sin θ, cos θ) (1,0) (0,1)

Rigid Transformation in 3D q = (t x,t y,t z,R) with R a 3x3 rotation matrix Robot R 0  R 3 given in reference frame T 0 What’s the new robot R q ? R q = {T q (x,y,z) | (x,y,z)  R 0 } Define rigid transformation T q (x,y,z) : R 3  R 3 T q (x,y,z) = R xyzxyz txtytztxtytz + Affine translation3D rotation matrix

3-D Rigid Rotations r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 (1,0,0) (0,1,0) (0,0,1) (Right-handed coordinate system) det(A) = +1 Orthogonal rows and columns: A T A=I, AA T =I ||Ax|| = ||x|| for any x, L 2 norm ||.|| Product of two rotations is a rotation (1,0,0) (r 11,r 21,r 31 ) (0,0,1) (r 13,r 23,r 33 ) (r 12,r 22,r 32 )

Coordinate Frames (1,0,0) (0,1,0) (0,0,1) (r 11,r 21,r 31 ) (r 13,r 23,r 33 ) (r 12,r 22,r 32 ) R R -1 = R T Local frame World frame [x,y,z] T R[x,y,z] T

3 representations Euler angles R Euler (  Axis angle R AA (v,  ) Quaternion R Quat (q) All representations are “equivalent” but may have certain mathematical or computational advantages

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

Parameterization of SO(3)  Euler angles: (  x yz x yz x y z  x yz 1  2  3  4

Euler Angles  Which axes to pick, and what order?  Convention A,B,C  R Euler (  ) = R A (  )R B (  )R C (  )  E.g., ZYZ, ZYX (roll-pitch-yaw), etc

Euler Angles  Which axes to pick, and what order?  Convention A,B,C  R Euler (  ) = R A (  )R B (  )R C (  )  E.g., ZYZ, ZYX (roll-pitch-yaw), etc  Disadvantages  Must constrain to range of values  Singularities, e.g. ZYZ when  =0 or  (Gimbal lock)  Interpolation?

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 (v T x) (1 - cos θ) R AA (θ,v) = cos θ I + sin θ [v] + (1 - cos θ) v v T Cross product matrix Or in matrix form… 0 -v z v y v z 0 -v x -v y v x 0

Recovering Axis and Angle from the Rotation Matrix θ = Angle(R) = cos -1 ((r 11 +r 22 +r 33 -1)/2) = cos -1 ((tr(R)-1)/2) v = Axis(R) = 1/(2 sin θ) r 32 -r 23 r 13 -r 31 r 21 -r 12

Properties of Axis-Angle Disadvantages: Non unique: R AA (θ,v)=R AA (-θ,-v) (can constrain θ to range [0,  ]) 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” R EM (w) = R AA ( ||w||, w/||w|| )

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

Unit quaternion representation q = (q 0,q 1,q 2,q 3 ), ||q||=1 Related to axis angle: q = (cos  /2,v x sin  /2, v y sin  /2, v z sin  /2) R Q (q) = 2(q 0 2 +q 1 2 )-12(q 1 q 2 -q 0 q 3 )2(q 1 q 3 +q 0 q 2 ) 2(q 1 q 2 +q 0 q 3 ) 2(q 0 2 +q 2 2 )-1 2(q 2 q 3 -q 0 q 1 ) 2(q 1 q 3 -q 0 q 2 ) 2(q 2 q 3 +q 0 q 1 ) 2(q 0 2 +q 3 2 )-1

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 9 parameters R ij in range [-1,1] Constraint: determinant +1 Advantages: composition, inversion are easy Euler angles 3 parameters (  in range [0,2  ) Axes picked by convention (e.g., Roll-Pitch-Yaw) Advantages: no constraints, simple interpolation “works” Disadvantages: multiple representation, singularities, inversion not easy Axis-angle 4 parameters (  v),  in [0,  Constraint: |v|=1 Advantages: inversion is easy Disadvantages: constraint, interpolation, multiple representation at  Moment representation (aka exponential map) 3 parameters w (=  v), ||w||<=  Advantages: no constraints, inversion is easy Disadvantages: interpolation, multiple representation at  Quaternion 4 parameters q=(q 0,q 1,q 2,q 3 ) Constraint: |q|=1 Advantages: interpolation formulas, inversion easy Disadvantages: constraint, multiple representation R(q) = R(-q)

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

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(A T B), θ = Angle(A T B) Verify that X(s) = A R AA (sθ,v) satisfies the desired properties Actually a geodesic in SO(3)!

Angular Velocities For parameterized rotation trajectory R EM (wt), we can show: d/dt R EM (wt) = [w] R EM (wt) => |w| is the speed of rotation => 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 Multiple representations of SO(3) All four implemented robustly in C++ in KrisLibrary Axis angle / moment implemented in Python Notion of distance, straight line, speed in SO(3)

Next Lecture Read Principles Ch. 3.8 Optional: A Mathematical Introduction to Robotic Manipulation, Ch