CS I400/B659: Intelligent RoboticsRigid Transformations

3D Rigid Objects

Rigid Transformation in 2D• q = (tx,ty,q) with q [0,2p)

• Robot R0R2 given in reference frame T0

• What’s the new robot Rq? {Tq(x,y) | (x,y) R0}

• Define rigid transformation Tq(x,y) : R2 R2

Tq(x,y) = cos θ -sin θsin θ cos θ

xy

tx

ty+

2D rotation matrix Affine translation

Rigid Transformation in 3D• q = (tx,ty,tz,R) with R a 3x3 rotation matrix

• Robot R0R3 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

Tq(x,y,z) = R

xyz

tx

ty

tz

+

Affine translation3D rotation matrix

3-D Rigid Rotations

r11 r12 r13

r21 r22 r23

r31 r32 r33

(1,0,0)

(0,1,0)

(0,0,1)

(Right-handed coordinate system)

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

(1,0,0)

(r11,r21,r31)

(0,0,1)

(r13,r23,r33)

(r12,r22,r32)

Coordinate Frames

(1,0,0)

(0,1,0)

(0,0,1)

[𝑟11𝑟 21𝑟 31 ]R

R-1 = RT

Local frame World frame

[𝑥𝑦𝑧 ]

R

[𝑟13𝑟 23𝑟 33 ]

[𝑟12𝑟 22𝑟 32 ]

3 representations

• 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

cos θ -sin θ 0sin θ cos θ 0 0 0 1

Rotate about:

Z axis

Y axis

X axis

cos θ 0 sin θ 0 1 0

-sin θ 0 cos θ

1 0 0 0 cos θ -sin θ 0 sin θ cos θ

Parameterization of SO(3) Euler angles: ( , , )f q y

x

y

z

xy

z

f

x

y

z

q

x

y

z

y

1 2 3 4

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

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

Disadvantages Must constrain to range of values Singularities, e.g. ZYZ when q=0 or p (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 (vT x) (1 - cos θ)

RAA(θ,v) = cos θ I + sin θ [v] + (1 - cos θ) v vT

Cross product matrix

Or in matrix form…

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?

Quaternions were a forerunner of vectors and were once mandatory of all students of physics and math!

q = q0+q1i + q2j +q3k, where

i2 = j2 = k2 = -1i = jk = -kjj = ki = -ikk = ij = -ji

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)

RQ(q) = 2(q0

2+q12)-1 2(q1q2-q0q3) 2(q1q3+q0q2)

2(q1q2+q0q3) 2(q02+q2

2)-1 2(q2q3-q0q1)2(q1q3-q0q2) 2(q2q3+q0q1) 2(q0

2+q32)-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 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: 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• 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(R1

TR2)-1)/2) measures the minimum angle needed to rotate from frame R1 to R2

• 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(ATB), θ = Angle(ATB)• Verify that X(s) = A RAA(sθ,v) satisfies the desired properties

Actually a geodesic in SO(3)!

Angular Velocities in 2D

• For a rotation trajectory parameterized by time, we can show:

• => is the speed of rotation• For a point in world coordinates , we have

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

• Multiple representations of SO(3)• All four implemented robustly in C++ in KrisLibrary• Rotation matrix, axis angle, moment representations

are implemented in Python klampt.so3 module• Notion of distance, geodesic, speed in SO(3)

Next Weel• Read Principles Ch. 3.8• Optional: A Mathematical Introduction to Robotic

Manipulation, Ch. 3.1-4• http://www.cds.caltech.edu/~murray/mlswiki/?title=First_edition