CS计算机代考程序代写 arm CS 4610/5335
CS 4610/5335
Rotation Representation and Kinematics
Robert Platt Northeastern University
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A
Why are we studying this?
You want to put your hand on the cup…
• Suppose your eyes tell you where the mug is and its orientation in the robot base frame (big assumption)
• In order to put your hand on the object, you want to align the coordinate frame of your hand w/ that of the object
• This kind of problem makes representation of pose important…
Why are we studying this?
Joint encoders tell us head angle
Visual perception tells us object position and orientation (pose)
Need to know where hand is…
Need to tell the hand where to move!
KIT Humanoid
Why are we studying this?
2-D and 3-D Transforms
Representing Position: Vectors
Representing Position: vectors
(“column” vector)
(“row” vector)
y
2
y
p
p
5
x
x
z
Representing Position: vectors
yˆb
• Vectors are a way to transform between two different reference frames w/ the same orientation
Frame a
• The prefix superscript denotes the reference frame in which the vector should be understood
2
b
xˆb Point p
Frame b
5
Same point, two different reference frames
Representing Position: vectors
yˆb • Note that I am denoting the axes as
Frame a
orthogonal unit basis vectors
This means “perpendicular”
xˆb yˆb
2
b
5
xˆb Point p
A vector of length one pointing in the direction of the b (base) frame x axis
b frame y axis a frame y axis
Frame b
What is a unit vector? These are the elements of a:
x and y components of a
Distance in each basis direction
Basis vectors
What is a unit vector? These are the elements of a:
Vector length/magnitude:
Definition of unit vector:
How convert a non-zero vector a into a unit vector pointing in the same direction?
b yˆ 2
5
b ^x
What is a unit vector? These are the elements of a:
Vector length/magnitude:
Definition of unit vector:
b yˆ 2
b ^x a
a2 a2 xy
5
How convert a non-zero vector a into a unit vector pointing in the same direction?
a
ˆ
Orthogonal vectors
First, define the dot product: ab axbx ayby
Under what conditions is the dot product zero?
b aˆ
Orthogonal vectors
First, define the dot product: ab axbx ayby
Under what conditions is the dot product zero? ab0 when: a0
b0 cos0
b aˆ
or, or,
Orthogonal vectors
First, define the dot product: ab axbx ayby
Under what conditions is the dot product zero? ab0 when: a0
b aˆ
b0 cos0
Unit vectors are orthogonal iff the dot product is zero:
is orthogonal to iff
or, or,
pˆ y
pˆ x
z
A couple of other random things
b yˆ 2
b
y
Vectors are elements of
Rn
y
5
b xˆ
x
z
x
right-handed coordinate frame
left-handed coordinate frame
The importance of differencing two vectors
b xeff
b xerr
b xobject
The eff needs to make a Cartesian displacement of this much to reach the object
The importance of differencing two vectors
yˆ o b j e c t object
yˆb
err
b
The eff needs to make a Cartesian displacement of this much to reach the object
xˆobject yˆ eff
eff
xˆb
xˆeff
Representing Orientation: Rotation Matrices
• The reference frame of the hand and the object have different orientations
• We want to represent and difference orientations just like we did for positions…
2-D Rotation
Before we go there – review of matrix transpose
Question:
Before we go there – review of matrix transpose
Important property:
and matrix multiplication…
Can represent dot product as a matrix multiply:
Same point – different reference frames
3.8 2
• for the moment, assume that there is no difference in position…
p
5 3.8
Another important use of the dot product: projection
abaxbx ayby
b
l
aˆ
Same point – different reference frames
aˆaab xbppcospx
B-frame’s y axis written in A frame
ap
5 3.8
a xˆ b
a yˆ b 3.8
2
B-frame’s x axis written in A frame
2
B-frame’s y axis written in A frame
p
5 3.8
a xˆ b
Think-pair-share
a yˆ b 3.8
Calculate given
Same point – different reference frames
baˆa pyybp
2
B-frame’s y axis written in A frame
ap
5 3.8
a yˆ b 3.8
baˆa pxxbp
a xˆ b
B-frame’s x axis written in A frame
Same point – different reference frames
A yˆ B 3.8
2 BAˆA
p
pyyBp
5 3.8
BAˆA
pxxBp xˆB
A
Same point – different reference frames
Where:
Rotation matrix
or
Rotation matrices
A rotation matrix is a 2×2 or 3×3 matrix R such that:
1. 2.
Rows and columns are unit length and orthogonal
Right handed coordinate frame
Rotation matrix inverse equals transpose:
Rotation matrices
A rotation matrix is a 2×2 or 3×3 matrix R such that:
1. 2.
Rows and columns are unit length and orthogonal
Right handed coordinate frame
Unit vectors and orthogonal to each other
1. 2.
Rows and columns are unit length and orthogonal
Right handed coordinate frame
Rotation matrices
A rotation matrix is a 2×2 or 3×3 matrix R such that:
Unit vectors and orthogonal to each other
By convention: where Similarly:
Notice: Therefore:
where
Rotation matrices
Because of properties of rotation matrix
Think-pair-share
Given: Calculate:
y
Example: 2D rotation matrix
aˆ bˆy
bˆ x
aˆ x
ay^)=(cos(θ) −sin(θ)) b b sin(θ) cos(θ)
a x^b=(cos(θ)) sin(θ)
a ^y b = ( − s i n ( θ ) ) cos(θ)
aR=(a^x b
b Ra=(cos(θ)
−sin(θ) cos(θ)
sin(θ))
Composition of 2D rotation matrices
c ^y θ2
b yˆ
a ^y θ1
p
c x^
b ^x a x^
AAB
RRR CBC
a Rc=(cos(θ1) −sin(θ1))(cos(θ2) −sin(θ2))=(c1 c2−s1 s2 sin(θ1) cos(θ1) sin(θ2) cos(θ2) s1c2+c1 s2
=(c12 −s12) s12 c12
−c1 s2−s1 c2) c1c2−s1 s2
2-D Rotation: Summary
3-D Rotation
Rotations about x, y, z
cos(α) −sin(α) 0 R (α)= sin(α) cos(α) 0
z
()
001
cos(β) 0 sin(β) Ry(β)=(0 1 0)
−sin(β) 0 cos(β) 100
R (γ)= 0 cos(γ) −sin(γ)
x
() 0 sin(γ) cos(γ)
These rotation matrices encode the basis vectors of the after- rotation reference frame in terms of the before-rotation reference frame
Example: 3D rotation matrix
Ay
45
Bz
Ax
Bx
Az
By
1. Calculate:
2. What’s the magnitude of this rotation?
A
y
A AˆAˆAˆ RB xB yB zB
Example: 3D rotation matrix
Bz
Ax
45
Az
101
Bx √2√2
ARB=(0−1 0) 1 0−1
√2 √2
By
Example: 3D rotation matrix
zˆa
yˆ a
xˆc xˆb
cθ −sθ 0 aRb=s c 0
xˆa
c−φ 0 s−φ cφ 0 −sφ
bR=0 1 0=0 1 0 (θ θ)c()()
0 0 1 −s−φ 0 c−φ sφ 0 cφ
Example: 3D rotation matrix
zˆa
yˆ a
xˆc xˆb
xˆa
cθcφ −sθ −cθsφ
cθ −sθ 0 cφ 0 −sφ
aR=aRbR=s c 0 0 1 0=sc c −ss
c bc(θ θ)()(θφ θ θφ) 0 0 1 sφ 0 cφ sφ 0 cφ
Homogeneous transforms
Rotation matrices assume that the origins of the two frames are co-located.
What if they’re separated by a translation?
By Ay
p
Ax
BdA
Bx
Homogeneous transform
Bp
AdB
Ap
Bˆ y
Aˆ y
(same point, two reference frames)
AABA pRB p dB
Aˆ x
Bˆ x
Homogeneous transform
By y
p
AABA A pRB p dB
ARAdBp =( B B)(1)
adb
rrrAd AxBx
01
11 12 13 x
rrrAdBpA Bp = 21 22 23 y (1)= TB(1)
r31 r32 r33 dz (A)
0001
always zeros
always one
Example 1: homogeneous transforms
Bx Bz
By
Az
l Ay
Ax What’s BTA?
Example 1: homogeneous transforms
BAy
What’s TA?
cos(θ) −sin(θ) 0
By
Bx
AR=sin(θ) cos(θ) 0
Az,Bz
B
()
001
−l B BRBd Bd=(0) TA=( A
A)
Ay
By Bx B
A01 0
z l
Ax
B
cos(θ) sin(θ) 0 −l −sin(θ) cos(θ) 0 0
TA=(0 010) 0001
Ax
Az
zb
xb
yb
l
Think-pair-share
l
xa
za
This arm rotates about the zaaxis.
aT b
ya
Calculate:
z yb xb b
π cθ 0 −sθ l√2c(θ+4)
Example 2: homogeneous transforms
l l
aT =sθ 0 cθ l√2s(θ+π) 0−10 0
ya
b[ 4] 0001
This arm rotates about the zaaxis. xa
za
What’s
aT ? b
Example 3: homogeneous transforms
A yˆ
l
c yˆ
c xˆ c zˆ
A xˆ
A zˆ
cθ 0 sθ cφ −sφ 0 cθcφ −sφcθ sθ
aR=aRbR=010sc0=s c0 cbc()(φφ)(φ φ)
−sθ 0 cθ
Calculate and
0 0 1 −sθcφ sθsφ cθ
Example 3: homogeneous transforms
A yˆ
l
c yˆ
c xˆ c zˆ
−l
cθ cφ ad=−aRcd=− s
−sφ cθ c
lcθ cφ
A zˆ
A xˆ
sθ −l
cd=0
0 −sθ cφ sθ sφ cθ
0(0)= ls ()c(φφ)0(φ)
cθ cφ −sφ cθ sθ lcθ cφ a aR ad s c 0 ls
Tc=(c )=(φ φ φ) 0 1 −sθ cφ sθ sφ cθ −lsθ cφ
0001
−lsθ cφ
Representing Orientation
My head
is spinning…
The space of rotations
SO3RR33 |RRT I,det(R)1
Special orthogonal group(3): Why det(R) 1?
Rotations preserve distance: Rotations preserve orientation:
Rp Rp p p 1212
Rp Rp Rp p 1212
The space of rotations
SO3RR33 |RRT I,det(R)1
Special orthogonal group(3): Why it’s a group:
Closed under multiplication: if R , R SO3 then R R SO3 12 12
Has an identity:
Has a unique inverse… Is associative…
Why orthogonal:
vectors in matrix are orthogonal
Why it’s special: det(R) 1 , NOT det(R) 1
Right hand coordinate system
Possible rotation representations
You need at least three numbers to represent an arbitrary rotation in SO(3) (Euler theorem). Some three-number representations:
ZYZ Euler angles
ZYX Euler angles (roll, pitch, yaw) Axis angle
One four-number representation: quaternions
To get from A to B:
Rotate about z axis Then rotate about y axis Then rotateabout z axis
cosφ −sinφ 0 R(φ)=sinφ cosφ 0
ZYZ Euler Angles
r=
φ θ
zyz
ψ
()
z
()
001
cos θ 0 sin θ Ry(θ)=(0 1 0)
−sin θ 0 cos θ cosψ −sinψ 0
R (ψ)= sinψ cosψ 0
z
()
001
ZYZ Euler Angles
Remember that Rz () Ry () Rz ()encode the desired rotation in the pre- rotation reference frame:
pre rotation
Rz () Rpost rotation
Therefore, the sequence of rotations is concatentated as follows:
Rzyz,,Rz ()Ry ()Rz ()
cosφ −sinφ 0 cosθ 0 sinθ cosψ −sinψ 0
R (φ,θ,ψ)=sinφ cosφ 0 0 1 0 sinψ cosψ 0
zyz
( )( )( ) 0 01−sinθ0cosθ0 01
cφcθcψ−sφsψ −cφcθsψ−sφcψ cφsθ Rzyz(φ,θ,ψ)= s c c +c s −s c s +c c s s
(φθψφψ φθψφψφθ) −sθ cψ sθ sψ cθ
ZYX Euler Angles (roll, pitch, yaw)
To get from A to B:
Rotate about z axis Then rotate about y axis Then rotateabout x axis
cosφ −sinφ 0 R(φ)=sinφ cosφ 0
z
()
0 0 1
cos θ 0 sin θ Ry(θ)=(0 1 0)
−sin θ 0 cos θ 100
Rzyx,,Rz ()Ry ()Rx ()
cosφ−sinφ 0 cosθ 0 sinθ 1 0 0
R (φ,θ,ψ)=sinφ cosφ 0 0 1 0 0 cosψ −sinψ
zyz
( )( )( ) 0 0 1 −sinθ 0 cosθ 0 sinψ cosψ
R(ψ)=0 cosψ −sinψ
x
()
0 sin ψ cos ψ
3-D Rotation is noncommutative
Think-pair-share: problems w/ Euler angles How far apart are these two orientations, actually (x-y-z Euler)?
Think-pair-share: problems w/ Euler angles How far apart are these two orientations, actually (x-y-z Euler)?
So … differences between Euler angles may not reflect actual distances in orientation
An extreme case of this problem is known as “gimbal lock”.
– Euler system loses a degree of freedom – any Euler angle representation can suffer
from this
Problem w/ Euler Angles: gimbal lock
1. When a small change in orientation is associated with a large change in rotation representation
2. Happens in “singular configurations” of the rotational representation (similar to singular configurations of a manipulator)
3. This is a problem w/ any Euler angle representation
Problem w/ Euler Angles: gimbal lock
Axis-angle representation
Theorem: (Euler). Any orientation, R SO3 , is equivalent to a rotation
aboutafixedaxis, R3,throughanangle[0,2) (also called exponential coordinates)
Axis:
Converting to a rotation matrix:
Rk e
ISksinSk1cos
k = (y)
kx k
Angle:
kz
Sk 2
Rodrigues’ formula
Axis-angle representation Converting to axis angle:
Magnitude of rotation:
Axis of rotation:
θ=|k|=cos−1(trace(R)−1) 2
Where:
r11 r12 r13 R=r r r
and:
trace(R)r r r 11 22 33
r32−r23 r −r
1
2sinθ(13 31)
k^ =
r21−r12
(12 22 23) r13 r23 r33
Axis-angle representation
Axis angle is can be encoded by just three numbers instead of four:
If then and
Formostorientations, Rk ,isunique.
For rotations of 180, there are two equivalent representations:
If k 180then R R kk
0
r1=(0)r2=(0 ) 179 −179
r1−r2=(0 ) 358
, but the actual distance is
2
Axis-angle problems
Still suffers from the “edge” and distance preserving problems of Euler angles:
00
Distance metric changes as you get further from origin.
Projection distortions
Example: differencing rotations
Calculate the difference between these two rotations:
π 0 2 k=π
1()22
k = 0 ( )
0
k −k =−π 12(2)
0
kk 127.27
2
0
This is NOT the right answer:
According to that, this is the magnitude of the difference:
π
2
12
Question
Calculate the difference between these two rotations:
π 0 2 k=π
This is NOT the right answer:
According to that, this is the magnitude of the difference:
π
2
1()22
k = 0 ( )
0
k −k =−π 12(2)
0
kk 127.27
2
0
12
What is the real angular difference between these two orientations?
Example: differencing rotations Convert to rotation matrices to solve this problem:
π
1 R B R T B R k 1 = 0 k 2 = ( 2 )
0 2π
212 ()0 1000
π 0 cos(π)−sin(π) 1 0 0
b
bR=R(π)=(0 1 0)=(010)
R=R()= 1×2
2 2=0 0−1
π )( )
( π
0 sin(2) cos(2) 0 1 0
cos(π) 0 sin(π) 0 0 1 22
2y2
−1 0 0 100001001
−sin(π ) 0 cos(π ) 22
1R=BRBR=0 0 1 0 10=−1 0 0
2 1T 2 trace(R)−1
θ=cos−1(
)=cos−1(− )=2 π 2 3
k^=
1 r32−r23 1 −1 2π−1 r13−r31 =√ (1 ) k=3 1
( )( )( )
0 −1 0 −1 0 0 1
0 −1 0
2
2sinθ( )3−1 √3() r21−r12 −1
Quaternions
So far, rotation matrices seem to be the most reliable method of manipulating rotations. But there are problems:
Over a long series of computations, numerical errors can cause these 3×3 matrices to no longer be orthogonal (you need to “orthogonalize” them from time to time).
Although you can accurately calculate rotation differences, you can’t interpolate over a difference.’
Suppose you wanted to smoothly rotate from one orientation to another – how would you do it?
Answer: quaternions…
Quaternions
Generalization of complex numbers: Q q iq jq kq
Essentially a 4-dimensional quantity
0123 Qq0,q
Properties of complex dimensions:
iijjkkijk1 jkkji ij jik kiikj
Multiplication:
QP q iq jq kq p ip jp kp 01230123
Q P p 0 q 0 p q , p 0 q q 0 p p q
Complex conjugate:
**
Q q0,q q0,q
Quaternions Invented by Hamilton in 1843:
Along the royal canal in Dublin…
Quaternions
Let’s consider the set of unit 2 2 2 2 2
quaternions: Q q q q q 1 0123
Thisisafour-dimensionalhypersphere,i.e.the3-sphere S3 The identity quaternion is: Q 1,0
QQ* q0,qq0,qq0q0 q2,q0q q0qqq(1,0) Therefore, the inverse of a unit quaternion is: Q* Q 1
Since:
Question Associate a rotation with a unit quaternion as follows:
Given a unit axis, kˆ , and an angle, : (just like axis angle)
The associated quaternion is:
Therefore, Q represents the same rotation as Q
Qk^,θ=(cos(θ),k^sin(θ)) 22
Question Associate a rotation with a unit quaternion as follows:
Given a unit axis, kˆ , and an angle, : (just like axis angle)
The associated quaternion is:
Therefore, Q represents the same rotation as Q
Qk^,θ=(cos(θ),k^sin(θ)) 22
Why?
Quaternions Associate a rotation with a unit quaternion as follows:
Given a unit axis, kˆ , and an angle, : (just like axis angle)
The associated quaternion is:
Therefore, Q represents the same rotation as Q
Let i P 0,i p be the quaternion associated with the vector
You can rotate aP from frame a to b: bPQ aPQ * ba ba
Composition: Qca QcbQba
Q Q Q 1 cb caba
i p
Qk^,θ=(cos(θ),k^sin(θ)) 22
Inversion:
10 Rotatea by Q=1,1
Example: Quaternions
P=0, 0 (2 (2)) (() √ √
0
0 010
bP=QaPQ¿=1 , 1 0,0 1 ,−1 √√0√√
(2 (2)(())(2 (2)) 00
01 = 1 , 1 0, √2
√√1
(2 (2))((0 ))
0 −√2 1−1
22
0
=(0,(01 )+(0 1 )=(0,(0 )) −2−2 −1
Example: Quaternions
Find the difference between these two axis angle rotations:
sin(π)=cos(π)=1 4 4 √2
QP=(p0 q0−p⋅q , p0 q+q0 p+p×q) 0−1
Q =Q Q = 1 , 1 1 , √2
π0 2 π
0
Q=1 ,(1)Q=1 ,√2
k1= 0 k2= () (2)
00 1
√2 √2 ba 0 √2
cb()(()) 00
cb ca
ba √ √ √
−1 (2 (2)(2 (0 ))
0 0 −1−11
√2 2 −√3 =1,1 1 =1,(1) 1
θcb=cos−1(1)=2 π 23
2 √2(√2 ) 2 2 k =
1 (−1)(−) cb √3
() √2 2 −1
√3
Quaternions: Interpolation
Suppose you’re given two rotations, R1 and R2 How do you calculate intermediate rotations?
This does not even result in a rotation matrix
Suprisingly, this actually works Finds a geodesic
This method normalizes automatically (SLERP):
Q sin1Q sin 12
sin
R R 1R i12
Do quaternions help?
Q 1 Q Qi 1 2
Q 1Q 12
Qi
Forward Kinematics
•
Where is the end effector w.r.t. the “base” frame?
Composition of homogeneous transforms
Base to eff transform
3x 3y
l3 0012 l
TT T T 3123
q3 2 1y
2x Transform associated w/ link 3
Transform associated w/ link 2 Transform associated w/ link 1
1x q2
2y
q1
0y
0x 0z
l1
Forward kinematics: composition of homogeneous transforms
3x 3y
0012
TT T T 3123
c1 −s1 0 l1c1 0 T1= s1 c1 0 l1 s1
l3
0 0 1 0 2x q3 2
()l 0001 1y
1x q2
c2 −s2 0 l2c2
1 T2= s2 c2 0 l2 s2 2 y
0 y
()
0010
0001
q1
0x
l1
0z
Forward kinematics: composition of homogeneous transforms
3x c−s0lc 3y
0012
TT T T 3123
3333
l3
q l2
2 T3= s3 c3 0 l3 s3 0010
()x 0001
3
1y
2
1x q2
2y
q1
0y
0x 0z
l1
Remember those double-angle formulas…
sin sincoscossin cos coscossinsin
Forward kinematics: composition of homogeneous transforms
0012
TT T T 3123
c1 −s1 0T3=s1 c1
0 l1c1
c2 −s2 0 l2c2 s2 c2 0 l2s2
c3 −s3 s3 c3
0 l3c3
0 l3s3 ( )( )( )
0 l1s1 001000100010
000100010001 c123 −s123 0 l1 c1+l2 c12+l3 c123
0 T3= s123 c123 0 l1 s1+l2 s12+l3 s123 (001 0)
0001
Think-pair-share
2x
l2
2y
1y
1x q2
0y
q1
0x 0z
Calculate
l1
Think-pair-share
2x
l2
2y
1y
1x q2
0y
q1
0x 0z
l1
Calculate
Summary