82

Click here to load reader

Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Embed Size (px)

Citation preview

Page 1: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Chapter 2Rigid Motions and

Coordinate TransformationsDr. Amit Goradia

Page 4: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Forward Kinematics

• Given the various joint and link parameters, compute the end-effector (tool) location in task space.

• Given joint variables

– End-effector position and orientation, -

Formula?

),,,,,,( 654321 nqqqqqqqq

),,,,,( TAOzyxY x

y

z

Joint Frame

World Frame

Forward

Inverse

Page 5: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Inverse Kinematics

• Given a position in task space, compute the joint angles required to attain it.

• End effector position and orientation

• Formula for joint variables

x

y

z

),,,,,,( 654321 nqqqqqqqq

),,,,,( TAOzyx

Joint Frame

World Frame

Forward

Inverse

Page 7: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 2: Three Link Planer Robot

• Joint Coordinates

• Task Coordinates

• Forward Kinematics– Calculate Task

Coordinates in terms of Joint Coordinates

2l

1l

3l

1

321 ,,

)cos(

)cos()cos(

3213

21211

l

llx

)sin(

)sin()sin(

3213

21211

l

lly

)( 321

,, yx

2 3

Page 8: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Linear Algebra Concepts

• The set of Real numbers is represented by:

• The set of positive real numbers is formally defined as:

• is an Eucledian space of real dimension

• Geometric Objects– Points– Vectors– Transformations– Coordinate frames– Lines – Rays – Planes – Normals– etc.

R

,0: xRxR

nR

n

Page 10: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

1D Example: Time

• Point in time: class meeting time – class meets at 9:00 am

• Vector in time: class duration– class lasts 2 hours

• Operations: class at 2PM + class at 3PM ≠ class at 5PM !! 2 hour class + 3 hour class = 5 hours of classes class ends at 11AM – starts at 9AM = 3 hour class class starts at 9AM + lasts 2 hours = ends at 11AM

2 classes at 3PM ≠ one class at 6PM !! 2 classes last 3 hours = 6 hours of classes Class from 2PM to 10PM, half done at

1

22PM + 1

26PM  = 4PM

("affine combination")

Page 11: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

“Coordinate Systems” for Time

Knowing just the hour number doesn’t tell you everything…

• AM vs. PM (or use 8h00 vs 20h00)• Time zones: same point, many representations

– 10 (India) == 4:30 (UK)– to remove ambiguity, often use GMT (World frame)

• If always staying in local time zone, not important– if scheduling globally, must be careful.– convert from one time zone to another.

• Also, hours only good within one day– need to specify date & time– UNIX time: seconds since 01/01/1970, 00h00 GMT

• Notice: time durations are unaffected by all this!

Page 12: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Points and Vectors in Space

• Point describes a location in space– can subtract points to get a vector– can take weighted average of points to get a point

• Vector describes a displacement in space– has a magnitude and direction– can add, subtract, scale vectors– can add vector to a point to get a point– Has a dot product and cross product defined

• To represent them as three numbers, you must specify which coordinate system

Page 15: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Vectors: Dot Product

332211

3

2

1

321 bababa

b

b

b

aaababa T

2332211

2aaaaaaaaa T

)cos(baba

Think of the dot product as a matrix multiplication

The magnitude is the dot product of a vector with itself

The dot product is also related to the angle between the two vectors

Page 16: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Vectors: Cross Product

• The cross product of vectors A and B is a vector C which is perpendicular to A and B

• The magnitude of C is proportional to the sine of the angle between A and B

• The direction of C follows the right hand rule – this why we call it a “right-handed coordinate system”

)sin(baba

Page 17: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Arithmetic for Points and Vectors

w

u

v            vector+vector vector

v

w

u           vector-vector vector

us 

v                scale a vector

qp v           point+vector point

v q p          point-point vector

r 1

4p

3

4q    weighted average of points point

Page 19: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Coordinates

• If you have coordinate triples such as:

• Then, with frame such as you can construct a point or vector:

• Same coordinates, different frame different point or vector– Coordinates have no real meaning without a frame– vectors don’t depend on the origin of the frame, only the axes

3

6

1

    or     

9

2

5

p o 3x 6

y

z

v       9

x 2

y 5

z

o,x,y,z

Page 20: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Coordinates of a Frame

• Suppose you have a frame • In world coordinates, might have

• But in itself always have coordinates:

p,a,b,c

p 10

3

3

    a

0

1

1

    b

0.707

0

0.707

    c

0.707

0

0.707

p,a,b,c

p 0

0

0

    a

1

0

0

    b

0

1

0

    c

0

0

1

Page 21: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Coordinate equivalences

• Given a frame: – Can have a point with some coords:

– Can have a vector with same coords:

– Formally, we have:

– Informally, p and look and act about the same• People often sloppy, don’t distinguish point from vector• Can only get away with it if you stay in same frame!• And even then need to be careful…

Ox

y

z

v

p

b

ca

Tcbap

Tcbav

zyxo

opv

vop

v

Page 27: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Properties of Rotation Matrices• Basic Rotation Matrix

– Obtain the coordinate of from the coordinate of

– R is an orthogonal matrix (inverse equal to transpose)

uvwxyz RPP

wzvzuz

wyvyuy

wxvxux

kkjkik

kjjjij

kijiii

R

xyzuvw QPP

TRRQ 1

31 IRRRRQR T

uvwP xyzP

<== 3X3 identity matrix

z

y

x

w

v

u

p

p

p

p

p

p

zwywxw

zvyvxv

zuyuxu

kkjkik

kjjjij

kijiii

Dot products are commutative!

Page 28: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Properties of Rotation Matrices

• Columns of R are unit vectors and are mutually orthogonal

• Columns of R represent a vector in the UVW frame w.r.t. XYZ frame

• Rows of r are also unit vectors and represent a basis in XYZ frame wrt UVW

• • Space of all possible rotation matrices is

denoted as (special orthogonal group of order 3)

1)det( R

)3(SO

Page 31: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 3• A point is attached to a rotating frame,

the frame rotates 60 degree about the OZ axis of the reference frame. Find the coordinates of the point relative to the reference frame after the rotation.

)2,3,4(uvwa

2

964.4

598.0

2

3

4

100

05.0866.0

0866.05.0

)60,( uvwxyz azRota

Page 32: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 4• A point is the coordinate w.r.t. the

reference coordinate system, find the corresponding point w.r.t. the rotated OU-V-W coordinate system if it has been rotated 60 degree about OZ axis.

)2,3,4(xyza

uvwa

2

964.1

598.4

2

3

4

100

05.0866.0

0866.05.0

)60,( xyzT

uvw azRota

Page 34: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Composite Rotation Matrix• A sequence of finite rotations

– matrix multiplications do not commute– rules:

• if rotating coordinate O-U-V-W is rotating about principal axis of OXYZ frame, then Pre-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix

• if rotating coordinate OUVW is rotating about its own principal axes, then post-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix

Page 35: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 5• Find the rotation matrix for the following

operations:

Post-multiply if rotate about the OUVW axes

Pre-multiply if rotate about the OXYZ axes

...

axis OUabout Rotation

axisOW about Rotation

axis OYabout Rotation

Answer

SSSCCSCCSSCS

SCCCS

CSSSCCSCSSCC

CS

SCCS

SC

uRotwRotIyRotR

0

0

001

100

0

0

C0S-

010

S0C

),(),(),( 3

Page 38: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Orientation Representation Revisited

• Rotation matrix representation needs 9 elements to completely describe the orientation of a rotating rigid body.

• There are only 3 unique quantities in a rotation matrix • There are 6 redundant elements!!!!!!• Compact method to represent orientation?

Euler Angles Representation

wzvzuz

wyvyuy

wxvxux

kkjkik

kjjjij

kijiii

R

Page 39: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Orientation Representation

• Problems:– 6 extra elements need to be specified– Long series of computations with inherent

numerical errors causes R to loose orthogonal property. Need to orthogonalize from time to time

– Can calculate a rotation difference but canot smoothly interpolate over a difference.

• Cannot smoothly rotate from one orientation to another.

Page 40: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Orientation Representation• Euler Angles Representation ( , , )

– Many different types– Description of Euler angle representations

Euler Angle I Euler Angle II Roll-Pitch-Yaw

Sequence about OZ axis about OZ axis about OX axis

of about OU axis about OV axis about OY axis

Rotations about OW axis about OW axis about OZ axis

X convention Y convention Cardanian

Page 48: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Orientation Representation• Description of Roll Pitch Yaw

X

Y

Z`

– Description of Yaw,

Pitch, Roll• A rotation of about the

OX axis ( ) -- yaw• A rotation of about the

OY axis ( ) -- pitch• A rotation of about the

OZ axis ( ) -- roll

,xR

,yR

,zR

Page 50: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Valid Rotation Sequences

• 6 sets of Euler Angles– ZXZ, ZYZ, XZX, XYX, YZY, YXY

• 6 sets of Cardanian angles– XYZ, YZX, ZXY, XZY, ZYX, YXZ

• Euler Rotation Theorem:– In three-dimensional space, any displacement

of a rigid body such that a point on the rigid body remains fixed, is equivalent to a rotation about a fixed axis through that point.

Page 51: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Universality of Euler Angles

• Consider Euler 1 notation (X Convention)

• Remember that:– Rotation matrix represents moving frame

basis wrt fixed frame basis– Columns represent moving axes wrt fixed

axes– Rows represent fixed axes wrt moving axes

CCSSS

CSCCCSSCCSSC

SSCSCCSCSSCC

RRRR wuz '''

Page 52: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Universality of Euler Angles

• Show that:– Parameterization of R can represent any

proper orthonormal matrix– Columns of R can be transformed into any

arbitrary set of orthonormal vectors

• Proof:– Column 3 : – Representation of unit vector with spherical

angle coordinates– Can be made to coincide with any arbitrary

vector by choosing proper angles

TCCSSSe 3

Page 53: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Universality of Euler Angles• Column 1:

• Where,

• Now, u1 , u2 are unit vectors and

• So u1 and u2 span the plane perpendicular to e3 and we can choose phi to generate any angle in that plane

211 u sinu cos

SS

CCSSC

CSSCC

e

S

CC

CS

S

C

21 u ,

0

u

0uuuu 213231 ee

Page 54: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Universality of Euler Angles

• With choice of and , we can locate the Z axis of the new coordinate system arbitrarily

• With the choice of , we can locate the X axis perpendicular to Z and at any desired angle in that plane

• Choose Y to complete the triad

• Proof complete

Page 56: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Angle Axis Representation

• Define rotation axis r and an angle of rotation about r

• Results in a compact but non-minimal representation

• Procedure– Rotate by about axis Z till r is

coplaner with axis X– Rotate by about axis Y to align

r with Z– Rotate about Z by– Return r to original position by

rotating about Y by and about z by

• Result

r

XY

Z

Page 59: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Rotation Representation using Quaternions

• Quaternions were first introduced for complex number analysis

• Generalization of complex number representation

• 4 dimensional quantity– One scalar– A three tuple representing a vector.

• Avoid drawbacks of angle axis representation by– Distinguish between rotations of different signs– Avoid singularity at

3210 kqjqiqqQ

qqQ ,0

,0

Page 60: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

QuaternionsInvented by Hamilton in 1843:

Along the royal canal in Dublin…

Popularized for use in rigid motion representation incomputer graphics by Shoemake in 80’s

Increases computational efficiency as it requires less number of additions and multiplications for computing a given pose of a rigid body

Page 62: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Quaternions

123

22

21

20

2 qqqqQLet’s consider the set of

unit quaternions:

This is a four-dimensional hypersphere, i.e. the 3-sphere3S

Therefore, the inverse of a unit quaternion is:1* QQ

)0,1(,, 00* qqqqQQ

The identity quaternion is: 0,1Q

Consider:

Page 63: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Quaternions

You can rotate from frame a to b:Pa *ba

aba

b PQQP

Given a unit axis, , and an angle, :

Associate a rotation with a unit quaternion as follows:

2sinˆ,

2cos

kQkThe associated quaternion is:

Therefore, represents the same rotation asQ Q

Let be the quaternion associated with the vector pP ii ,0 pi

bacbca QQQ Composition:1 bacacb QQQInversion:

Page 65: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 7

0

02

1

kFind the difference between these two axis

angle rotations:

0

0

22k

0

0

,2

12

1caQ

0

0,2

1

21

baQ 2

144 cossin

0

0,

0

0

,2

1

21

21

211

bacacb QQQ

qppqqpqpqpQP 0000 ,

21

21

21

21

212

12

1

21

21 ,,

120cos2 211

cb

332

332

332

cbk

Page 66: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Quaternions: Interpolations

Suppose you’re given two rotations, and

How do you calculate intermediate rotations?

1R 2R

21 1 RRRi

Do quaternions help?

21

21

1

1

QQ

QQQi

Suprisingly, this actually works

• Finds a geodesic (shortest path between points in space)

Page 68: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Rigid Motions• position vector of P in {B} is transformed to position vector of P in {A}• description of {B} as seen from an observer in {A}•Rigid body motions are Affine transformations

Rotation of {B} with respect to {A}

Translation of the origin of {B} with respect to origin of {A}

Page 72: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Homogeneous Coordinates• Add an extra component. 1 for a point, 0 for a vector:

• Transform a point:– Top 3 rows are the affine transform– Bottom row stays 1

• Transform a vector– Top 3 rows are the linear transform– Bottom row stays 0

Tzyx pppp 1 Tzyx vvvv 0

rRpp

p

p

rR

p

p

p

z

y

x

z

y

x

1

10

1

'

'

'

vRv

v

v

rR

v

v

v

z

y

x

z

y

x

0

10

0

'

'

'

Page 73: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Homogeneous Arithmetic

• Legal operations always end in 0 or 1!

vector+vector:       0

0

0

vector-vector:       0

0

0

scalar*vector:               s0

0

point+vector:         1

0

1

point-point:         1

1

0

point+point:        1

1

2

scalar*point:                s1

s

weighted average

affine combination

of points:   1

3

1

2

3

1

1

Page 77: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 10• Find the homogeneous transformation matrix

(T) for the following operations:

:

axis OZabout ofRotation

axis OZ along d ofn Translatio

axis OX along a ofn Translatio

axis OXabout Rotation

Answer

44,,,, ITTTTT xaxdzz

1000

00

00

0001

1000

0100

0010

001

1000

100

0010

0001

1000

0100

00

00

CS

SC

a

d

CS

SC

Page 80: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Homogeneous Transformation

21

10

20 AAA

Composite Homogeneous Transformation Matrix

0x

0z

0y

10 A

21A

1x

1z

1y 2x

2z2y

?i

i A1 Transformation matrix for adjacent coordinate frames

Chain product of successive coordinate transformation matrices

Page 81: Chapter 2 Rigid Motions and Coordinate Transformations Dr. Amit Goradia

Example 11• For the figure shown below, find the 4x4 homogeneous transformation

matrices and for i=1, 2, 3, 4, 5

1000zzzz

yyyy

xxxx

pasn

pasn

pasn

F

ii A1

iA0

0x 0y

0z

a

b

c

d

e

1x

1y

1z

2z2x

2y

3y3x

3z

4z

4y4x

5x5y

5z

1000

010

100

0001

10

da

ceA

1000

0100

001

010

20 ce

b

A

1000

0001

100

010

21 da

b

A

Can you find the answer by observation based on the geometric interpretation of homogeneous transformation matrix?