Ch1a [Compatibility Mode] (1)

8/2/2019 Ch1a [Compatibility Mode] (1)

1/80

Chapter 1: Kinematics

Chew Chee Meng


2/80

2

What you would learn Definitions of positions and orientations of rigid

bodies Analysis of different representations for

orientation

Transformation of coordinates Determination of new positions and

orientations after a sequence of rigid bodymotions

Kinematics modeling of robotic manipulators

Forward and inverse kinematics of position


3/80

3

Spatial descriptions and Transformations

Need to specify spatial attributes of

various objects with which a manipulationsystem deals

Cartesian coordinate frames are used

Where is the

cup?


4/80

4

Spatial descriptions and Transformations

Position & orientation of a Rigid Body

Position Orientation

Location of rigid body


5/80

5


Position: Attribute of a point

Represented

by a position

vector

With reference

to a coordinateframe

3

z

y

x

A

p

p

p

P

xA

zA

yA{A}

AP


6/80

6


Orientation: Attribute of a body

Represented by Matrices

(Rotation Matrices)

RA

B = [AxB

AyBAzB ]

3x3

A rigid body

body frame

Reference frame

xA

zA

yA

{A} xB

zB yB{B}

By attaching a coordinate

frame to the body and

then give a description of

this coordinate framerelative to the reference

coordinate frame


7/80

7


Orientation representation

RA

B = [AxB

AyBAzB ]

3x3

Orthogonal unit vectors(orthonormal matrix)

1)det( RAB Multiplication of rotation matricesgenerally not commutativei.e. R1R2R2R1

xA

zA

yA{A} xB

zB yB{B}

Note: AxB means xB expressed in {A}


8/80

8



RA

B = [AxB

AyBAzB ]

ABABAB

ABABAB

ABABAB

zzzyzx

yzyyyx

xzxyxx

Remark:

Components are direction cosines

(dot product of two unit vectors yields the cosine of the angle

between them)

Choice of frame is arbitrary for the vectors, but must be the same

xA

zA

yA

{A} xB

zByB{B}


9/80

9


Orientation

RA

B = [AxB

AyBAzB ]

B T

AT

B T B B B B T

A A A A A

B TA

R

x

y x y z

z

Note: Rows of the matrix are the unit

vectors of {A} expressed in {B}

ABABAB

ABABAB

ABABAB

zzzyzx

yzyyyx

xzxyxxxA

zA

yA

{A} xB

zB

yB{B}

transpose of zA expressed in {B}


10/80

10


Orientation

RB

A

TA

BR

3100

010

001

IRR BA

B

A

B

A

T

B

A

T

B

A

T

B

A

A

B

TA

B

zyx

z

y

x

Hence, description of frame {A} relative to {B},

This suggests:

Verification:

TA

B

A

B RR 1

I3 denotes the 3x3 identity matrix

Linear algrebra: inverse of a matrix with orthonormal columns is

equal to its transpose.


11/80

11



xA

zA

yA{A} xB

zB yB{B}Uniqueness

3 independent

parameters

333231

232221

131211

rrr

rrr

rrr

R


12/80

12


Elementary (Basic, Fundamental) Rotation

matrices

xAo

zAo

yA1

zA1

Rotation about xAo by angle

cossin0

sin-cos0

001

)(RR XA0

1

A

yAo

, xA1


13/80

13


Elementary (Basic, Fundamental) Rotation

matrices

xAo

zAo

,yA1

zA1

Rotation about yAo by angle

yAo

cos0sin-

010

sin0cos

)(RR YA0

1

A

xA1


14/80


15/80

15


Description of a frame

A set offour vectors givingposition (typically of the origin) and

orientation information of the frame

relative to the reference frame

xA

zA

yA

{A}xB

zB yB{B}

ApBORG

BORGAABR p,

E.g., Frame {B} relative to frame {A} is described by

Once we have attached a frame to a

rigid body, how to represent the

frame location and orientation?


16/80

16

Coordinate Transformations (Mappings)

Mapping: Changing descriptions from frame to

frame Expressing the position vector of a point in

space in terms of various reference coordinate

systems

xA

zA

yA{A}

xB

zByB

p

{B}? B PGiven

FindA

P


17/80

17


Mappings involving translated frames (i.e.,

frames having same orientations)

AP = BP + APBORG

Only applicable when orientations of frames A and B are the same


18/80

18


Mappings involving rotated frames

Above equation assumes origins of Frames A & B are coincident.

What if the two origins are not coincident?

PRPBA

B

A

Coordinates of P in B

Coordinates of P in A

P


19/80

19


Example 1-1: Figure 1-1 shows a frame {B} which is

rotated relative to frame {A} about z by 30 degrees.Here, z is pointing out of the page. There is a point Pwhose position vector expressed in {B} is Bp = [0 2 0]T.What is the position vector of the point P expressed in

{A}?

P

xA

xB

yAyB

30oFigure 1-1


20/80

20


Solution:

Writing the unit vectors of {B} in terms of {A} and stacking

them as columns of the rotation matrix we obtain:

100

0866.05.0

05.0866.0

RA

B

GivenB

p = [0 2 0]T

,

0

732.1

1

ppBA

B

AR

P

xA

xB

yAyB

30o


21/80

21


Solution:

100

0866.05.0

05.0866.0

RA

B

P

xA

xB

yAyB

30o

Remark:

RA

B acts as a mapping which isused to describe p relative to frame{A} given Bp. The original vectorp is notchanged in space. We simply

compute the new description of thevector relative to a new frame.


22/80

22


Mappings involving general frames

xA

zA

yA

{A}

xB

zByB

p

{B}

?B PGiven

FindA

P


23/80

23



xA

zA

yA

xA

zA

yA

{A}

xB

zByB

p

{B}

A A A B A A B

BORG B BORG BP P R P P R P

PRP BABA PPP ABORGAA and

3' IRA

A

{A} and {A} same

orientation:

RR ABA

B


24/80

24



PRPP BABBORGAA

Translational

transformation

Rotational

transformation

xA

zA

yA

xA

zA

yA

{A}

xB

zByB

p

{B}


25/80

25

Homogeneous Transformation

xA

zA

yA

{A}

xB

zByB

p

{B}

PRPPBA

BBORG

AA

PTPBA

B

A Homogeneous transformation


26/80

26


1

z

A

yA

x

A

A

p

p

p

P

1

z

B

y

Bx

B

B

p

p

p

P

10BORG

AA

BAB PRT

Homogeneoustransformation matrix

Augmented vector

4x4 matrix


27/80

27

Homogeneous Transformation Example 1-2

Figure 1-2 shows a frame {B} which is rotated relative to

frame {A} about z by 30 degrees, and translated 10 units inxA , and 5 units in yA. Find

Ap where Bp = [3 7 0]T.

Figure 1-2


28/80

28

Homogeneous Transformation Solution:

1000

0100

50866.05.0

1005.0866.0

TA

B

0

562.12

098.9

pAThat is,

1

0

562.12098.9

1

0

73

1000

0100

50866.05.01005.0866.0

pp BABA T


29/80

29


Interpretation 1:

Trepresents coordinate transformations in compactform

xA

zA

yA{A}

xB

zByB

p

{B}

10

BORG

AA

BA

B

PRT


30/80

30


Interpretation 2:

Trepresents position & orientation of the coordinateframe {B} relative to {A}

xA

zA

yA{A}

xB

zB yB

{B}

10

BORG

AA

BA

B

PRT


31/80

31


Interpretation 3:

Trepresents rotation and translation of the coordinateframe {A} to {B}

xA

zA

yA{A}

10

BORG

AA

BA

B

PRT

xB

zByB

{B}


32/80

32


For free vectors (e.g. force, velocity, etc),

augment the vectors with 0 rather than 1:

0

z

A

yA

x

A

A

p

p

p

P


33/80

33


Consecutive transformation:

x0

z0

y0{0}

x1

z1 y1

{1}

xn

znyn

{n}

p

n PGiven

Find0

P

xn-1

zn-1

yn-1{n-1}

Ai

i

1 homogeneous transformationmatrix from frame ito frame i-1

(i= 1,,n)

augmented position vector of P

expressed in frame n


expressed in frame 0


34/80

34


Consecutive transformation:

0 0 1 1 0

1 2

n n n

n nP A A A P A P

Ai

i

1homogeneous transformation

matrix from frame ito frame i-1(i= 1,,n)

Pn

Po


expressed in frame n


expressed in frame 0

0 1 1 0

1 2

n

n n A A A A homogeneous transformation

from frame n to frame 0

where


35/80

35


Inverse of a Homogeneous Transformation

Matrix

TAB TB

A

1T

A

BGiven , find or

ITTA

B

A

B 1

3 3

1 0 0 0

00 1 0 0

0 1 0 10 1 0 0 1 0

0 0 0 1

A A B BORG X Y IR P

IRXAB RRRXB

ATA

BA

B 1

0 BORGAA

B PRYBORG

ATA

BBORG

AA

B PRPRY 1

By definition,

=>

=>

i.e.


36/80

36


Inverse of a Homogeneous Transformation

Matrix

1

0 1

A T A T A

B B BORGB AA B R R PT T


37/80

37

Homogeneous Transformation Example 1-4

Figure 1-5 shows a frame {B} which is rotated relative to frame {A}

about z by 30 degrees, and translated four units in xA, and threeunits in yA. Find .T

B

A

Figure 1-5


38/80

38

Homogeneous Transformation Solution:

1000

0100

30866.05.0

405.0866.0

TA

B

1000

0100

598.00866.05.0

964.405.0866.0

11

0

pBORG

ATA

B

TA

BAB

BA

RRTT

E l 1 5


39/80

39

Example 1-5

Assume we know TBT in Figure 1-6, which describes the frame at the

manipulators fingertips {T} relative to the base of the manipulator, {B}. Also,

we know where the tabletop is located in space relative to the manipulators

base because we have a description of the frame {S} which is attached to the

table as shown, TB

S . Finally, we know the location of the frame attached to the

bolt lying on the table relative to the table frame, that is, TSG . Calculate the

position and orientation of the bolt relative to the manipulators hand, TT

G .

Figure 1-6


40/80

40

Solution:

1T T B S B B S

G B S G T S GT T T T T T T


41/80

41

Operators: Translations, Rotations, and

Transformation

Operators :

Another interpretation of the mathematicalforms used for coordinate transformations

Only one coordinate frame is involved


42/80

42


Transformation

Translational operators: Moves a point in space afinite distance along a given vector direction

xA

zA

yA{A}

AP1 AP2

AQ

How a vectorAP1 is translated by a vectorAQ :

Result of the operation is a new vectorAP2 =AP1 +AQ

12

1000

100

010

001

Pq

q

q

PA

z

y

x

A

To write the translation operation as a matrix operator,

where qx, qy and qz are components of the translation vector Q

x

y

z

q

q

q


43/80

43


Transformation

Rotational operators:

Operates on a vectorAP1

and changes that vector to a new

vectorAP2, by means of a rotation, R (about origin)

Same as the rotation matrix that describes a frame rotated byR relative to the reference frame

Define a rotational operatorRK():

AP2 = RK()AP1

- Kis the axis of rotation- (in degrees) is the amount of

rotation about K


44/80

44


Transformation

Rotational operators:

10000100

00cossin

00sincos

)(

ZR

E.g. operator that rotates about the Zaxis by can be written as:

100

0cossin

0sincos

)(

Z

R

homogeneous transformation matrix3x3 rotation matrix


45/80

45


Transformation Example 1-6

Figure 1-3 shows a vectorAp1 = [0 2 0]

T. We wish to compute the vector

obtained by rotating this vector about z by 30 degrees. Call the new vectorAp2.

Figure 1-3


46/80

46


Transformation

Solution:

Note: Rotation matrix which rotates vectors by 30 degrees about z =Rotation matrix which describes a frame rotated 30 degrees about zrelative to the reference frame.

100

0866.05.0

05.0866.0

)0.30(zR

0

732.1

1

0

2

0

100

0866.05.0

05.0866.0

)0.30( 12 pp zAA

R


47/80

47


Transformation

Transformation operators

Rotate and translate

Define operatorT to be one which rotates and translates a vectorAP1 to a new vector

AP2:

AP2= TAP1

(1-3)

The transform that rotates by R and translates by Q is thesame as the transform that describes a frame rotated by R

and translated by Q relative to the reference frame.


48/80

48


Transformation

Example 1-7

Figure 1-4 shows a vectorAp1. We wish to rotate it about z by 30degrees, and translate it 10 units in xA, and 5 units in yA. Find

Ap2 whereAp1 = [3 7 0]

T.

O T l i R i d


49/80

49


Transformation

Solution:

The operator T, which performs the rotation and translation, is

1000

0100

50866.05.0

1005.0866.0

T

10

562.12

098.9

10

7

3

10000100

50866.05.0

1005.0866.0

12 ppAA

T

Note that this example is numerically similar to Example 1-2, but the

interpretation is different.


50/80

50

Other Orientation Representation

Orientation of rigid body represented by 3x3

rotation matrix R

9 elements Subject to:

orthogonalityconditions (3 equations) and

unit length conditions (3 equations)=> only 3 of 9 elements are independent (ie. there is

redundancy in R)

Different representations of orientation whichrequires only three or four parameters


51/80

51


X-Y-Z fixed angles

Each of the three rotations takes place about an axis in the

fixed reference frame, {A}

X Y Z(roll) (pitch) (yaw)


52/80

52


X-Y-Z fixed angles

X

ZYAW

PITCH

ROLL

Y


53/80

53


X-Y-Z fixed angles

( , , ) ( ) ( ) ( )

0 0 1 0 0

0 0 1 0 0

0 0 1 0 0

A

B XYZ Z Y X R R R R

c s c s

s c c s

s c s c

c c c s s s c c s c s s

s c s s s c c s s c c s

s c s c c

where cand s denote cosine and sine functions, respectively


54/80

54


X-Y-Z fixed angles

Inverse transformation

Given

333231

232221

131211

rrr

rrr

rrr

RAB , find , ,

Nine equations (6 dependencies) and three unknowns

11 12 13

21 22 23

31 32 33

r r r c c c s s s c c s c s s

r r r s c s s s c c s s c c s

r r r s c s c c


55/80

55


X-Y-Z fixed angles


11 12 13

21 22 23

31 32 33


r r r s c s s s c c s s c c sr r r s c s c c

Remark:

Atan2(y,x) computes tan-1(y/x) and uses signs of both x and y to

determine the quadrant in which the angle lies

)1,(2tan

),(2tan

2

3131 rrA

csA


56/80

56


X-Y-Z fixed angles


21 11

tan 2( , )

tan 2( , )

A s cr r

Ac c

11 12 13

21 22 23

31 32 33




57/80

57


X-Y-Z fixed angles


11 12 13

21 22 23

31 32 33



32 33

tan 2( , )

tan 2( , )

A s c

r rA

c c


58/80

58


X-Y-Z fixed angles


),(2tan2

21

2

1131 rrrA 32 3321 11tan 2( , ) tan 2( , )

r rr rA A

c c c c

Remark:

2 solutions in general If |r31|=1 (implies r11 = r21 = r32 = r33= 0), the solution degenerates(mathematical singularity) such that only the difference of and may be computed:

If r31 = +1: => + = Atan2 (-r23 , r22)

If r31 = -1: => - = Atan2 (r23 , r22)

12 13

22 23

31

0

0

0 0

r r c c c s s s c c s c s s

r r s c s s s c c s s c c s

r s c s c c

0tan 2( 1,0) 90A 0

tan 2(1,0) 90A


59/80

59


Z-Y-X Euler angles

Each rotation is performed about an axis of the movingframe {B},

rather than the fixed reference frame, {A}

ZY X


60/80

60


Z-Y-X Euler angles

RRRRB

B

B

B

A

B

A

B

( , , ) ( ) ( ) ( )

0 0 1 0 00 0 1 0 0

0 0 1 0 0

A

B Z Y X Z Y X R R R R

c s c ss c c s

s c s c

c c c s s s c c s c s ss c s s s c c s s c c s

s c s c c

Note:

Same as the result obtained for the same three rotations taken in theopposite order about fixed axes.

Recall:Consecutive

transformation

O O


61/80

61


Four-parameter representations for

orientationEquivalent angle-axis

Euler parameters or Unit Quaternion Nonminimal representations of

orientation

Oth O i t ti R t ti


62/80

62


Equivalent angle-axis

Anyrotation matrix can berepresented by choosing the

properaxis and angle (Eulers

theorem on rotation)

quadruple of ordered real

parameters consisting ofone

scalar (angle of rotation) and one

unitvector (axis of rotation)(according to right-handrule)

K

AK

Oth O i t ti R t ti


63/80

63



)( KAR

cvkkskvkksk-vkk

sk-vkkcvkkskvkk

skvkksk-vkkcvkk

zzxzyyzx

xyzyyzyx

yxzzxyxx

=

where v = (1 - cos), ,T

A

x y zK k k k , sign of is determined by right-hand rule

Remark:

Forsmallangular rotations, axis of rotation becomes ill-defined.

Non-unique representation: )()( KK RR

(1-4)

1 (unit vector)AK

Oth O i t ti R t ti


64/80

64


Example 1-8

A frame {B} is described as follow: initiallycoincident with {A} we rotate {B} about the vectorAk = [0.707 0.707 0]T (passing through the origin)

by an amount = 30 degrees. Give the framedescription of {B} with reference to {A} after therotation.



65/80

65


Solution:

x x y x z z x y

x y z y y z y x

x z y y z x z z

k k v30 c30 k k v30 - k s30 k k v30 k s3030 k k v30 k s30 k k v30 c30 k k v30 - k s30

k k v30 - k s30 k k v30 k s30 k k v30 c30

o o o o o o

A o o o o o o o

o o o o o o

R

k

where v30o = 1-cos 300;

Since there is no translation of the origin,

0 0.933 0.067 0.354 0

30 0 0.067 0.933 0.354 0

0 0.354 0.354 0.866 0

0 0 0 1 0 0 0 1

A o

A

B

RT

k

k = [kx, ky, kz]T =[0.707 0.707 0]T



66/80

66


Example 1-9

A frame {B} is described as follows: initially coincident with {A}, we rotate

{B} about the vector

TA

]0.0707.0707.0[k , passing through the point TA P 321 , by o30 . Give the frame description of {B} with reference to{A}.

Solution:

Before performing the rotation, {A} and {B} are coincident. We define

two new frames, {A} and {B} which are obtained by translating {A} and

{B}, respectively, to point P. {B} and {B} can be treated as if they aremounted on the same rigid body, whereas {A} and {A} are fixed in the

space.



67/80

67


Solution:

1000

3100

2010

1001

'TA

A

1000

3100

2010

1001

TBB



68/80

68


Solution:

Note that the point P is on the axis of rotation. Now, lets

rotate {B} relative to {A}. This is a rotation about an axiswhich passes through the origin, so we may use

cvkkskvkksk-vkk

sk-vkkcvkkskvkk

skvkksk-vkkcvkk

zzxzyyzx

xyzyyzyx

yxzzxyxx

k

RA

to compute {B} relative to {A} after the rotation.

1000

0866.0354.0354.0

0354.0933.0067.0

0354.0067.0933.0

1000

0

0

0

30oAAB

RT k



69/80

69


Solution:

1000

05.0866.0354.0354.0

13.1354.0933.0067.0

13.1354.0067.0933.0

TTTT BBA

B

A

A

A

B

Finally,

Note: After rotating about the axis k , {B}becomes separated from {A} as shown in Fig.

1-9.

Figure 1-9



70/80

70



Inverse Problem

RAB AK

x x x

A

B y y y

z z z

x x y x z z x y

x y z y y z y x

x z y y z x

n o a

Let R n o a

n o a

k k v c k k v - k s k k v k s

k k v k s k k v c k k v - k s

k k v - k s k k v k s

z zk k v c

given

Given Find ,

where v = (1 - cos)



71/80

71



Inverse Problem

RA

B

RA

B

From the diagonal elements,

Trace ( ) = 1 + 2cos

=> cos = ( Trace ( ) 1 )

(Two solutions for = A)+A

-A

x x y x z z x yx x x

y y y x y z y y z y x

z z z x z y y z x z z

k k v c k k v - k s k k v k sn o a

n o a k k v k s k k v c k k v - k s

n o a k k v - k s k k v k s k k

A

KR

v c

where v = (1 - cos)



72/80

72



Inverse Problem

x x y x z z x yx x x

y y y x y z y y z y x

z z z x z y y z x z z

k k v c k k v - k s k k v k sn o a

n o a k k v k s k k v c k k v - k s

n o a k k v

- k s

k k v

k s

k k

A

KR

v

c

sin2

n-ak zxy

sin2

a-ok

yz

x

sin2

o-n

k

xy

z

If sin 0 :From : nz & ax elements

From : oz & ay elements

From : ny & ox elements



73/80

73



Inverse Problem

AK

In general:

2 solutions

(1)

AK

(2)



74/80

74



Inverse Problem

100

010

001

RAB Identity (null rotation)

2

zzyzx

zy

2

yyx

zxyx

2

x

A

B

2k1-k2kk2k

k2k2k1-k2k

k2kk2k2k1-

R

If = 0

If = 180

( symmetric )

K = any axis (infinite solutions)



75/80

75



Inverse Problem

x

zz

x

y

y

z

yz

x

2k

ak

2k

nk

2o

nnk

2 solutions for

(If one solution is ,

the other is )

2

x x y x zx x x

A 2

B y y y x y y y z

2z z z x z y z z

-1 2k 2k k 2k k n o a

R n o a = 2k k -1 2k 2k k

n o a 2k k 2k k -1 2k

If = 180 (cont)

K

1K

1K



76/80

76

Ot e O e tat o ep ese tat o


Inverse Problem

RA

B

Ifall off-diagonal terms are 0, one of kx, ky or kz = 1 &

the rest are 0s. The component of that is 1 has avalue of 1 in the diagonal of .

E.g. kx = 1, ky = kz = 0

A

B

1 0 0

R 0 -1 0

0 0 -1

If = 180 (cont)

K



77/80

77

p

Euler parameters (Unit Quaternion)

Able to solve the non-uniqueness problem

encountered by angle/axis representation

2sin1

xk

2sin2

yk

2sin3

zk

2cos4

K

Euler parameters:

where kx, ky and kz are the coordinates of the equivalent axis

and is the equivalent angle

4 parameters not

independent because:

1242

3

2

2

2

1



78/80

78

p


Based on Eq ( 1-4) (equivalent angle-axis) and the definition of unit

quaternion:

1)2(2)(2)(2

)(21)(2)(2

)(2)(21)(2

2

3

2

441324231

4132

2

2

2

44321

42314321

2

1

2

4

R (1-5)

x x y x z z x y

x y z y y z y x

x z y y z x z z

k k v c k k v - k s k k v k s

k k v k s k k v c k k v - k s

k k v - k s k k v k s k k v c

A

B R

1

sin

2

xk

2

sin

2

yk

3

sin

2

zk

2 22 4cos 2cos 1 2 1

2 22sin 1 cos , etc



79/80

79

p


Inverse Problem

333231

232221

131211

rrr

rrr

rrr

RAB

1)sgn(2

133221123321 rrrrr

1)sgn(2

111332231132 rrrrr

1)sgn(2

122113312213 rrrrr

3322114 12

1rrr

Given , find i

Note: sgn(x) = 1 for x 0 and sgn(x) = -1 for x < 0

2 2

4 1 1 2 3 4 1 3 2 4

2 2

1 2 3 4 4 2 2 3 1 4

2 2

1 3 2 4 2 3 1 4 4 3

2( ) 1 2( ) 2( )

2( ) 2( ) 1 2( )

2( ) 2( ) 2( ) 1

R



80/80

80

p


Inverse Problem

1)sgn(2

133221123321 rrrrr

1)sgn(2

111332231132 rrrrr

1)sgn(2

122113312213 rrrrr

3322114 12

1rrr

Note:

Implicitly assumed (by considering [-,])04

No singularity occurs (compared to angle/axis representation)

Ch1a [Compatibility Mode] (1)

Documents

Transcript of Ch1a [Compatibility Mode] (1)