1

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

321

332211

3

0 )()()(

AAA

qAqAqAT

2

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

3

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.

4

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

5

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

6

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 ,,,,

7

1000

100

0

0

1

1

1

1111

111

l

slcs

clsc

A

1000

0100

0

0

122111212

122111212

21

2

0

slslcs

clclsc

AAT

1000

100

0

0

1

2

2

2222

222

l

slcs

clsc

A

1

2

1l

2l

8

Three link cylindrical robot

Link ai αi di i

1

2

3

1000

0

,,,,

i

i

i

xaxdzzi

dcs

sascccs

casscsc

RotTransTransRotA

ii

iiiiii

iiiiii

iiii

2d

3d

1l

1

9

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

2d

3d

1l

1

10

1

2

4

5

6

3d

Link ai αi di i

1

2

3

4

5

6

Stanford Manipulator

1l2l

3l

11

SCARA Manipulator

12

43d

Link ai αi di i

1

2

3

4

12

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

13

1

2

1l

2l

1000

0100

0

0

122111212

122111212

2

0

slslcs

clclsc

T

p

112211

12211

slsl

clcl

p

p

p

p

z

y

x

Dll

llpp yx

21

2

2

2

1

22

2 2cos

)1,tan( 2

2 DDA

),tan(),tan( 222211 slcllAppA yx

14

Kinematic Decoupling

• Last three joints intersecting at a point (spherical wrist)– Inverse position kinematics– Inverse orientation kinematics

• Let R )q,,q,(q 621

6

0 R

d )q,,q,(q 621

6

0 d

15

cd0

6

0d

6, za

0z

4z

6oo

Rkddd

Rkdooc

60

66

6d

effector)-end theoflength theis ( 6d

1

0

0

k

1. Find q1, q2, q3 such that is located at o.

2. With q1, q2, q3 obtain

3. Solve for q1, q2, q3 using the expression for from forward kinematic equations and its numerical value obtained from

cd0

RRR T)( 3

0

6

3

6

3R

3

0R

16

0y

0x

0z

p

0x

0y

),( yx pp1

),tan(1 yx ppA

r

0z),( sr

22

yx ppr

1lps z 2

3

2a

3aDaa

aasr 32

2

3

2

2

22

3 2cos

)1,tan( 2

3 DDA

),tan(),tan( 333322 sacaaAsrA

1l

17

Euler Angles

333231

232221

131211

uuu

uuu

uuu

csscs

ssccscsscccs

sccssccssccc

)1,tan( 2

3333 uuA

18

Velocity Kinematics

1000

0100

0

0

122111212

122111212

2

0

slslcs

clclsc

T

012211

12211

slsl

clcl

p

p

p

p

z

y

x

1

2

1l

2l

p

0z

1z

2z

0x

1x

2x

100

0

0

1212

1212

2

0 cs

sc

R

2

0

2

0

2

0 ][ RSR

)( 2

0

2

0 ddtd

v

Angular velocity of frame 2 wrt frame 0

velocity of p wrt frame 0

19

2

0

2

021122112

21122112

2

0 ][

000

0)()(

0)()(

RSsc

cs

R

11

00

00

;0

0

000

00)(

0)(0

100

0

0

000

0)()(

0)()(

][

2

1

21

2

021

21

1212

1212

21122112

21122112

2

0

JJ

cs

sc

sc

cs

S

20

2

1

2

112212211

12212211

21122111

21122111

00

0

)(

)(

v

z

y

x

Jclclcl

slslsl

clcl

slsl

p

p

p

dtd

p

0012212211

12212211

clclcl

slslsl

J v

21

n

n

nn RRR 1

1

0

3

2

2

0

2

1

1

0

1

00

1 ii

i

i kq

prismaticjoint ith if 0

revolutejoint ith if 1

i

so that Note 1

01 kRz i-

i

nnn

n qzqzqzqz 13232121010

Angular Velocity

22

n

nn

n

q

q

q

q

zzzz

3

2

1

12312010

1231201 nn zzzzJ

23

Linear Velocity

n

ii

i

nnn q

qd

dv1

000

prismaticjoint ith if

revolutejoint ith if ][

1

1110

i

n

ii

i

n

i

i

n

z

dzS

qd

qd

)4,3:1()4,3:1( 1

001

inn

i TTd

24

Velocity Kinematics - 2 DOF

1000

0100

0

0

122111212

122111212

2

0

slslcs

clclsc

T

1

2

1l

2l

p

0z

1z

2z

0x

1x

2x

1000

0100

0

0

111

111

1

1

1

slcs

clsc

A

1000

0100

0

0

222

222

2

2

2

slcs

clsc

A

vJJddzz ,,,,, :for sexpression Write 2

1

2

010

25

Velocity Kinematics - 2 DOF

000

;

0122

122

11

11

12211

12211

2

112211

12211

2

0 sl

cl

sl

cl

slsl

clcl

dslsl

clcl

d

0z

1

2

1l

2l

p

1z

2z

0x

1x

2x

1

0

0

;

1

0

0

00 zz

;

00

)()( 12212211

12212211

2

11

2

00

clclcl

slslsl

dzSdzSJ v

11

00

00

10

zzJ

26

Singularities

T

n

T

zyxzyx

qqqq

vvvX

dqqJdXqqJX

],,,[

],,,,,[

))( (also )(

21

The values of q for which the rank of J decreases are called the singularities or singular configurations.

27

Singularities 2-DOF

;

00 2

112212211

12212211

clclcl

slslsl

v

v

v

z

y

x

Velocity can be given in only x and y direction.

0))(()( 1221221112212211 slclclclslsl

when theta2 is zero. In that configuration arbitrary velocity in x and y direction cannot be imparted. One example is velocity in line with the manipulator when theta2 is zero.

28

Importance of Singularities

• certain direction of motion may be unattainable.• bounded gripper velocities to unbounded joint

velocities.• bounded gripper forces and torques may correspond

to unbounded joint torques.• usually points on the boundary of the workspace.• unreachable points in workspace under small

perturbation of link parameters such as length, offset, etc.

• non-unique solution to inverse kinematics problem.

29

Inverse kinematics and Jacobian

T

n

T

zyxzyx

qqqq

vvvX

XqJqdqqJdXqqJX

],,,[

],,,,,[

))( and )( (also )(

21

1

For desired end-point position and orientation as a function of time (the usual robot operation) X-dot can be evaluated and by inverting the Jacobian and so by integrating from a known initial value of q, other values of q can be obtained for given positions and orientations.

This method is great but for singularities of the Jacobian.

30

Dynamics

• Differential equation relating input torques and forces to the positions (angles) and their derivatives.

• Like force = mass times acceleration.

)(),()( qgqqqcqqD

anglesjoint of vector theis

uesinput torq of vector theis

q

31

Euler-Lagrange Equations

• Equations of motion for unconstrained system of particles is straightforward (F = m x a).

• For a constrained system, in addition to external forces, there exist constrained forces which need to be considered for writing dynamic equations of motion.

• To obtain dynamic equations of motion using Euler-Lagrange procedure we don’t have to find the constrained forces explicitly.

32

Holonomic ConstraintsF

(x,y)

m

l222 lyx

mjtqq nj ,,1,0),,,(

sconstraint holonomicfor expression General

1

33

Nonholonomic Constraints

)(sin)(

)(cos)(

ttry

ttrx

mjtqqqqf nnj ,,1,0),,,,,,( 11

x

y

General expression for nonholonomic constraints is:

Nonholonomic constraints contain velocity terms which cannot be integrated out.

A rear powered front steering vehicle

34

krr ,,1

Consider a system of k particles, with corresponding coordinates,

kiqqrr

qq

nii

n

,,1),,,(

and ,,

1

1

Often due to constraints or otherwise the position of k particles can be written in terms of n generalised coordinates (n < k),

In this course we consider only holonomic constraints and for those constraints one can always find in principle n (n < k) independent generalised coordinates.

35

Virtual Displacements

ir

kidtt

rdq

q

rdr i

n

jj

j

ii ,,2,1,

1

Define virtual displacements from above by setting dt=0.

kiqq

rr

n

jj

j

ii ,,2,1,

1

In our case dqj are independent and satisfy all the constraints. If they additional constraints have to be added to dqj to finally arrive at a statement of virtual displacements which have only independent dqj, we can replace dqj with jq

36

Virtual Work

forces ngconstraini theare

forces external theare )(a

j

j

f

f

k

jj

Ta

jj

k

jj

T

j rffrFW1

)(

1

)(

Let Fi be total force on every particle, then virtual work is defined as:

Constraining forces do no work when a virtual displacement takes place (as is the case with holonomic constraints), so in equilibrium

0)(1

k

jj

T

j rfW

37

D’Alembert’s Principle

D’Alembert’s principle states that, if one introduces a fictitious additional force, the negative of the rate of change of particle i, then each particle will be in equilibrium.

dt

drmprpfW j

j

k

jj

T

jj

,0)(1

38

Generalised Forces

is called the i-th generalised force. The equations of motion become:

.,,2,1,

where

1i

11 11

niq

rf

qqq

rfrf

k

j i

jT

j

i

n

ii

k

j

n

ii

i

jT

j

k

jj

T

j

.0))((11

i

k

j i

jT

ji

n

ii q

q

rrm

39

F

(x,y)

m

l

222 lyx

cos

sin

coordinate dGeneralise

;sin

cos

1,1

1

1

1

F

F

F

FF

q

l

l

y

xr

nk

y

x0))((11

i

k

j i

jT

ji

n

ii q

q

rrm

40

F

(x,y)

m

l

222 lyx

cos

sin

coordinate dGeneralise

;sin

cos

1,1

1

1

1

F

F

F

FF

q

l

l

y

xr

nk

y

x

0)(

0))((

1

11

yym

xxm

qq

rrm i

k

j i

jT

ji

n

ii

FlFlFl

l

lF

q

rf T

j i

jT

j

22

1

2

1

sincos

sin

cos

2222

2222

))(cossin()cos(

))(cossin()sin(

llx

y

llx

x

Fml

:ismotion ofequation The

41

Euler-Lagrange Equations of Motion

k

j i

jT

ji

i

jT

ji

k

j i

jT

ji

q

r

dtd

rmq

rrm

dtd

q

rrm

1

1

)()(

)(

i

j

l

n

l li

j

i

j

i

j

i

j

i

n

i

jT

jj

q

vq

qq

r

q

r

dtd

q

r

q

v

qq

rrv

1

2

11

and

Then

)(

42

k

j i

jT

ji

i

jT

ji

k

j i

jT

ji q

vvm

q

vvm

dtd

q

rrm

11

)(

ii

k

j i

jT

ji

k

jj

T

jj qK

q

Kdtd

q

rrmvvm

11

)(21

K

be K toenergy kinetic theDefine

.,2,1,0

0))((111

niqK

q

Kdtd

qqK

q

Kdtd

qq

rrm

i

ii

n

iii

ii

i

k

j i

jT

ji

n

ii

43

K-VL

niqL

q

Ldtd

qV

i

ii

i

i

i

Where

.,2,1,

Then

such that

forces),or torques(external tau and energy) (potential V

exist theresuppose Finally,

44

F

(x,y)

m

l

y

xyxmK21

.,2,1, niqL

q

Ldtd

i

ii

Write the dynamic equations of motions for this system.

45

F

(x,y)

m

l

Fl

mly

xyxmK

2

2

21

21

.,2,1, niqL

q

Ldtd

i

ii

Flml

2

:ismotion of

equation dynamic The

46

Expression for Kinetic Energy

B

T

B

T

B

dmzyxvzyxv

dxdydzzyxzyxvzyxvK

mdxdydzzyx

),,(),,(21

),,(),,(),,(21

),,(

BBody

0)(or 1

1,

1,

1

:asgiven is ),,( mass of Centre

B

c

B

c

B

c

B

c

B

c

ccc

dmrrrdmm

r

ydmm

zydmm

yxdmm

x

zyx

47

Attach a coordinate frame to the body at its centre of mass, then velocity of a point r is given by:

mass of centre theofocity linear vel theis

mass of centre sbody' the toattached

frame theoflocity angular ve theis

where

)(

c

c

v

rSvv

4321

)()(21

KKKK

dmrSvrSvK c

T

B

c

48

c

T

cc

T

B

c vvmdmvvK21

21

1

)0 (since 0)(21

)(21

2 c

B

T

c

T

B

c rdmrSvdmrSvK

0)(21

3 dmvrSK c

T

B

)()(21

)()(21

)()(21

4

T

TT

B

T

B

JSSTr

dmSrrSTr

dmrSrSK

BBB

BBB

BBB

T

B

dmzyzdmxzdm

yzdmdmyxydm

xzdmxydmdmx

dmrrJ

2

2

2

Where

49

BBB

BBB

BBB

dmyxyzdmxzdm

yzdmdmzxxydm

xzdmxydmdmzy

I

)(

)(

)(

Where

22

22

22

0

0

0

)(

xy

xz

yz

S

IJSSTrK TT

21

)()(21

4

50

00000

4

0

21

21

21

IRIR

IK

R

TTT

T

T

Frame for I and Omega

The expression for the kinetic energy is the same whether we write it in body reference frame or the inertial frame but it is much easier to write I in body reference frame since it doesn’t change as the body rotates but its value in the inertial frame is always changing.

So we write the angular velocity and the inertia matrix in the body reference frame.

51

Jacobian and velocity

frame) referencebody in is ( )()(

,)(

i

T

ii

vci

qqJqR

qqJv

i

ci

qqJqRIqRqJqJqJmqKn

i

T

iii

T

v

T

vi

T

iicici1

)()()()()()()(21

qqDqK T )()(21

D(q) is a symmetric positive definite matrix and is known as the inertia matrix.

52

Potential Energy V

B B

c

TTT mrgrdmgrdmgV

53

Two Link Manipulator

0z

1

2

11, lm

22 , lm

p

1z

2z

0x

1x

2x

Links are symmetric, centre of mass at half the length.

21,,, Find21

cc vv

54

xy

z

0y

1z

1y

1x0x

0z

0y1z1y1x0x

0z0o

1o