the l-e (torque) dynamical model:
DESCRIPTION
The L-E (Torque) Dynamical Model:. Gravitational Forces. Inertial Forces. Coriolis & Centrifugal Forces. Frictional Forces. Lets Apply the Technique -- . 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:. - PowerPoint PPT PresentationTRANSCRIPT
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 w.r.t. Fi
Step 4 Set Fc as the translation of Frame F1 to Cm of i Compute Inertia Tensor Di about Cm w.r.t. Fc
Step 5 Compute: zi-1(q); T0i; ci
bar(q); Di the Nor. Inertial Tensor
Before Starting lets define a L-E Algorithm:
Step 6 Compute Special Case of Ji(q)
Step 7 Partition 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
00
0 0 1 00 0 0 1
00
0 0 1 00 0 0 1
C S l CS C l S
A
C S l CS C l S
A
Not Exactly D-H Legal (unless there is more to the robot than these 2 links!)
So Let’s find T02
T02= 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 120
12 12 2 12 1 1
12 12 2 12 1 120
00
0 0 1 00 0 0 1:
00
0 0 1 00 0 0 1
CC S S C S S C l C C l S S l CS C C S S S C C l S C l C S l S
T
simplifiedC S l C l CS C l S l S
T
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 110
1 11
1 1 1 11 1 1
1 1 1 1
2001
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 cl
c c
C S l CS C l S
H T
l Cl
C S l Cl Sc S C l S
C2(bar) Computation:2 0
2
2
2 2
12 12 2 12 1 1
12 12 2 12 1 120
2
12 12 2 12 1 12
12 12 2 12 1 1
2001
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 cl
c c
C S l C l CS C l S l S
H T
lC S l C l C
c S C l S l S
2 121 1
2 121 1
2
20
l Cl C
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
1 11 0 1 0
1 1 1 121 1
1 1 1 1 1
21 1
[ ]
0 0 00 00 0 0 0
120 0 1 0 0 1
0 012
TD R D R
C S C Sm lD 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
00
120 0 1
m l m lS S C
m l m lD S C C
m l
S S Cm lD S C C
Continuing for Link 2
2 22 0 2 0
12 12 12 1222 2
2 12 12 12 12
22 2
212 12 122
22 22 12 12 12
[ ]
0 0 00 00 0 0 0
120 0 1 0 0 1
0 012
00
120 0 1
TD R D R
C S C Sm lD S C S C
m l
S S Cm lD S C C
Now lets compute the Jacobians
1
1 1
0
1 1
11 1 1
01
0
0
200
20 0
cJ q
Z
l S
c l CZ cq
Finishing J1
1 1
1 1
1
02
020 00 00 01 0
l S
l C
J
Note the 2nd 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
:
000
c cJ q q
Z Z
here
c Z cq
c Z c dq
This is 4th column of A1
Continuing:
22
01
2 12 2 121 1 1 1
2 12 2 121 1 1 1
000
2 20 00 0
2 21 00 0
c Z cq
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
200
21 00
2
20
l Cl Cl C
c l Sl 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 00 00 01 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 Normalized Inertial Tensor of Linki
defined in the Base space but acting on the link end
Building D(q)1
D(q)1 = (A1)Tm1A1 + (B1)TD1B1
Here: 1 1
1 1 1
1
02
020 0
0 00 01 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 10 1
0 0440 00 0
1 014
T
st
l S l S
l C l CTerm m
l S
l S l Cl Cm
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
00
120 0 1
S S Cm lD 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 00 012
nd
S S Cm lTerm 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 00 0 0 04 12
1 00 03
m l m lD q
m l
Building the Full Manipulator D(q)
D(q)man = D(q)0 + 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 00 01 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 202 2
2 200 02 2
4 4 2
4 2 4
Tm A A
l S l Sl Sl S l Cl S l C
l C l Cm l Cl S l C
l l l l Cl l l Cm
l 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
l S 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
l l l l S C C S S C C S S C
l l l l S C C S S C C C S S
l l l l C
similar reasoning for the other terms in the matrix
Building the 2rd D(q)2 Term:
Recall D2(nor. In. Tensor):
Then:
212 12 122
22 22 12 12 12
00
120 0 1
S S Cm lD S C C
212 12 122
2 2 22 22 12 12 12
22 2
0 0 00 0 1
0 0 00 0 112
0 0 1 1 1
1 11 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 Cm 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
12m 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 CD q
m 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
( ) ( ) ( ) ( )2kj
D q D q D q D qq q q q
CD q D q D q D qq q q q
man
here i = 1; j = 1 or 2; k = 1 or 2we take 'terms' from D(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 CD q
m 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 312
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:
11 2 2 2
1 2 2 2
1 2 2 2
0 0 0 010 02
20 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 the gravitational constant In the ‘h’ model Aki
j is extracted from the relevant Jacobian matrix (– for Joint i)
Here:3 2
1 11 1
( )jk j k
k j
h g m A q
Continuing:
2
1 21 2 21 0 1 21 2 21
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 12 41 04
kj j jkk
C D q D qq q
D q D q D q D qq q q q
D q D q D q D qq 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 S q
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 C b q