Notes_08_13
1 of 7
Differential-Algebraic Equations (DAE) for Anthropomorphic Manipulator
y1
C
r2 = AG2
d2 = AB
x3
y3
G3
3
r3 = BG3
d3 = BC
T2on3
x2
T1on2
G2
x2
B
2
x1
A
Two solid rigid bars with revolute joints A and B
Tool center point (TCP) at C (endpoint)
Centroids G2 and G3
Masses m2 and m3
Centroidal mass moments of inertia JG2 and JG3
T1on2 is torque of ground on bar 2 about revolute A measured CCW positive
T2on3 is torque of bar 2 on bar 3 about revolute B measured CCW positive
Gravity g acts along negative y axis
nL = 3
nJ1=2
m = 3 (nL-1) – 2 nJ1 = 2
Notes_08_13
Lagrangian method (from Notes_09_02)
 
2 

3 
q  
T1 on 2 

T2 on 3 
q     2 
Q  
3 
Note: 3 measured relative to 2
J B  m 3 a 32  J 3
J A  J B  m 2 a 22  m 3 d 22  J 2  2m 3 d 2 a 3 cos  3
C  J B  m 3 d 2 a 3 cos 3
D  m 3 d 2 a 3 sin 3
G 2  m 2 a 2  m 3 d 2 g cos 2
G 3  m 3 a 3 g cos ( 2  3 )
inverse dynamics
2
T2  J A C 
 2 
 
 D 3  2D 2  3 
 G 2  G 3 


  
  



2





D

T3  C J B  
2
 3 
  G3 
know driver motion at any time t - find q
compute driver torques T1 on 2
compute joint forces F1 on 2
x
q  q
T2 on 3 from Lagrangian equations
F1 on 2
y
F2 on 3
x
F2 on 3
y
from Newtonian equations
may arbitrarily choose any other time t
forward dynamics
1
2

 2 
 J A C  T2  
 D 3  2D 2  3 
 G 2  G 3  

   






2


 3 
 C J B   T3  
 D 2
  G 3 
know current state q and q  at current t
compute q from Lagrangian equations
compute F1 on 2
x
F1 on 2
y
F2 on 3
x
F2 on 3
y
from Newtonian equations
must integrate q to get new q and q  at the next time step
2 of 7
Notes_08_13
3 of 7
Newtonian method (from free body diagrams)
F1 on 2  F2 on 3  m2 x2
x
x
F1 on 2  F2 on 3  m2 g  m2 y2
y
y
s 2 A  F1 on 2  s 2 B  F2 on 3  T1 on 2  T2 on 3  J G 22
F2 on 3  m3 x3
x
F2 on 3  m3 g  m3 y3
y
s3 B  F2 on 3  T2 on 3 J G 33
inverse dynamics
 1

0

 s 2 A

0


0

0


0

Y

1
1
0
0
0
1
1
s  
0
0
1
0
0
0
0
1
 s 2 

0
A X
0
0
B Y
2

 s3 

B Y

 s 2 


B X
 s3 

B X
0   F1 on 2 x   m2 x2


y


0   F1 on 2  m2 y2  m2 g 
 1  T1 on 2   J G 22 



0   F2 on 3 x   m3 x3

0   F2 on 3 y   m3 y3  m3 g 

 

 1  T2 on 3   J G 33 
know driver motion at any time t - find positions, velocities and accelerations

compute joint forces F1 on 2
x
F1 on 2
y
F2 on 3
x
F2 on 3
x
T1 on 2 T2 on 3

T
may arbitrarily choose any other time t
forward dynamics
know current positions and velocities at current t
very cumbersome to compute joint forces and accelerations simultaneously
must integrate accelerations to get new positions and velocities at the next time step
Notes_08_13
4 of 7
DAE dynamics
x 2 
y 
 2
 
q   2 
x 3 
y3 
 
  3 
 AG 2 

 0 
s 2 ' B  
 BG 3 

 0 
s 3 ' C  
s 2 ' A  
BG 2 

 0 
s 3 ' B  
CG 3 

 0 
 A  A 
ΦKINEMATIC   r2 B  r1 B 
r3   r2  
ΦDRIVERS
 f 1 q, t 


f 2 q, t 
inverse dynamics
 M 

  q
 
   q  Q
T
0  
12 x1
q
12 x12


EXT




12 x1
r2 A  r1 A 


  r3 B  r2 B 
 

DRIVERS 

know driver motion at any time t, find q
FA1 on A 2 
F

 B 2 on B3 
  

 T1 on 2 
 T2 on 3 
 
q   QEXT
q
compute q and  simultaneously
may arbitrarily choose any other time t
forward dynamics
 M 

  q
 
   q  Q
10 x10
T
0   
10 x1
q
EXT

10 x1



A
 A

  r2 B  r1B 
r3   r2  
know current state q and q  at current t, find 
QEXT
F

   A1 on A 2 
FB 2 on B3 
 
compute q and  simultaneously
must integrate q to get new q and q  at the next time step
q
Notes_08_13
5 of 7
Inverse Dynamics – joint interpolated motion
(independent position controllers on each joint)
REV _ A




REV _ B


  

2  2 _ START  2 t 

   START   t 

  3   2
q
6x6
 M 

  q
 
REV _ A 


 REV _ B 
  

 0 
 0 
   q  Q
T
0  
q
12 x1
12 x12
EXT



QEXT
3
A
  END  START  / t
02 x1
0 
   2 x1 
 2 
 3 
 
C
2  2 _ END  2 _ START / t
2
B
 0 
 m g 
2 

 0 


 0 
  m 3g 


 0 
FA1 on A 2 
F

 B 2 on B3 
  

 T1 on 2 
 T2 on 3 



12 x1
Inverse Dynamics – straight-line TCP interpolated motion
(interpolated position controllers on each joint)
REV _ A


REV _ B
  
x C3  x C3 _ START  v C3 _ x
 y C 3  y C3 _ START  v C3 _ y




t
t 
 02 x1
 0 
   2 x1 
v C3 _ X 
v C3 _ Y 
 
q
6x6
 M 

  q
 
12 x12
T
0  
12 x1


v C 3 _ y  y C3 _ END  y C 3 _ START / t
REV _ A 


   REV _ B 
 0 
 0 
   q  Q
q
v C3 _ x  x C 3 _ END  x C3 _ START / t
EXT

12 x1



FA1 on A 2 
F

 B 2 on B3 
  

 FEFF _ CX 
 FEFF _ Cy 
QEXT
 0 
 m g 
2 

 0 


 0 
  m 3g 


 0 
C
3
A
2
B
Notes_08_13
r cos 2  r3 cos2  

 r2 sin 2  r3 sin 2   
  3   2
    
3
6 of 7
r3 C   2
2
 r2 sin  2  r3 sin  2    r3 sin  2    2 
 
r3 cos 2      
 r2 cos  2 r3 cos 2  
r3 C  
T
T
T1 on 2   2 
FEFF _ CX 
C

 r3   
  0
 FEFF _ Cy 
T2 on 3    
T1 on 2 
 r2 sin  2  r3 sin  2    r3 sin  2   FEFF _ CX 

  


r3 cos 2     FEFF _ Cy 
 r2 cos  2 r3 cos 2  
T2 on 3 
T
Forward Dynamics – double pendulum
(no actuators)


   REV _ A 
REV _ B 


   REV _ A 
REV _ B 
0 
   2 x1 
02 x1
 
q
4x6
 M 

  q
 
   q  Q
10 x10
T
0  
10 x1
q


EXT

10 x1



QEXT
F

   A1 on A 2 
FB 2 on B3 
 0 
 m g 
2 

 0 


 0 
  m 3g 


 0 
Notes_08_13
7 of 7
Forward Dynamics – proximal link kinematically driven, distal link pendulum
(position controller only on proximal joint)


REV _ A


  
REV _ B

  

2 _ CENTER   sin 2 f t 
 2
02 x1




  
02 x1

2 f  cos2 f t  


 
q
5x 6
   q  Q
 M 

  q
T
  0  
q


11x1
11x11
EXT




11x1

REV _ A




  
REV _ B

 42f 2  sin 2 f t 


FA1 on A 2 
  FB2 on B3 
 T

 1 on 2 
QEXT
 0 
 m g 
2 

 0 


 0 
  m 3g 


 0 
Forward Dynamics – computed torque control
(torque controllers on each joint)
T2 on 3


   REV _ A 
REV _ B 
T1 on 2


   REV _ A 
REV _ B 
 
0 
   2 x1 
02 x1
 M 

  q
   q  Q
q
4x6
 
10 x10
T
0  
q
10 x1


EXT

10 x1



QEXT
F

   A1 on A 2 
FB 2 on B3 
0


 m g 
2


T1 on 2  T2 on 3 


0


  m 3g 


 T2 on 3
