Transcript Rigid transformations, rotations

CS I400/B659:
Intelligent Robotics
Rigid Transformations
3D Rigid Objects
Rigid Transformation in 2D
• q = (tx,ty,q) with q  [0,2p)
• Robot R0R2 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 θ
2D rotation matrix
x
y
+
tx
ty
Affine translation
Rigid Transformation in 3D
• q = (tx,ty,tz,R) 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
Tq(x,y,z) =
R
3D rotation matrix
x
y
z
+
tx
ty
tz
Affine translation
3-D Rigid Rotations

r11 r12 r13
r21 r22 r23
r31 r32 r33
(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
(0,1,0)
(r11,r21,r31)
(1,0,0)
(0,0,1)
(r13,r23,r33)
(1,0,0)
(0,0,1)
(r12,r22,r32)
Coordinate Frames
World frame
Local frame
(0,1,0)
𝑥
𝑦
𝑧
R
𝑟13
𝑟23
𝑟33
𝑟11
𝑟21
𝑟31
(1,0,0)
𝑥
R 𝑦
𝑧
(0,0,1)
R-1 = RT
𝑟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
Rotate about:
Z axis
cos θ -sin θ 0
sin θ cos θ 0
0
0
1
Y axis
cos θ 0 sin θ
0
1
0
-sin θ 0 cos θ
X axis
1 0
0
0 cos θ -sin θ
0 sin θ cos θ
Parameterization of SO(3)
 Euler angles: (f,q,y)
z
z
z
z
y
f
1234
y
q
y
y
x
x
x
x
y
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!
• Rv=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
RQ(q) =
2(q02+q12)-1
2(q1q2+q0q3)
2(q1q3-q0q2)
2(q1q2-q0q3)
2(q02+q22)-1
2(q2q3+q0q1)
• Related to axis angle:
• q = (cos q/2,vx sin q/2, vy sin q/2, vz sin q/2)
2(q1q3+q0q2)
2(q2q3-q0q1)
2(q02+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(R1TR2)-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 𝑅 𝜃 𝑡
we can show:
parameterized by time,
cos 𝜃 𝑡
𝜃 𝑡 =
sin 𝜃 𝑡
− sin 𝜃 𝑡 𝜃′(𝑡) − cos 𝜃 𝑡
𝑑
𝑅
𝑑𝑡
cos 𝜃 𝑡
𝑑
𝑑𝑡
𝜃′(𝑡) − sin 𝜃 𝑡
0 −1
𝜃′ 𝑡
𝑅 𝜃 𝑡
1 0
• => 𝜃 ′ 𝑡 is the speed of rotation
− sin 𝜃 𝑡
cos 𝜃 𝑡
𝜃′(𝑡)
=
𝜃′(𝑡)
=
• For a point in world coordinates 𝑝 𝑡 = 𝑅 𝜃 𝑡 𝑝0 , we
0 −1
have 𝑝′ (𝑡) = 𝜃 ′ 𝑡
𝑝 𝑡
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
• 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