View
245
Download
1
Category
Preview:
Citation preview
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
Recommended