Download 1 Chapter 2: Rigid Body Motions and Homogeneous Transforms

Chapter 2:
Rigid Body Motions and Homogeneous Transforms
(original slides by Steve from Harvard)
Representing position
•
Definition: coordinate frame
– A set n of orthonormal basis vectors spanning Rn
⎡0⎤
⎡0⎤
⎡1⎤
– For example,
⎢ ⎥
⎢ ⎥
⎢ ⎥
iˆ = ⎢0⎥, ˆj = ⎢1⎥, kˆ = ⎢0⎥
⎢⎣1⎦⎥
⎢⎣0⎦⎥
⎣⎢0⎦⎥
•
When representing a point p, we need to specify a coordinate frame
– With respect to o0:
– With respect to o1:
•
⎡5 ⎤
p0 = ⎢ ⎥
⎣6⎦
⎡− 2.8⎤
p1 = ⎢
⎥
⎣ 4 .2 ⎦
v1 and v2 are invariant geometric entities
– But the representation is dependant
upon choice of coordinate frame
⎡5 ⎤
⎡7.77⎤ 0 ⎡− 5.1⎤ 1 ⎡− 2.8⎤
v 10 = ⎢ ⎥, v 11 = ⎢
⎥ , v 2 = ⎢ 1 ⎥ , v 2 = ⎢ 4 .2 ⎥
⎣6 ⎦
⎣ 0 .8 ⎦
⎣
⎦
⎣
⎦
1
Rotations
•
2D rotations
– Representing one coordinate frame in terms of another
[
R10 = x10
y 10
]
– Where the unit vectors are defined as:
⎡cos θ ⎤ 0
⎡− sinθ ⎤
x10 = xˆ 0 ⎢
⎥, y 1 = yˆ 0 ⎢ cos θ ⎥
θ
sin
⎣
⎦
⎣
⎦
⎡cos θ
R10 = ⎢
⎣ sinθ
− sinθ ⎤
cos θ ⎥⎦
– This is a rotation matrix
Alternate approach
•
Rotation matrices as projections
– Projecting the axes of from o1 onto the axes of frame o0
⎡ yˆ ⋅ xˆ ⎤
⎡ xˆ ⋅ xˆ ⎤
x10 = ⎢ 1 0 ⎥, y 10 = ⎢ 1 0 ⎥
ˆ
ˆ
x
y
⋅
⎣ yˆ1 ⋅ yˆ 0 ⎦
⎣ 1 0⎦
⎡ xˆ1 ⋅ xˆ0
R10 = ⎢
⎣ xˆ1 ⋅ yˆ 0
yˆ1 ⋅ xˆ0 ⎤
yˆ1 ⋅ yˆ 0 ⎥⎦
⎡
xˆ1 xˆ0 cos θ
⎢
=⎢
⎢ xˆ yˆ cos⎛⎜ π − θ ⎞⎟
⎢⎣ 1 0
⎝2
⎠
=
π ⎞⎤
⎛
yˆ1 xˆ0 cos⎜θ + ⎟⎥
2 ⎠⎥
⎝
yˆ1 yˆ 0 cos θ ⎥
⎥⎦
2
Properties of rotation matrices
•
Inverse rotations:
•
Or, another interpretation uses odd/even properties:
Properties of rotation matrices
•
Inverse of a rotation matrix:
(R )
0 −1
1
⎡ xˆ ⋅ xˆ
=⎢ 1 0
⎣ xˆ1 ⋅ yˆ 0
yˆ1 ⋅ xˆ 0 ⎤
⎥
yˆ1 ⋅ yˆ 0 ⎦
−1
⎡
xˆ1 xˆ 0 cos θ
⎢
=⎢
⎢ xˆ yˆ cos⎛⎜ π − θ ⎞⎟
⎢⎣ 1 0
⎝2
⎠
⎡cos θ
=⎢
⎣ sinθ
•
−1
π ⎞⎤
⎛
yˆ1 xˆ 0 cos⎜θ − ⎟⎥
2 ⎠⎥
⎝
yˆ1 yˆ 0 cos θ ⎥
⎥⎦
− sinθ ⎤
1
=
cos θ ⎥⎦
det R10
⎡ cos θ
( ) ⎢⎣− sinθ
−1
sinθ ⎤
= R10
cos θ ⎥⎦
( )
T
The determinant of a rotation matrix is always ±1
– +1 if we only use right-handed convention
3
Properties of rotation matrices
•
Summary:
– Columns (rows) of R are mutually orthogonal
– Each column (row) of R is a unit vector
R T = R −1
det (R ) = 1
•
The set of all n x n matrices that have these properties are called the
Special Orthogonal group of order n
R ∈ SO(n )
3D rotations
•
General 3D rotation:
⎡ xˆ1 ⋅ xˆ 0
R10 = ⎢⎢ xˆ1 ⋅ yˆ 0
⎢⎣ xˆ1 ⋅ zˆ0
•
yˆ1 ⋅ xˆ 0
yˆ1 ⋅ yˆ 0
zˆ1 ⋅ zˆ0
zˆ1 ⋅ xˆ 0 ⎤
xˆ1 ⋅ yˆ 0 ⎥⎥ ∈ SO (3 )
zˆ1 ⋅ zˆ0 ⎥⎦
Special cases
– Basic rotation matrices
0
⎡1
R x,θ = ⎢⎢0 cos θ
⎢⎣0 sinθ
⎡ cos θ
Ry ,θ = ⎢⎢ 0
⎢⎣− sinθ
0 ⎤
− sinθ ⎥⎥
cos θ ⎥⎦
sinθ ⎤
0 ⎥⎥
0 cos θ ⎥⎦
0
1
⎡cos θ
Rz,θ = ⎢⎢ sinθ
⎢⎣ 0
− sinθ
cos θ
0
0⎤
0⎥⎥
1⎥⎦
4
Properties of rotation matrices (cont’d)
•
SO(3) is a group under multiplication
1 Closure: if R1 , R2 ∈ SO (3) ⇒ R1R2 ∈ SO(3 )
1.
2. Identity:
⎡ 1 0 0⎤
I = ⎢⎢0 1 0⎥⎥ ∈ SO (3 )
⎣⎢0 0 1⎥⎦
3. Inverse: R T = R −1
4. Associativity: (R1R2 )R3 = R1(R2R3 )
Allows us to combine rotations:
Rac = Rab Rbc
•
In general, members of SO(3) do not commute
Rotational transformations
•
Now assume p is a fixed point on the rigid object with fixed coordinate
frame o1
– The point p can be represented in the
frame o0 (p0) again by the projection onto
the base frame
⎡u ⎤
p1 = ⎢⎢ v ⎥⎥
⎢⎣w ⎥⎦
⎡ p1 ⋅ xˆ0 ⎤
⎥
⎢
p 0 = ⎢ p1 ⋅ yˆ 0 ⎥
⎢ p1 ⋅ zˆ0 ⎥
⎦
⎣
⎡ (uxˆ1 + vyˆ1 + wzˆ1 ) ⋅ xˆ0 ⎤
= ⎢⎢(uxˆ1 + vyˆ1 + wzˆ1 ) ⋅ yˆ 0 ⎥⎥
⎢⎣ (uxˆ1 + vyˆ1 + wzˆ1 ) ⋅ zˆ0 ⎥⎦
⎡ uxˆ1 ⋅ xˆ0 + vyˆ1 ⋅ xˆ0 + wzˆ1 ⋅ xˆ0 ⎤
= ⎢⎢uxˆ1 ⋅ yˆ 0 + vyˆ1 ⋅ yˆ 0 + wzˆ1 ⋅ yˆ 0 ⎥⎥
⎢⎣ uxˆ1 ⋅ zˆ0 + vyˆ1 ⋅ zˆ0 + wzˆ1 ⋅ zˆ0 ⎥⎦
=
5
Rotating an Object
Rotating a vector
•
Another interpretation of a rotation matrix:
– Rotating a vector about an axis in a fixed frame
– Ex: rotate v0 about y0 by π/2
⎡0⎤
v 0 = ⎢⎢ 1⎥⎥
⎣⎢ 1⎦⎥
6
Rotation matrix summary
•
Three interpretations for the role of rotation matrix:
1. Representing the coordinates of a point in two different frames
1
2. Orientation of a transformed coordinate frame with respect to a fixed frame
3. Rotating vectors in the same coordinate frame
7
Similarity transforms
•
All coordinate frames are defined by a set of basis vectors
– These span Rn
– Ex: the unit vectors i, j, k
•
In linear algebra, a n x n matrix A is a mapping from Rn to Rn
– y = Ax, where y is the image of x under the transformation A
– Think of x as a linear combination of unit vectors (basis vectors), for
example the unit vectors:
e1 = [1 0 ... 0] , ..., en = [0 0 ... 1]
T
T
– If we want to represent vectors with respect to a different basis,
basis e
e.g.:
g : f1, …,
fn, the transformation A can be represented by:
A′ = T −1AT
– Where the columns of T are the vectors f1, …, fn,
– A’ is called the similarity transformation.
Similarity transforms
•
Rotation matrices are also a change of basis
– If A is a linear transformation in o0 and B is a linear transformation in o1,
then they are related as follows:
•
Ex: the frames o0 and o1 are related as follows:
( )
B = R10
−1
AR10
8
Compositions of rotations
•
w/ respect to the current frame
– Ex: three frames o0, o1, o2
p 0 = R10 p1
p 0 = R10R21 p 2
p1 = R21 p 2
p =R p
0
•
0
2
R20 = R10R21
2
This defines the composition law for successive rotations about the
current reference frame: post-multiplication
Compositions of rotations
•
Ex: R represents rotation about the current y-axis by φ followed by θ
about the current zz-axis
axis
R = R y ,φ Rz,θ
⎡ cos φ 0 sin φ ⎤ ⎡cos θ
= ⎢⎢ 0
1
0 ⎥⎥ ⎢⎢ sinθ
⎢⎣− sin φ 0 cos φ ⎥⎦ ⎢⎣ 0
•
− sinθ
cos θ
0
0⎤ ⎡ cos φ cos θ
0⎥⎥ = ⎢⎢
sinθ
1⎥⎦ ⎢⎣− sin φ cos θ
− cos φ sinθ
cos θ
sin φ sinθ
sin φ ⎤
0 ⎥⎥
cos φ ⎥⎦
What about the reverse order?
9
Compositions of rotations
•
w/ respect to a fixed reference frame (o0)
– Let the rotation between two frames o0 and o1 be defined by R10
– Let R be a desired rotation w/ respect to the fixed frame o0
– Using the definition of a similarity transform, we have:
[(
R20 = R10 R10
•
)
−1
]
RR10 = RR10
This defines the composition law for successive rotations about a fixed
reference frame: pre-multiplication
Compositions of rotations
•
Ex: we want a rotation matrix R that is a composition of φ about y0 (Ry,φ)
and then θ about z0 (Rx,
x θ)
– the second rotation needs to be projected back to the initial fixed frame
R20 = (Ry ,θ ) Rz,θ Ry ,θ
−1
= Ry ,−θ Rz,θ R y ,θ
– Now the combination of the two rotations is:
R = R y ,φ [R y ,−φ Rz,θ R y ,φ ] = Rz,θ R y ,φ
10
Compositions of rotations
•
Summary:
– Consecutive rotations w/ respect to the current reference frame:
•
Post-multiplying by successive rotation matrices
– w/ respect to a fixed reference frame (o0)
•
Pre-multiplying by successive rotation matrices
– We can also have hybrid compositions of rotations with respect to the
current and a fixed frame using these same rules
Parameterizing rotations
•
There are three parameters that need to be specified to create arbitrary
rigid body rotations
–
1.
2.
3.
We will describe three such parameterizations:
Euler angles
Roll, Pitch, Yaw angles
Axis/Angle
11
Parameterizing rotations
•
Euler angles
– Rotation by φ about the z-axis, followed by θ about the current y-axis, then
ψ about
b t the
th currentt z-axis
i
⎡cφ − sφ 0 ⎤ ⎡ cθ 0 sθ ⎤ ⎡cψ − sψ 0⎤
RZYZ = Rz,φ Ry ,θ Rz,ψ = ⎢⎢sφ cφ 0 ⎥⎥ ⎢⎢ 0
cψ 0⎥⎥
1 0 ⎥⎥ ⎢⎢sψ
⎢⎣ 0
0
1⎥⎦ ⎢⎣− sθ 0 cθ ⎥⎦ ⎢⎣ 0
0
1⎥⎦
⎡cφ cθ cψ − sφ sψ − cφ cθ sψ − sφ cψ cφ sθ ⎤
⎢
⎥
= ⎢sφ cθ cψ + cφ sψ − sφ cθ sψ + cφ cψ sφ sθ ⎥
⎢
− sθ cψ
sθ sψ
cθ ⎥⎦
⎣
Parameterizing rotations
•
Roll, Pitch, Yaw angles
– Three consecutive rotations about the fixed principal axes:
•
Yaw (x0) ψ, pitch (y0) θ, roll (z0) φ
R XYZ = Rz,φ R y ,θ R x ,ψ
⎡cφ
= ⎢⎢sφ
⎣⎢ 0
⎡cφ cθ
⎢
= ⎢sφ cθ
⎢
⎣ − sθ
− sφ
cφ
0
0⎤ ⎡ cθ
0⎥⎥ ⎢⎢ 0
1⎦⎥ ⎣⎢− sθ
0 sθ ⎤ ⎡ 1 0
⎢
1 0 ⎥⎥ ⎢0 cψ
0 cθ ⎥⎦ ⎢⎣0 sψ
− sφ cψ + cφ sθ sψ
cφ cψ + sφ sθ sψ
cθ sψ
0 ⎤
⎥
− sψ ⎥
cψ ⎥⎦
sφ sψ + cφ sθ cψ ⎤
⎥
− cφ sψ + sφ sθ cψ ⎥
⎥
cθ cψ
⎦
12
Parameterizing rotations
•
Axis/Angle representation
– Any rotation matrix in SO(3) can be represented as a single rotation about a
suitable axis through a set angle
⎡k x ⎤
– For example, assume that we have a unit vector: k̂ = ⎢k y ⎥
⎢ ⎥
– Given θ, we want to derive Rk,θ:
⎢⎣ k z ⎥⎦
•
Intermediate step: project the z-axis onto k:
Rk ,θ = RRz,θ R −1
•
Where the rotation R is given by:
R = Rz,α R y ,β
⇒ Rk ,θ = Rz,α Ry ,β Rz,θ Ry ,− β Rz,−α
Parameterizing rotations
•
Axis/Angle representation
– This is given by:
Rk ,θ
⎡ k x2v θ + cθ
⎢
= ⎢k x k y v θ + k z sθ
⎢k x k zv θ − k y sθ
⎣
k x k y v θ − k z sθ
k y2v θ + cθ
k y k zv θ + k x sθ
k x k zv θ + k y sθ ⎤
⎥
k y k zv θ − k x sθ ⎥
2
k z v θ + cθ ⎥⎦
– Inverse problem:
•
Given arbitrary R, find k and θ
13
Rigid motions
•
Rigid
g motion is a combination of rotation and translation
– Defined by a rotation matrix (R) and a displacement vector (d)
R ∈ SO (3 )
d ∈ R3
– the group of all rigid motions (d,R) is known as the Special Euclidean
group, SE(3)
SE (3 ) = R n × SO (3 )
– Consider three frames, o0, o1, and o2 and corresponding rotation matrices
R21, and R10
• Let d21 be the vector from the origin o1 to o2, d10 from o0 to o1
• For a point p2 attached to o2, we can represent this vector in frames o0 and o1:
p1 = R21 p 2 + d 21
p 0 = R10 p1 + d10
(
)
= R10 R21 p 2 + d 21 + d10
= R10R21 p 2 + R10d 21 + d10
Homogeneous transforms
•
We can represent
p
rigid
g motions ((rotations and translations)) as matrix
multiplication
– Define:
⎡R 0 d10 ⎤
H10 = ⎢ 1
⎥
1⎦
⎣0
⎡R 1 d 21 ⎤
H 21 = ⎢ 2
⎥
⎣ 0 1⎦
– Now the point p2 can be represented in frame o0: P 0 = H10H 21P 2
– Where the P0 and P1 are:
⎡ p0 ⎤
⎡ p1 ⎤
P 0 = ⎢ ⎥, P 1 = ⎢ ⎥
⎣1⎦
⎣1⎦
14
Example
• Find the homogeneous transformation matrix
(T) for the following operations:
Rotation α about OX axis
Translation of a along OX axis
Translation of d along OZ axis
Rotation of θ about OZ axis
T = Tz ,θ Tz ,d Tx ,aTx ,α I 4×4
Answer :
Homogeneous Transformation
–Composite Homogeneous Transformation Matrix
z1
z2
y2
z0
0
y1
A1
y0
x2
1
A2
x1
x0
–?
i −1
i
A
A20 = A10 A21
–Transformation matrix for
adjacent coordinate frames
–Chain product of successive coordinate
transformation matrices
15
Example 8
• For the figure shown below, find the 4x4 homogeneous transformation
matrices Aii −1 and Ai0 for i=1, 2, 3, 4, 5
0 ⎤
⎡− 1 0 0
⎡ nx s x a x p x ⎤
c
⎢
⎥
⎢n s a
p y ⎥⎥ A0 = ⎢ 0 0 − 1 e + c ⎥
y
y
y
⎢
1
z3
F=
⎢ 0 −1 0 a − d ⎥
b
⎢ nz s z a z p z ⎥
y3 x
⎥
⎢
3
⎢
⎥
1 ⎦
d
z5
⎣0 0 0
⎣0 0 0 1 ⎦
x5
y5
z4
x4
a
e
z2
y4
x2
z1
z0
y2
y1
y0
x0
x1
–Can you find the answer by observation based on
the geometric interpretation of homogeneous
transformation matrix?
Homogeneous transforms
•
The matrix multiplication
p
H is known as a homogeneous
g
transform
and we note that
()
H ∈ SE 3
•
Inverse transforms:
⎡RT
H −1 = ⎢
⎣0
− RT d ⎤
⎥
1 ⎦
16
Homogeneous transforms
•
Basic transforms:
– Three pure translation, three pure rotation
Trans x ,a
Trans y ,b
Trans z,c
⎡1
⎢0
=⎢
⎢0
⎢
⎣0
⎡1
⎢0
=⎢
⎢0
⎢
⎣0
⎡1
⎢0
=⎢
⎢0
⎢
⎣0
0 0 a⎤
1 0 0⎥⎥
0 1 0⎥
⎥
0 0 1⎦
0 0 0⎤
1 0 b ⎥⎥
0 1 0⎥
⎥
0 0 1⎦
0 0 0⎤
1 0 0⎥⎥
0 1 c⎥
⎥
0 0 1⎦
Rot x,α
Rot y ,β
Rot z,γ
⎡1 0
⎢0 c
α
=⎢
⎢0 sα
⎢
⎣0 0
⎡ cβ
⎢ 0
=⎢
⎢− s β
⎢
⎣ 0
⎡cγ
⎢s
=⎢ γ
⎢0
⎢
⎣0
0
− sα
cα
0
0 sβ
1 0
0 cβ
0
0
− sγ
0
cγ
0
0
1
0
0
0⎤
0⎥⎥
0⎥
⎥
1⎦
0⎤
0⎥⎥
0⎥
⎥
1⎦
0⎤
0⎥⎥
0⎥
⎥
1⎦
17