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: ab axbx ayby
Under what conditions is the dot product zero?
b aˆ


Orthogonal vectors
First, define the dot product: ab axbx ayby
Under what conditions is the dot product zero? ab0 when: a0
b0 cos0
b aˆ
or, or,


Orthogonal vectors
First, define the dot product: ab axbx ayby
Under what conditions is the dot product zero? ab0 when: a0
b aˆ
b0 cos0
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
abaxbx ayby
b

l
aˆ

Same point – different reference frames
aˆaab xbppcospx
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 pyybp
2
B-frame’s y axis written in A frame
ap
5 3.8
a yˆ b 3.8
baˆa pxxbp
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
pyyBp
5 3.8
BAˆA
pxxBp 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
RRR 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 pRB p dB
Aˆ x
Bˆ x

Homogeneous transform
By y
p
AABA A pRB 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
SO3RR33 |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 Rp p  1212

The space of rotations
SO3RR33 |RRT I,det(R)1
Special orthogonal group(3): Why it’s a group:
Closed under multiplication: if R , R  SO3 then R R  SO3 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 rotateabout 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 rotateabout 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  SO3 , is equivalent to a rotation
aboutafixedaxis, R3,throughanangle[0,2) (also called exponential coordinates)
Axis:
Converting to a rotation matrix:
Rk e
ISksinSk1cos
k = (y)
kx k
Angle:

kz
Sk 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 180then R R kk

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
kk 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
kk 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 Qq0,q
Properties of complex dimensions:
iijjkkijk1 jkkji ij jik kiikj
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,qq0,qq0q0  q2,q0q q0qqq(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: bPQ 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 sin1Q sin 12
sin 
R R 1R i12
Do quaternions help?
Q 1 Q Qi 1 2
Q 1Q 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
TT 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
TT 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
TT 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 sincoscossin cos coscossinsin

Forward kinematics: composition of homogeneous transforms
0012
TT 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