The L-E (Torque) Dynamical Model:

1 1 1

n n ni

i ij j kj k j i ij k j

D q q C q q q h q b q

Inertial Forces

Coriolis & Centrifugal

Forces

Gravitational Forces Frictional

Forces

Lets Apply the Technique --

Z0

Y0

Cm1

X0

X1

Z1

X2

Y2

Y1

Cm2

Z2

Lets do it for a 2-Link “Manipulator”

Link 1 has a Mass of m1; Link 2 a mass of m2

Before Starting lets define a L-E Algorithm:

Step 1 Apply D-H Algorithm to build Ai matrices and find Fi the “link frame”

Step 2 Set T00=I; i=1; D(q)=0

Step 3 Find ci the Homogenous coordinate of the center of mass of link I WRT Fi

Step 4 Set Fc as the translation of Frame F1 to Cm of i Compute Inertia Tensor Di about Cm wrt Fc

Step 5 Compute: zi-1(q); T0i; ci

bar(q); Di(q)

Before Starting lets define a L-E Algorithm:

Step 6 Compute Special Case of Ji(q)

Step 7Partition Ji and compute D(q) = D(q) + {ATmKA + BTDKB}

Step 8 Set i = i+1 go to step 3 else (i=n+1) set i=1 & continue

Step 9 Compute Ci(q); hi(q) and frictioni

Step 10 Formulate Torquei equation

Step 11 Advance “i” go to step 9 until i>n

We Start with Ai’s

1 1 1 1

1 1 1 11

2 2 2 2

2 2 2 22

0

0

0 0 1 0

0 0 0 1

0

0

0 0 1 0

0 0 0 1

C S l C

S C l SA

C S l C

S C l SA

Not Exactly D-H Legal (unless there is more to the robot than these 2 links!)

So Let’s find 0T2

0T2 = A1*A2

1 2 1 2 1 2 1 2 2 1 2 2 1 2 1 1

1 2 1 2 1 2 1 2 2 1 2 2 1 2 1 102

12 12 2 12 1 1

12 12 2 12 1 102

0

0

0 0 1 0

0 0 0 1

:

0

0

0 0 1 0

0 0 0 1

CC S S C S S C l C C l S S l C

S C C S S S C C l S C l C S l ST

simplified

C S l C l C

S C l S l ST

I’ll Compute Similar Terms back – to – back rather than by the Algorithm

1 01

1

1 1

1 1 1 1

1 1 1 101

1 11

1 1 1 11 1 1

1 1 1 1

20

0

1

01 0 0 0

00 1 0 0

0 0 1 00 0 1 0

0 0 0 1

22000

200 0 1 0 01

K

K

c H T c

l

c c

C S l C

S C l SH T

l Cl

C S l Cl S

c S C l S

C2(bar) Computation:

2 02

2

2 2

12 12 2 12 1 1

12 12 2 12 1 102

2

12 12 2 12 1 12

12 12 2 12 1 1

20

0

1

01 0 0 0

00 1 0 0

0 0 1 00 0 1 0

0 0 0 1

200000 0 1 01

K

K

c H T c

l

c c

C S l C l C

S C l S l SH T

llC S l C l C

c S C l S l S

2 121 1

2 121 1

2

20

l CC

l Sl S

Finding D1

Consider each link a thin cylinder

These are Inertial Tensors with respect to a Fc aligned with the link Frames at the Cm

21 1

1

21 1

22 2

2

22 2

0 0 0

0 012

0 012

0 0 0

0 012

0 012

m lD

m l

m lD

m l

Continuing for Link 1

0 01 1 1 1

1 1 1 121 1

1 1 1 1 1

21 1

[ ]

0 0 00 0

0 0 0 012

0 0 1 0 0 10 0

12

TD R D R

C S C Sm l

D S C S C

m l

Simplifying:

2 22 1 1 1 11 1 1

2 221 1 1 1

1 1 1 1

21 1

21 1 12

21 11 1 1 1

012 12

012 12

0 012

0

012

0 0 1

m l m lS S C

m l m lD S C C

m l

S S Cm l

D S C C

Continuing for Link 2

0 02 2 2 2

12 12 12 1222 2

2 12 12 12 12

22 2

212 12 122

22 22 12 12 12

[ ]

0 0 00 0

0 0 0 012

0 0 1 0 0 10 0

12

0

012

0 0 1

TD R D R

C S C Sm l

D S C S C

m l

S S Cm l

D S C C

Now lets compute the Jacobians

1

1 1

0

1 1

11 1 1

01

0

0

20

02

0 0

cJ q

Z

l S

c l CZ c

q

Finishing J1

1 1

1 1

1

02

020 0

0 0

0 0

1 0

l S

l C

J

Note the 2 column is all zeros – even though Joint 2 is revolute – this is the special case!

Jumping into J2

2 2

2 1 2

0 1

22

01

22

1 12

:

0

0

0

c cJ q q

Z Z

here

cZ c

q

cZ c d

q

This is 4th column of A1

Continuing:

22

01

2 12 2 121 1 1 1

2 12 2 121 1 1 1

0

0

0

2 20 0

0 02 2

1 00 0

cZ c

q

l C l Sl C l S

l S l Cl S l C

And Again:

2 121 1

1 122 12

1 1 1 12

2 12

2 12

20

02

1 00

2

20

l Cl C

l Cc l S

l S l Sq

l S

l C

Summarizing, J2 is:

2 12 2 121 1

2 12 2 121 1

2

2 2

2 20 0

0 0

0 0

1 1

l S l Sl S

l C l Cl C

J

Developing the D(q) Contributions

D(q)I = (Ai)TmiAi + (Bi)TDiBi

Ai is the “Upper half” of the Ji matrix

Bi is the “Lower Half” of the Ji matrix

Di is the Inertial Tensor of Linki defined in the Base space

Building D1

D(q)1 = (A1)Tm1A1 + (B1)TD1B1

Here: 1 1

1 1 1

1

02

020 0

0 0

0 0

1 0

l S

l CA

B

Looking at the 1st Term (Linear Velocity term)

1 1 1 1

1 1 1 11

1 1

1 1 1 11 1

1

2 2 2 2 21 1 1 1 1

1 1

21 1

0 02 2

1 : 0 02 20 1 0 1

02

002 2

20 0 1

0 1

0 0440 00 0

1 01

4

T

st

l S l S

l C l CTerm m

l S

l S l Cl C

m

l S l C lm m

m l

0 0

Looking at the 2nd Term (Angular Velocity term)

Recall that D1 is:

Then:

21 1 12

21 11 1 1 1

0

012

0 0 1

S S Cm l

D S C C

21 1 12

21 11 1 1

21 1

0 0 00 0 1

2 : 0 0 00 0 012

0 0 1 1 0

1 0

0 012

nd

S S Cm l

Term S C C

m l

Putting the 2 terms together, D(q)1 is:

2 2

1 1 1 11

21 1

1 0 1 0

0 0 0 04 12

1 0

0 03

m l m lD q

m l

Building the Full Manipulator D(q)

D(q)man = D(q)1 + D(q)2

Where– D(q)2 = (A2)Tm2A2 + (B2)TD2B2

And recalling (from J2):

2 12 2 121 1

2 2 12 2 121 1

2

2 2

2 20 0

0 0

0 0

1 1

l S l Sl S

l C l CA l C

B

Building the 1st D(q)2 Term:

2 22

2 12 2 121 1

2 12 2 121 1 1 1

2 12 2 122 1 1

2 12 2 12

2 222 2 1 2 21 1 2 2

2 2 22 1 2 2 2

2 20

2 22 2

00 02 2

4 4 2

4 2 4

Tm A A

l S l Sl S

l S l Cl S l C

l C l Cm l C

l S l C

l l l l Cl l l C

ml l l C l

How about term (1,1) details!

2 12 2 12 2 12 2 121 1 1 1 1 1 1 11,1

2 2 2 22 2 2 22 12 1 2 12 1 2 12 1 2 12 11 1 1 1

22 2 2 2 2212 12 1 1 1 1 2 12 1

2 2 2 2

2 24 2 4 2

4

l S l S l C l Cterm l S l S l C l C

l S l l S S l C l l C Cl S l C

lS C l S C l l S S C

12 1

2221 1 2 1 2 1 2 1 1 2 1 2 1

22 2 221 1 2 1 2 1 1 2 1 2 1 1 2

2221 1 2 2

4

4

4

C

ll l l S C C S S C C S S C

ll l l S C C S S C C C S S

ll l l C

similar reasoning for the other terms in the matrix

Building the 2rd D(q)2 Term:

Recall D2:

Then:

212 12 122

22 22 12 12 12

0

012

0 0 1

S S Cm l

D S C C

212 12 122

1 1 22 22 12 12 12

22 2

0 0 00 0 1

0 0 00 0 112

0 0 1 1 1

1 1

1 112

TS S C

m lB D B S C C

m l

Combining the 3 Terms to construct the Full D(q) term:

2 222 2 1 2 2

2 21 1 2 21 1 2 2

2 2 22 1 2 2 2

2 2 2 2 221 1 2 2 2 2 1 2 2 2 2

2 1 1 2 2 2

2 2 22 1 2 2 2 2 2

2 2

1 0 1 14 4 20 0 1 13 12

4 2 4

3 4 12 4 2 12

4 2 12 4

Man

l l l l Cl l l C

m l m lD q m

l l l C l

m l l m l l l l C m lm l l l C m

l l l C m l lm m

22 2

12

m l

Simplifying then D(q) is:

2 2 221 1 2 2 2 1 2 2 2 2

2 1 2 1 2 2

2 22 1 2 2 2 2 2 2

3 3 2 3

2 3 3

man

m l m l m l l C m lm l m l l C

D qm l l C m l m l

NOTE: D(q)man is Square in the number of Joints!

This Completes the Fundamental Steps:

Now we compute the Velocity Coupling Matrix and Gravitation terms:

,( ) ,( )

3

1

1( ) ( ) ( )

2

( )

ikj man ij man kj

k i

nj

i k j kik j i

C q D q D qq q

h g m A q

For the 1st Link

11 12 11 12

1 1 1 11

11 12 21 22

2 2 1 1

1

2

man

here i = 1; j = 1 or 2; k = 1 or 2

we take 'terms' from D(q)

kj

D D D D

q q q qC

D D D D

q q q q

Plugging ‘n Chugging

From Earlier:

THUS:

2 2 221 1 2 2 2 1 2 2 2 2

2 1 2 1 2 2

2 22 1 2 2 2 2 2 2

3 3 2 3

2 3 3

man

m l m l m l l C m lm l m l l C

D qm l l C m l m l

2 2 2 2 22 21 1 2 2 2 1 2 2 2 2 1 1 2 2 2 1

2 1 2 1 2 2 2 1 2 1 2 2

1 1 11

2 2 221 1 2 2 2 1 2 2 2 2

2 1 2 1 2 2

2 2

3 3 2 3 3 3

1

2

3 3 2 3

kj

m l m l m l l C m l m l m l m l lm l m l l C m l m l l C

q q qC

m l m l m l l C m lm l m l l C

q q

22 2 2 2

1

222 22 1 2 2 2 2

1 1

2 3

32 3

C m l

q

m lm l l C m l

q q

P & C cont:

1

1 2 2 21 2 2 2

1 2 2 2

0 00 01

0 022

0 0

11 2

kjC l l m Sl l m S

l l m S

Finding h1:

Given: gravity vector points in –Y0 direction (remembering the model!)

gk =(0, -g0, 0)T

g0 is gravitational constant

In the ‘h’ model Akij is extracted from the

relevant Jacobian matrix Here:

3 2

1 11 1

( )jk j k

k j

h g m A q

Continuing:

2

1 21 2 21 0 1 12 2 12

1

1 1 2 121 0 1 2 1 1

1 2 2 120 2 1 1

2 2

2 2

jj

j

h g m A g m A m A

l C l Ch g m m l C

m m l Cg m l C

looking back to jacobians and substituting:

Note: Only k = 2 has a value for gk which is g0!

Stepping to Link 2

22

2

21 22 11 12

1 1 2 2

21 22 21 22

2 2 2 2

1 2 2 2

1( ) ( )

2

1

2

1 1

2 41

04

kj j jkk

C D q D qq q

D D D D

q q q q

D D D D

q q q q

l l m S

Computing h2

3 2

2 21 2

2 0 2 2 122 0 2 22 ( ) 2

jk j k

k j

h g m A

g m l Ch g m A q

Building “Torque” Models for each Link

In General:

1 1 1

n n ni

i ij j kj k j i ij k j

D q q C q q q h q b q

For Link 1:

The 1st terms:

2nd Terms:

1 11 1 12 21

2 221 2 2 1 2 2

2 1 2 1 2 2 1 2 23 3 3 2

n

j jj

D q q D q D q

m l l l l Cm l m l l C q m q

2 21 1 2 1 1 1 2

11 1 21 1 2 12 2 1 22 21 1

22 1 2 21 2 2 2 2 1 2

22

2 1 2 2 2 1

( )

0 02

2

jk k jk j

C q q q C q C q q C q q C q

m l l Sl l m S q q q

qm l l S q q

Writing the Complete Link 1 Model

2 221 2 2 1 2 2

1 2 1 2 1 2 2 1 2 2

22 1 2 2 12

2 1 2 2 2 1 0 2 1 1 1 1

3 3 3 2

( )2 2 2

m l l l l Cm l m l l C q m q

q m m l Cm l l S q q g m l C b q

And, Finally, For Link 2:

22 2 2 2 2

1 1 1

n n n

j j kj k jj k j

D q q C q q q h q b q

Ist 2 terms:

1st Terms:

2nd Terms:

2 21 1 22 21

2 22 1 2 2 2 2

2 1 23 2 3

n

j jj

D q q D q D q

l l l C m lm q q

22 1 2 2 2 1 2 2 2 1 2 21 1 2 1 2

22 1 2 21

02 2 2

2

m l l S m l l S m l l Sq q q q q

m l l Sq

Finalizing Link 2 Torque Model:

2 222 1 2 2 2 2 2 1 2 2

2 2 1 2 1

0 2 2 122 2

3 2 3 2

( )2

l l l C m l m l l Sm q q q

g m l Cb q