1

Overview of the course

• Forward Kinematics (given theta, find end-point)

• Inverse Kinematics (given end-point find theta)

• Dynamics (given input torques find theta as a function of time)

• Control (design torque to follow a given end-point trajectory)

1

2

),( ee yx

Mon 30 July 2007

2

A Note on Control )(),()( qgqqqcqqD

Control Problem - Given q(t) find appropriate tau

Two solutions:

•Feedforward control – calculate tau from the above equation (since LHS is known)

•Feedback control – let tau = f(q desired – q measured) – control design means how to find the function f.

When to use feedforward control and when to use feedback control?

Stability, optimality, robustness, etc., several issues to be considered for practical control

Robot dynamical model

3

0x

0y

0z

0o

1x

1y

1z

1o

p1

1

00 pRp

Representation in frame 0

Representation in frame 1

Rotation matrix from frame 1 to frame 0

0

0

11 pRp

10

1

0

1

1

0

RRR T

010101

010101

010101

1

0

...

...

...

kkkjki

jkjjji

ikijii

R

101010

101010

101010

0

1

...

...

...

kkkjki

jkjjji

ikijii

R

4

0x

0y

1x1y

100

02

12

1

02

12

1

1

0R

045

100

02

12

1

02

12

1

0

1R

Example

10 , zz

5

Exercise

,

,

,

z

y

x

R

R

RFill in

0x

0y

0z

0o

6

Rotation Matrix

• Rotation matrix columns are of unit length and perpendicular to each other (proof needed).

• Columns are unit vectors of new x, y, and z axes expressed in the old frame.

• Determinant of the rotation matrix is plus or minus one (proof needed).

• SO(3) – set of all 3 x 3 rotation matrices

7

Composition of Rotations

1

1

00 pRp

2

2

11 pRp

2

2

1

1

00 pRRp

2

1

1

0

2

0 RRR

also

1) Phi deg about the current y-axis (i.e., y0)

2) Theta deg about the current z-axis (i.e., z1)

1) Theta deg about the current z-axis (i.e., z0)

2) Phi deg about the current y-axis (i.e., y1)

0x

0y

0z

8

About Fixed Axes

• For example, phi about y0 and then rotate theta about z0 (not about z1 as in the previous example).

• Is the composition rotation matrix

R = R(y,phi) R(z,theta) ?

9

About fixed axes

iy

Phi about y0 Theta about z0

Red – frame 0 Blue – frame 1 Green – frame 2

We want a rotation matrix from frame 2 to frame 0.

phi

theta

ix

iz

10

phi

thetadashed frame is frame 0 rotated by theta about z0

Rotation matrix from green frame to the dashed frame is the same as from blue frame to red frame which is R(y,phi).

Rotation matrix from dashed frame to red frame is R(z,theta)

Finally the rotation matrix from green frame to red frame is: (first green to dashed and then dashed to red)

R(z,theta) R(y,phi)

Pre-multiply for rotation about fixed axes

Post-multiply for rotation about current axes

11

2,,0 pRRp yz

0z

0y

k

0x

p

2z

1x

kzy pRRp ,,3

kzyzyz pRRRRRp ,,,,,0

,,,,,, zyzyzk RRRRRR

Frame 3 from frame 0 by

first R(z,alpha) to align x0 with x-y plane projection of k-axis – frame 1then R(y,beta) to align z1 with the k-axis – frame 2then R(z,theta) to obtain frame 3.

vector p in frame i is written as frame k is frame 0 after theta rotation about k-axis

3,2 pRp z

Rotation matrix from frame 2 to frame 0 is the same as from frame 3 to frame k.

ip

See Similarity Transform (Section 2.3.1 p47)

,, zk RRRR

12

R(k,theta)

• Nine elements but only three independent parameters – what are they?

• Any R in SO(3) can be written as R(k,theta) where

theta = acos(trace(R) – 1)/2 and

k = (1/2 sin(theta))[r32-r23; r13-r31; r21-r12]

Representation is non-unique

13

)cos1)((sin)( 2

, kSkSIRk

)(

,

ks

k eR

z

y

x

k

k

k

k

0

0

0

)(

xy

xz

yz

kk

kk

kk

kS

S(k) is a skew symmetric matrix

0)()( kSkS T

? isWhat ,

d

dRk

? isWhat S(a)a

z

y

x

a

a

a

a

14

Rigid body rotation

• Euler angles – {theta, phi, psi}-Rotations about current axes R = R(z,phi) R(y,theta) R(z,psi)

• Roll, Pitch, and Yaw Angles – Rotations about fixed-axes

15

Homogeneous Transformation

0y

1z

1y

1x

0x

0z

1

0d

0o

1o

1

01

1

00 dpRp 2

12

2

11 dpRp

1

0

2

1

1

0

2

0

2

1

1

0

2

0

2

02

2

00

1

0

2

1

1

02

2

1

1

00

and ddRdRRR

dpRp

ddRpRRp

Can we do this operation using matrices?

16

10

dRH

vector31 a is 0

vector13 a is

matrix 33 a is

d

R

IHH

PHPHHH

1

2

2

00

2

1

1

0

2

0

)2

i.e., )1

Verify

1i

i

pP

101 dRR

HTT

17

Elementary Transformations

axTrans ,

byTrans ,

czTrans ,

,xRot

,yRot

,zRot

1000zzzz

yyyy

xxxx

dasn

dasn

dasn

T

0y

1z

1y

1x

0x

0z

1

0d

0o

1o

18

Skew Symmetric Matrix ss(3)

0

0

0

)(

xy

xz

yz

aa

aa

aa

aS

Rb Rab) R(a

R

papaS

bSaSbaS

nmatrix therotation a if

)(

)()()(

Verify

)()(

)(

)()(

)(

)(

RaSRaRS

bRaS

bRRRa

bRaR

bRaRS

T

T

T

T

19

)(

show toabove theofeither Use

) (Also

,0)()(

(why?) )()(

)(

,

SRdθdR

eR

dθdR

RS

dθRd(R

IRR

kS

k

T

T

T

T

Example

dθ

dRk , What is k?Find

20

Angular velocity and Acceleration

)()(

)3()(

tRtSR

SOtRR

That vector which parameterises S(t), i.e., the vector of the three components in S(t), is known as the angular velocity.

locityangular ve ;

))(()(Let

z

y

x

tStS

0

0

0

)(

xy

xz

yz

S

21

Example

0

0

1

)(

))((

)(

)(

)(,

)(,

)(,

t

RtS

RiSdtd

ddR

dtdR

RtR

tx

tx

tx

22

Velocity and Acceleration

10 )( ptRp

1111110 )()()()()()()(

ptRptRptRptRSptRptRp

1111

11111

11110

2)(

)()()()()()()()(

))()()(())()((

pRpRRpRp

ptRptRptRSptRSptRS

dtptRptRSd

dtptRptRd

p

Transverse acceleration

Centripetal acceleration

Coriolis acceleration

23

Addition of Angular Velocities

2

1

1

0

1

0

2

0

2

1

1

0

1

0

2

0

2

0

2

1

1

0

1

0

2

0

2

0

2

0

2

1

1

0

1

0

2

1

1

0

0

1

2

1

1

0

2

1

1

0

1

0

2

1

2

1

1

0

2

1

1

0

1

0

2

1

1

0

2

1

1

0

2

0

2

1

1

0

2

0

][][][

][][][

][][

][][

][][

R

RSSS

RRSSRS

RRSS

RRRSRRRS

RSRRRSRRRRR

RRR

n

n

nn RRR 1

1

0

3

2

2

0

2

1

1

0

1

00

24

0y

1z

1y

1x

0x

0z

0o

1o

2z

3z

2x

3x

1

2

3

Forward Kinematics

3321321

3

00 ),,,,,( PlllTP

1l

2l

3l)()( 1 i

i

iii qTqA

331

332211

3

0 )()()(

AAA

qAqAqAT

25

DH Representation

1000

0

,,,,

i

i

i

xaxdzzi

dcs

sascccs

casscsc

RotTransTransRotA

ii

iiiiii

iiiiii

iiii

i

id

i

ia

i

i

i

i

joint of angle

link ofoffset

link of twist

link oflength

iii dq or variablefor the stands

26

DH Representation - Frames

• (Convention) Frame i is attached to link i. The inertial frame is Frame 0 and Earth is link 0. Joint i joins links i-1 and i.

• (Another convention) The joint i+1 rotates about axis z i

• (DH1) The axis xi is perpendicular to the axis zi-1

• (DH2) The axis xi intersects the axis zi-1

• DH convention imposes two constrains thus enabling the use of only four parameters instead of six.

27

0y

1z

1y

1x

0x

0z

0o

1oa

1

0d

1

1

iiii xaxdzzi RotTransTransRotA ,,,,

3332

232221

131211

010101

010101

010101

1

0

0...

...

...

rr

rrr

rrr

kkkjki

jkjjji

ikijii

R),(),( 2111 scrr

),(),( 3332 scrr

,,

1

0 and Compare xz RRRDH1

d

28

0y

1z

1y

1x

0x

0z

0o

1oa

1

0d

1

1

iiii xaxdzzi RotTransTransRotA ,,,,

d

as

ac

aRidkd

10

1

0

) (i.e., and

ofn combinatiolinear a is

001

1

0

kzRi

dDH2

d

29

Assign frames at the two joints and at the end-

point based on DH convention

Write A1, A2, and A3

1

2

1l

2l

Link ai αi di

1

2

i

•Frame i is attached to link i. •The inertial frame is Frame 0 and Earth is link 0. •Joint i joins links i-1 and i.•The joint i+1 rotates about axis zi

iiii xaxdzzi RotTransTransRotA ,,,,

30

Three link cylindrical robot

2d

3dLink ai αi di i

1

2

3

1l

1000

0

,,,,

i

i

i

xaxdzzi

dcs

sascccs

casscsc

RotTransTransRotA

ii

iiiiii

iiiiii

iiii

31

1000

100

00

00

1

111

11

l

cs

sc

A

1000

0

,,,,

i

i

i

xaxdzzi

dcs

sascccs

casscsc

RotTransTransRotA

ii

iiiiii

iiiiii

iiii

1000

110

0100

0001

2

2 dA

1000

100

0010

0001

3

3 dA

1000

010

0

0

21

3

3

321

3

0111

111

dl

dccs

dssc

AAAT

32

1

2

4

5

6

3d

Link ai αi di i

1

2

3

4

5

6

Stanford Manipulator

1l2l

3l

33

SCARA Manipulator

12

43d

Link ai αi di i

1

2

3

4

34

Inverse Kinematics

1000333231

232221

131211

6

0

z

y

x

drrr

drrr

drrr

T

621

6

0

6

0 q,,q,q find d and Given R

35

xy

z

0y

1z

1y

1x0x

0z

0y1z1y1x0x

0z0o

1o