[SOLVED] 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
yb
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
xb Point p
Frame b
5
Same point, two different reference frames

Representing Position: vectors
yb Note that I am denoting the axes as
Frame a
orthogonal unit basis vectors
This means perpendicular
xb yb
2
b
5
xb 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
yb
err
b
The eff needs to make a Cartesian displacement of this much to reach the object
xobject y eff
eff
xb
xeff

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
aaab xbppcospx
B-frames y axis written in A frame
ap
5 3.8
a x b
a y b 3.8
2

B-frames x axis written in A frame

2
B-frames 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
baa pyybp
2
B-frames y axis written in A frame
ap
5 3.8
a y b 3.8
baa pxxbp
a x b
B-frames x axis written in A frame

Same point different reference frames
A y B 3.8
2 BAA
p
pyyBp
5 3.8
BAA
pxxBp xB
A

Same point different reference frames
Where:
Rotation matrix
or

Rotation matrices
A rotation matrix is a 22 or 33 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 22 or 33 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 22 or 33 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 by

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 c2s1 s2 sin(1) cos(1) sin(2) cos(2) s1c2+c1 s2
=(c12 s12) s12 c12
c1 s2s1 c2) c1c2s1 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. Whats the magnitude of this rotation?

A
y
A AAA RB xB yB zB
Example: 3D rotation matrix
Bz
Ax
45
Az
101
Bx 22
ARB=(01 0) 1 01
2 2
By

Example: 3D rotation matrix
za
y a

xc xb
c s 0 aRb=s c 0
xa
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
za
y a

xc xb
xa
cc s cs
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 theyre 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 Whats BTA?

Example 1: homogeneous transforms
BAy
Whats 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 l2c(+4)
Example 2: homogeneous transforms
l l

aT =s 0 c l2s(+) 010 0
ya
b[ 4] 0001
This arm rotates about the zaaxis. xa
za
Whats
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 cc sc s
aR=aRbR=010sc0=s c0 cbc()()( )
s 0 c
Calculate and
0 0 1 sc ss 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 its 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 its 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 01sin0cos0 01
cccss ccssc cs 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 ()
cossin 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|=cos1(trace(R)1) 2
Where:
r11 r12 r13 R=r r r
and:
trace(R)r r r 11 22 33
r32r23 r r
1
2sin(13 31)
k^ =
r21r12
(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
r1r2=(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()= 12
2 2=0 01
)( )
(
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
=cos1(
)=cos1( )=2 2 3
k^=
1 r32r23 1 1 21 r13r31 = (1 ) k=3 1
( )( )( )
0 1 0 1 0 0 1
0 1 0
2
2sin( )31 3() r21r12 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 33 matrices to no longer be orthogonal (you need to orthogonalize them from time to time).
Although you can accurately calculate rotation differences, you cant 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
Lets 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 11
22
0
=(0,(01 )+(0 1 )=(0,(0 )) 22 1

Example: Quaternions
Find the difference between these two axis angle rotations:
sin()=cos()=1 4 4 2
QP=(p0 q0pq , p0 q+q0 p+pq) 01
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 111
2 2 3 =1,1 1 =1,(1) 1
cb=cos1(1)=2 23
2 2(2 ) 2 2 k =
1 (1)() cb 3
() 2 2 1
3

Quaternions: Interpolation
Suppose youre 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 cs0lc 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