Recursive Robot Dynamics - Prof. Subir Kumar Saha | Ph. D...
Transcript of Recursive Robot Dynamics - Prof. Subir Kumar Saha | Ph. D...
What is recursive?
1
#n
#1
#i
i
n
#1: vel., accn.
1: rate, accn
+ #0
#i: vel., accn.
#n: vel., accn.
#n: inertia force
n: torque
i: torque
#i inertia force
#1:inertia force
#0: vel., accn. = 0
1: torque
Why recursive?
• Efficient, i.e., less computations and CPU time
Compute jt. torque Compute jt. accn. (Inverse) (Forward)
0 5 10 15 20 25 30
0
2000
4000
6000
8000
10000
12000
14000
Total number of joints
Co
mp
uta
tio
na
l C
ou
nts
Equal number of 1-, 2- and 3-DOF joints
Proposed
Balafoutis
Featherstone
Angeles
0 5 10 15 20 25 30
0
0.5
1
1.5
2
2.5x 10
4
Total number of joints
Co
mp
uta
tio
na
l C
ou
nts
1-, 2- and 3-DOF joints
Proposed
Mohan and Saha
Lilly and Orin
Fetherstone
Ref. : Shah, S.,V. Saha, S.K., Dutt, J.K, Dynamics of Tree-type Robotic Systems, Springer, 2013
Why recursive? (contd.)
• Numerically stable Simulation is realistic Recursive Non-recursive (Forward) (Forward)
Ref. : Mohan, A., and Saha, S.K., A recursive, numerically stable, and efficient simulation algorithm for serial robots with flexible links, Multibody System Dyn., V. 21, N. 1,pp. 1—35.
Review of Dynamic Formulations
• Euler-Lagrange (EL)
• Newton-Euler (NE) i i i
i i i i i i
m
v f
I ω ω I ω n
i
i i
d L L
dt q q
i i i i i i M t WM t w
, ,i i ii
i i i i
i i im
I O ω nω 1 ΟM W t ,w
O 1 v fΟ Ο
Starting point for the Recursive Dynamics (using the DeNOC matrices)
DeNOC matrices
Newton-Euler to Euler-Lagrange
• Analytical expressions of vector and matrices,
Decomposition of inertia Matrix, Recursive algorithms,
Dynamics model simplifications, etc.
,
Decoupled form of the velocity transformation matrix
Minimal order Equations of motion
(Euler-Lagrange)
Newton-Euler Equations of
motion ×
Eliminate constraint forces and moments from the NE equations.
Rate of Independent coordinate θ.
Link velocities t (ω and v) = ×
DeN
OC
m
atri
ces
=
Example: A Moving Mass
: Vertical component Reaction
: Horizontal component Motion
Mass, m
Force,
f
fvf
vf
f
x
Equation of Motion
Newton’s 2nd law: Velocity constraint: NOC: Euler-Lagrange:
cff mce
x ][ic
][i
ef
[ ] [ ( ) ] [ ]T Tv cf f f mx f mx i i j i i
External force,
Mass, m
i
j
c
cfReaction,
Note that
[ ] ( )[ ] 0Tv cf f i j
Uncoupled NE Equations
• Newton-Euler (NE) equations for the ith rigid body
• NE equations in compact form
where
i
i i i i i i
i im
I ω ω I ω n
v f
i i i i i i M t WM t w
, , ,i i ii
i i i i
i i im
I O ω nω 1 ΟM t W w
O 1 v fΟ Ο
Uncoupled NE Equations
#0
1
#n
#1
#i
i
n
E C
i i i i i i i M t WM t w w
E C Mt WMt w w
1 2
1 2
diag [ ]
diag [ ]
n
n
M M M M
W W W W
• Separate the bodies n bodies
• 6n NE equations
Kinematic (Velocity) Constraints
Bij: the 6n 6n twist-propagation matrix
pi: the 6n-dimensional joint-rate propagation vector or twist generator
( )
ij
ij j i
c
1 O
B r d 1 1i
i
i i
ep
e d
Coupled Equations of Motion
• Pre-multiplication by NT
• Equations in compact form
I : nn Generalized inertia matrix (GIM)
C : nn Matrix of convective inertia (MCI) terms
: n-dimensional vector of generalized forces due to driving torques/forces, and those resulting from the gravity, environment and dissipation.
Iθ Cθ τ
τ
)()( CETTwwNWMttMN
n coupled EL equations
- no partial differentiation T C N w 0
Generalized Inertia Matrix (GIM)
• Generalized inertia matrix (GIM)
where
• Each element of the GIM
• Mass matrix of composite body
15
T
ij i i ij ji p M B p
1, 1 1,
T
i i i i i i i M M B M B
Td dI N MN
l
T
l MNNM ~
Vector of Convective Inertia (VCI)
• Vector of Convective Inertia
where and
• Each element of h
'
1, 1
where , and
and
T
i i i
T
i i i i i n n
i i i i i i
h p w
w w B w w w
w M t W M t
'~wNθChT
d
)('~ WMtMt'Nw T
d θWNNt' )( ll
Generalized Force (Joint Torque)
• Generalized Force
where
• Each element is obtained recursively
1, 1
,
where , and
T E
i i i
E E T E E E
i i i i i n n
p w
w w B w w w
T Edτ N w
ET
l
EwNw ~
Example: One-link arm
11
2
( )
1
3
T
T T
I i
m ma
p Mp
e Ie (e d) (e d)
where and m
e I Op M M
e d O 1
;
TT asac ]02
1
2
1[][ ;]100[][ 11 de
20 0 0
[ ] 0 1 012
0 0 1
ma
2
I
2
22
0
[ ] [ ] 012
0 0 1
s s c
maT s c c1 2
I Q I Q
100
0
0
cs
sc
Q
Ref: “Introduction to Robotics” by Saha
Moment of inertia about O
where
( )
[ )] 0
T
T
h
p MW WM p
e I(e e) (e Ie
f
ndeewN ])([1
TTET
l
TT mg ]00[][;]00[][ 11 fn
mgas2
11
Equation of motion: 21 1
3 2ma mgas
1
#n
#1
#i
i
n
#0
1 1 1
2 2 2 1
1n n n n
α p
α p α
α p α
1 1 1 1 1 1
2 2 2 2 2 2
21 1 21 1
, 1 1 , 1 1
n n n n n n
n n n n n n
β p Ω p
β p Ω p
B β B α
β p Ω p
B β B α
Recursive Inverse Dynamics
1 1 1 1 1 1 , 1
1 1 1 1 1 1 21 2
n n n n n n
T
n n n n n n n n n
T
γ M β W M α
γ M β W M α B γ
γ M β W M α B γ
1 1 1
1 1 1
T
n n n
T
n n n
T
p γ
p γ
p γ
Summary of Previous Lecture
• Why recursive?
• Define DeNOC (Decoupled Natural Orthogonal Complement) matrices from velocity constraints
• Derived NE EL equations (Constrained minimum set)
• Analytical expressions for the GIM, and other matrices
Chinese (PRC)
Spanish (Mexico)
RoboAnalyzer (Free: www.roboanalyzer.com)
where
( ) [ )] 0T Th p MW WM p e I(e e) (e Ie
f
ndeewN ])([1
TTET
l
TT mg ]00[][;]00[][ 11 fn
mgas2
11
Equation of motion: 21 1
3 2ma mgas
2
11
1( )
3
T T TI i m ma p Mp e Ie (e d) (e d)
Verify the results using MATLAB’s symbolic tool (MuPAD)
Example: Two-link Manipulator ;
11 12 21 1 1
21 22 2 2
( ); ;
i i i h
i i h
I h Cθ τ
22 :ScalarTi 2 2 22 2p M B p
2222222222222 ][][][][][ ddeIeTT mi
22 : 6 6 identity matrix
1 OB
O 1
2
2
2 2
: 6-dim. vector
ep
e d
2
2 2
2
: 6 6 sym. matrixm
I OM M
O 1
X3
Y3
2 2 2 2 2 2 2 21 1
[ ] [0 0 1] ; [ ] [ 0]2 2
T Ta c a s e d
2 2
2 2 2
0
0
0 0 1
c s
s c
Q
22 2 2
22
2 2 2 2 2 2 2
0
[ ] [ ] 02 3 120 0 1
T
s s c
mas c c
I Q I Q
2222222222222 ][][][][][ ddeIeTT mi
2 2 222 2 2 2 2 2 2
1 1 1
12 4 3i m a m a m a
X3
Y3
;
21 12 1 1( ) :ScalarTi i 2 2 2p M B p
21 2 1 2 1 1 1 2 2 1 1 1 1 2 1[ ] [ ] [ ] [ ] ([ ] [ ] )T Ti m e I e d d r d
211 2
: 6 6 matrix( )
1 OB
r d 1 1
2 1 2 1 1 2 2 2 12 2 12
1 1 1 1 1 1 1 1 2 1
1 1[ ] 0 0 1 ; [ ] [ ] [ 0]
2 2
1 1[ ] [0 0 1] ; [ ] [ ] [ 0]
2 2
T T
T T
a c a s
a c a s
e d Q d
e d r
r1 1 1
1 1 1
0
0
0 0 1
c s
s c
Q
2 2 221 2 2 2 1 2 2 2 2 2 2 2 1 2 2
1 1 1 1 1
12 2 4 3 2i m a m a a c m a m a m a a c
1
1
1 1
: 6-dim. vector
ep
e d
11 1 1 11 1 :ScalarTi p M B p
11 : 6 6 identity matrix
1 OB
O 1
1
1
1
: 6 6 sym. matrixm
I OM
O 1
X3
Y3
11δ
1δIBMBMM 2
11
11212111 ~
~~~
m
T
12 21
1 1 2 2 1 2 1
( )
( )m
c c
I I I r d δ 1
2121
~cδ m
211~ mmm
11 1 1 1 1 1 1 1 1 1 1
2 2 21 1 2 2 2 1 2 1 2 2
[ ] [ ] [ ] [ ] [ ]
1 ( )
3
T Ti m
m a m a m a m a a c
e I e d d
Vector of Convective Inertia
Link Joint ai
(m)
bi
(m)
i
(rad)
i
(rad)
1 r 0.3 0 0 JV [0]
2 r 0.25 0 0 JV [0]
22 2 2 2 1 2 2 1
1
2
Th m a a sθ θ p w 1 1 2 1 2 2 2 2 11
( )2
Th m a a sθ θ θ θ 1 p w
Link mi ri,x ri,y ri,z Ii,xx Ii,xy Ii,xz Ii,yy Ii,yz Ii,zz
(kg) (m) (kg-m2)
1 0.5 0.15 0 0 0 0 0 0.00375 0 0.00375
2 0.4 0.125 0 0 0 0 0 0.00208 0 0.00208
DH
an
d I
nert
ia p
ara
me
ters
Inverse Dynamics Results
Equation of motion for one-link arm
21 1
3 2ma mgas
Forward Dynamics & Simulation
2
3 1( )
2mgas
ma Forward Dynamics:
Integration (numerical): 1 2
1 2
2 12
;
3 1( sin )
2
y y
y y
y mga yma
1st order form:
( , )ty f y
Integrate (say, numerically) to obtain y(t) using, e.g., Runge-Kutta method (ode45 of MATLAB)
Simulation
Recursive Forward Dynamics
θ
34
Step 2: UDUT Decomposition
, where andT T
Analytically
UDU θ I UDU τ Cθ
Equation of the motion
The joint accelerations are then solved as
1 1θ U D U
T
Hence forward dynamics requires three steps
Step 1: Computation of φ
Step 3: Recursive computation of
Iθ Cθ τ
Observations
• Derivations appear to be complex for the simpler manipulators
• It was for demonstration only
• The computations will be done algorithmically
• Due to recursive natures, calculations are fast
• The algorithm should be used for complex robotic systems like 6- or more-DOF robots
Summary of Previous Lecture
• Use of RoboAnalyzer (RA) for inverse dynamics of one-link arm
• GIM for 2-link manipulator
• Inverse dynamics of KUKA robot
• Forward dynamics and simulation
• Use of RA for simulation
• Some observations for using recursive dynamics
Stew
art
Pla
tfo
rm
Parallel Manipulators
Four-bar Mechanism • Separate all links • Draw Free-body diagrams (FBD)
12 12
1 1 1 1 01 1 01 1 21 1 21
1 1 01 21 1 1 01 21;
x y
x y y x x y y x
x x x y y y
f f
I d f d f r f r f
m c f f m c f f
=
Equations of motion: 3 per link
Base, #0
#1
#3
C1
#2
f12
f21
f01
f32
f23
f03
d1
r1
#1
#2
a3
2
3
1
3 2
1 4
a2
a0
a1
Base, #0
#3
4
1
9 equations 13 – 4 (3rd law) = 9 unknowns
11 1 1 1 1 1
01 1 1
01 1 1
122 2 2 2 2 2
12 2 2
23 2 2
233 3 3 3 3
03
03
1
1 1 0 '
1 1
1 1
1 1
0 ' 1 1
1 1
y x y x
x x
y x
xy x y x
x x
x y
yy x y x
x
y
d d r r I
fs m c
f m c
fd d r r I
f m c
f m c
fd d r r I
fs
f
3
3 3
3 3
: 9 9 matrix x : 9 1 : 9 1
x
y
m c
m c
A b
• Ghosh and Mallik, Theory of Machines and Mechanisms
1 (Forward); (Backward)y
x=A b LUx=b; Ly=b Ux=y
Disadv.: Need to calculate even the reactions for inv. dyn.
Three-link Serial with f03 as External • Join first three links form
Equations of motion
Base, #0
#1
#3
C1
#2
f12
f21
f01
f32
f23
f03
d1
r1
Base, #0
033 3
( ) ( )
: 3 eqs.T
T T T T E Cd l d l
E C
J f
N N Mt WMt N N w w
I θ Cθ τ τ0 1 2 3
2 3
0
a a a a 0 J θ 0
#1
#2
a3
2
3
1
3 2
1
a2
a1
#3 1
f03 a0
• The DeNOC matrices for 3-link serial manipulator
Proof of C = JTf03
1 1 1
2 21 2 2
3 31 32 3 3
18 1 :18 18 :18 3 3 1
0' 0 '
0 '
l d
s s
s
N N
t 1 p
t B 1 p
t B B 1 p
32 21 31 21 32 31
2
1 21 2 32 3
21 31 1
32 2 2 32 3
3
3
( )
0 '
T T T
C
C T C T C
T T C
T C T C C T Cl
C
C
s
B B B B B B
w
w B w B w
1 B B w
N w 1 B w w B w
1 w
w
03 23 3 23 3 03
3
03 23
C
n n d f r fw
f f2 3
32
6 6
( )
0 '
T
s
1 r d 1B
1
f03
f23
r3
d3 n03
n23
C3
23
23
32 12 2 12 2 32
2 2 32 3
32 126 1
03 23 3 23 3 03 2 3 03 23
03 23
( ) ( )
C C T C
n
f
n n d f r f
w w B wf f
n n d f r f r d f f +
f f
2
03 12 2 3 3 03 2 12
2
03 12
( )C
p
n n r d r f d f
w
f f
03 01 1 03 1 01
1
03 01
C
n n p f d fw
f f
C2
r3 2
3
1
3 2
1
r2
1
f03
p2
p1
d3
Constraint Torque 1 1
2 2
3 3
0 '
0 '
T C
T C T Cd
T C
s
s
p w
N w p w
p w
( 1,2,3)i
ii i
i
ep
e d
03 01 1 03 1 01
1 1 1 1 1 1
03 01
1 03 01 1 03 1 01
1 1 03 01
1 1 03 1 1 03
( )
( )
( ) ( )
( ) ( )
C T C T T
Scalar
T
T
T T
n n p f d fp w e e d
f f
= e n n p f d f
e d f f
= e ρ f e ρ f
2 2 2 2 2 03 2 2 03
3 3 3 3 3 03 2 3 03
( ) ( )
( ) ( )
C T C T T
C T C T T
p w e ρ f e ρ f
p w e a f e a f
C2
a3 2
3
1
3 2
1
r2
1
f03
p1
1
d2
Equations of Motion
*1 03[ ,0,0]:
T T
E C
known
J fτ
Iθ Cθ τ τ
3
1 1 2 2 3 3
a
J e ρ e ρ e ρ
*1 1 1 2 12 3 123 1 1 2 12 3 123 1*2 2 12 3 123 2 12 3 123 03
*3 123 3 123 033
:3 3 :3 1:3 1
1
0
0
x
y
a s a s a s a c a c a c
a s a s a c a c f
a s a c f
A xb
Adv.: Reduced size of 33 (instead of 99) for inverse dynamics
Subsystem Recursive Method • Cut-open in a suitable location • Make serial systems • Apply serial-chain methods
( )I I I I I C I I θ C θ τ τ
( )II II II II II C II I θ C θ τ τ
Subsystem equations
: 2 eqs., 2 (x, y)? [2R: done]
: 1 eq., 3 (, x, y)? [1R: done]
1 0 2 3 a a a a
#1 #3
4
3
1
1
Base, #0
#2
#1
#2
a3
2
3
1
3 2
1 4
a2
a0
a1
Base, #0
#3
4
1
4
f12 (-)
f21 ()
123
2-3
I I II II J J θ
3 123 2 12 2 121 1
2 1 2 2 3 123 2 12 2 121 1
;
I I I II II IIen ln d en ln d
I II a s a s a sa s
a c a c a ca c
A N N A N N
J J
1 2 3 a a a
[0,0] ( )
( )T
II T
II II II II II C II
J λ
I θ C θ τ τ
1( )
( )T
I I I I I C II C
J λ
Subsystem II:
Subsystem I:
System + Motion
Solve
System + Motion Driving torque, 1
Adv.: Subsystem recursion.; Maximum size: 22 (not 99 or 33); Can use existing serial-chain dyn. algo.
123
3
1
1
2
;
II
I II
II
θ
Sub-system
(link) Mass (Kg)
Length (m)
I(#1) 1.5
0.038
II(#2) 5
0.2304
II(#3) 3
0.1152
0 0.5 1 1.50
50
100
150
200
250
300
350
400
Time (s)
Jo
int
an
gle
s (
de
g)
0 0.5 1 1.5-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
0.5
Time (s)
Dri
vin
g t
orq
ue
(N
m)
3
1
2
Free: http://www.redysim.co.nr/download
Recursive Robot Dynamics
Lecture 4
Prof. S.K. Saha ME Dept., IIT Delhi
International 2013
Int.: 2009 Indian: 2013
Summary of Previous Lecture
• Dynamics (classical way) of 4-bar mechanism using FBD
• Cut-open 3+0 constrained dynamics of 4-bar
• Cut-open 2+1 constrained dynamics of 4-bar
• Inverse dynamics using ReDySim
Forward Dynamics of 4-bar Mechanism
[0,0] ( )
( )T
II T
II II II II II C II
J λ
I θ C θ τ τ
11 ( )
( )T
I I I I I C II C
J λ
Constrained equations of motion:
1
:5 1:5 5 :5 1
)
0 ) -
I I T I I I
II II T II II II
I II I I II II
I C
xA b
O (J
I -(J θ C θ
J -J O λ -J J θ
0II II
II
J -J
0θVelocity constraints:
1x=A b
+ (jt. accn.) jt.
vel. & pos.
DAE [Diff. Algeb. Eqn.) formulation: System appr.]
System-level Lagrange Multiplier Method
[0,0] ( )
( )T
II T
II II II II II C II
J λ
I θ C θ τ τ
Constrained equations of motion:
1
2
T
I J θ
J O λ
2
II II I I II II
II
J
θ
J -J -J J θθ
jt. vel. & pos. (jt. accn.)
1:3 1:3 3 :3 2
( )
0 ( )
T
I I TI I I I
II II TII II II II
I C
θI J
O J-λ
I Jθ τ -C θ
11 ( )
( )T
I I I I I C II C
J λ
Velocity constraints:
1 1 1
1 2
:2 2
( ) )T
I
λ JI J (JI
1
1( )T θ I J λ
Adv.: Inversions of smaller (33 and 22) matrices
Five-bar 2-DOF Manipulator
Base, #0
#1
2
1
2
1
a2
a0
a1 1
Cut
here
#1
2
1
2
1
a2
a0
a1 1
-
Base, #0
( )I θ C θ τ τI I I I I I
( )I θ C θ τ τII II II II II II
Subsystem equations
: 2 eqs., 1 (2 )? [2R done]
: 2 eqs., 3 (1, x, y)? [2R done]
Symmetric; Need to combine: 4 eqs., 4 (1, 2, x, y)?
2 2
Subsystem Recursion for 5-bar
Base, #0
#1
2
1
2
1
a2
a0
a1 1
Cut
here
#1
2
1
2
1
a2
a0
a1 1
-
Base, #0
( )I θ C θ τ τI I I I I I
( )I θ C θ τ τII II II II II II
Subsystem equations
: 1 eq., 1 (2 )? [1R done]
: 3 eqs., 3 (1, x, y)? [3R done]
No need to combine: Solved
2 2
Inverse Dynamics (w/o Gravity) Link lengths: 1 m; Mass of links: 6 kg
1 11 1
2[ sin( )]
2
f ii
T tt
T T
Two-DOF Parallel Manipulator
#0 O5
O3
#3 #2
#1
O4
F2
C2
C1
r2
c12
d1
O2
#4
F3
C3
C4
r3
c43
d4
O1
Cut here
P
x2
l1 l2
θ1
ao
1 2 0 l l a 0
Kinematic constraints
Link bi
(m)
θi
(m)
ai
(m)
αi
(m)
1 0 1 0 /2
2 b2 [JV] 0 0 0
Sliding velocity (constant): 0.1 m/sec.
Link mi
(kg)
ri,x, ri,y
(m)
ri,z Ii,xx, Ii,yy
(kg-m2)
1,2 655 0 .625 399
Two RP serial manipulators; Combined: 4 eqs., 4 (f1, f2, x, y)?
Three-DOF RRR Parallel Robot
Subsystem I: 5 eqs., 5 (D1, 2x, 2y, 7x, 7y )? Subsystem II: 1 eq.. 1 (D2)? Subsystem III: 1 eq.. 1 (D3)?
#0
O1
O2
Subsystem I (3-DOF)
3 eqs.; 4 (x, y, x, f1)?
O1
O4
#3
#2
#1
O3
f λ
One particular limb
f1
fI
Moving platform
O2
#4
#5 O5
#3
#2
#1
O3
O4
O6 Cut joint
Dimensionless
links C3
C2
r3
c23
d2
C1
d23
d3 14
14
2I
T
1
T * g T λ
I I I I 2 I
TI 3
1I
*
0
( ) 0 +
1
f
e a
N w w = e a f
e
A A
f
FBD of VII Subsystem
-fIλ -fII
λ
-fIIIλ
-fIVλ -fV
λ
-fVIλ
pp1 pp2
pp3
pp4 pp5
pp6
VII VII VII
c c c+ × =•
I ω ω I ω n
VII VII
cm =•
c f
VII
λ
I
c
p1 p6
λ
VI
f
n p 1 p 1
f
=
VII
c λ λ
I VI= f f f
Subsystem VII (6-DOF) 6 eqs.; Zero?
Inverse Dynamics Algorithm ;
VII
VII
6c
1 61 p1 p6
6c 1 6
VI:6 61
:6 1
:6 1
Ii
i
i
i
f
f
J
τ
h
n xs p s p
s sf y
1τ J h
;
Ref: Sadana, M., 2009, Dynamic Analysis of 6-DOF Motion Platform, M. Tech Project Report, IIT Delhi
Trajectory and actuating force
Using GUI
Conclusions • Purpose of Recursive Robot Dynamics
– Efficiency – Numerical stability
• DeNOC matrices for serial-chain • NE to EL derivations • Constrained equations of motion for serial-chain systems • Schemes for Inverse and Forward Dynamics (Simulation) • RoboAnalyzer software for robot dynamics • Parallel robot application • Four-bar mechanism from FBD • Cut-open system and subsystem recursion • ReDySim software • Five-bar, 3-DOF RRR, Stewart platform dynamics • Custom-made GUI for Stewart platform • Simulation of walking robots and rope using ReDySim
Acknowledgements
• Mr. Majid Koul
• Mr. Rajivlochana C.G.
• Dr. Suril V. Shah
• Mr. Mukesh Sadana
• Dr. Himanshu Chaudhary
• Students of IIT Delhi and other institutes who used the materials mentioned in the lectures and given feedbacks.