Movie Segment

Robotic Reconnaissance Team,

University of Minnesota,
ICRA 2000 video proceedings

Dynamics

• Rigid Body Dynamics

• Newton-Euler Formulation
• Articulated Multi-Body Dynamics
• Recursive Algorithm
• Lagrange Formulation
• Explicit Form

MA23

1
 Joint Space Dynamics

M (q )q + V (q, q ) + G (q ) = Γ
q: Generalized Joint Coordinates
M (q ) : Mass Matrix - Kinetic Energy Matrix
V (q, q ) : Centrifugal and Coriolis forces
G (q) : Gravity forces
Γ: Generalized forces

Formulations n f n f
Newton-Euler Lagrange
Fi -ni+1
Ni
-n
I Ci -f link 3
-fi+1 n3
-ni mi
Link i Kinetic Energy: Ki∑ τ3 f3
-fi Potential Energy i
-f3 -n3
Newton: m v C = F Generalized Coordinates n2
Euler: N i = I C ω i + ω i × I C ωi τ2 -f2 link 2
1 T
i i
K = q M q f1 f2
Eliminate Internal Forces 2 τ1 -n1 -n2
τi =
RS n . Zi
T
i revolute M q + V + G = τ
Tf .Z
i
T
i prismatic
-f1 n1 link 1

-fi+1 Newton-Euler Algorithm

-ni+1
Ni Fi
s and Inertial f
a tion or
Link i eler ces
I Ci , acc
ies
mi Pi+1
lo cit
Ve
es
forc
Recursive Equations
ni fi fi = Fi + fi +1
ni = N i + ni +1 + p Ci × Fi + p i +1 × fi +1

τi =
RS
ni . Zi revolute
T
fi . Zi prismatic

2
 a particle Angular Momentum
Newton’s Law F F
m m v = F m
F = ma a take the moment /0 p

p × m v = p × F
0 0
inertial v inertial mv
Frame Frame
N

=0
d
( p × mv ) = p × mv + v × mv = p × mv
d dt
( mv ) = F rate of change of the d
dt linear momentum is equal (p × m v ) = N
to the applied force dt
Linear Momentum applied moment
ϕ = mv angular momentum φ = p × mv

Rigid Body vi = ω × pi
Rotational Motion
ω
mi φ = ∫ p × (ω × p ) ρ dv
V
pi p × (ω × p ) = p ( − p )ω

Angular Momentum = ∑ p i × mi v i
i
z
φ = [ − pp
  ρdv ]ω
V
Inertia Tensor
φ = ∑ mi p i × (ω × p i )
i φ = Iω
mi → ρdv ( ρ: density )
φ = ∫ p × (ω × p ) ρ dv
V

Inertia Tensor
Linear Momentum Angular Momentum
z
  ρdv
I = − pp ( − pp
  ) = ( p T p ) I3 − pp T

LMx OP I = z [(p p) I − pp ]ρdv

ϕ = mv φ = Iω V T
3
T

V
Newton Equation Euler Equation
MM z PP
p = y ;p p = x + y + z
T 2 2
F1
2
0 0 I
NQ (p p) I = ( x + y + z )G 0 JJ
d d
( mv ) = F ( Iω ) = N GH 0
T 2 2 2
1 0
1K
3
dt dt
LM x OP LM x xy xzOP 2 0
pp = y a x y z f = M xy y yz P
ϕ = F φ = N
T
MM z PP MN xz yz z PQ
2

NQ 2

ma = F Iω + ω × Iω = N LMy + z − xy − xz OP
2 2

ˆ ˆ ) = M − xy z + x
MN − xz − yz x + y PPQ
( − pp − yz 2 2

2 2

3
Inertia Tensor
LM I xx − I xy − I xz OP Parallel Axis theorem
I = M− I
MN− I
xy I yy
− I yz
− I yz
I zz
PP
Q
z
V
  ρdv
I = − pp
{A}
p
m

zzz
xz

I xx = ( y 2 + z 2 )ρdxdydz
( − pp
  ) = ( p T p ) I3 − pp T pc R| x c U|
S| y V|
Moments of I yy = zzz ( z 2 + x 2 )ρdxdydz Tz
c

c W
zzz
Inertia
Izz = ( x + y )ρdxdydz I A = I C + m [(p C p C ) I 3 − p C p C T ]
T
2 2

I xy = zzz xyρdxdydz
I A zz = I C zz + m ( x C2 + y C2 )
Products of I xz = zzz xzρdxdydz
I A xy = I C xy + m x C y C
zzz
Inertia
I yz = yzρdxdydz

a
Example 2 Newton-Euler Algorithm
I C zz = ∫a ∫∫ ρ ( x + y 2 ) dxdydz
2
{C}
−
nd Inertia
ions a
2
l fo
I Czz =
1
ρ a5; m = ρa 3 el erat rce
, acc s
6 ies
cit
a
ma 2 Ve
lo
I Cxx = I Cyy = I Czz =
es
6 forc
a
A
x c = yc = z c =
A A
{A} 2
ma2 2 2
I Axx = I Ayy = I Azz = ICzz + = ma
2 32
ma
I A xy = I A xz = I A yz =
4

Newton-Euler Equations Angular Acceleration

ωι+1
Translational Motion m . v C = F
m {i}
m v C = F IC
ωι
ω i +1 = ω i + Ω i +1
Rotational Motion
Ω i +1 = θ i +1 Zi +1
I C ω + ω × I C ω = N
b
ω i +1 = ω i + θ i +1 ω i × Z i +1 + θ i +1 Zi +1 g

4
 Linear Acceleration Velocity and Acceleration
Zι+1
Zι at center of mass
pι+1
ωι xι+1 ωι+1 vCi+1
vi+1 = vi +ωi × pi+1 +Vi+1
aι
xι vi+1 Ci+1
{i+1} p Ci+1
Vi +1 = di +1 Zi +1
Pi +1 = ai x i + di +1 Z i +1 v Ci +1 = v i +1 + ω i +1 × p Ci +1
v i +1 = v i + ω i × p i +1 + ω i × p i +1 + Vi +1 v Ci +1 = v i +1 + ω i +1 × p Ci +1 + ω i +1 × (ω i +1 × p Ci +1 )
v i +1 = v i + ω i × p i +1 + ω i × (ω i × p i +1 )
+ 2 d ω × Z + d Z
i +1 i i +1 i +1 i +1

Dynamic forces on Link i − fi +1

− fi +1 − ni +1
ICiω i + ωi × ICiωi − ni +1
p i +1
mi vCi fi Fi
ni Link i
Ni
mi v Ci = ∑ forces fi pCi
I C iω i + ω i × I C iω i = ∑ moments / ci Fi = fi − fi +1
ni
Inertial forces/moments N i = ni − ni +1 + ( − p Ci ) × fi + (p i +1 − p Ci ) × ( − fi +1 )
Fi = mi v Ci
N i = I C iω i + ω i × I C iω i

Newton-Euler Algorithm Recursive Equations

fi = Fi + fi +1
ni = N i + ni +1 + p Ci × Fi + p i +1 × fi +1
s and Inertial f
tion
,a
cce
lera or ces
τi =
RS
ni . Zi revolute

lo c ities T
fi . Zi prismatic
Ve Fi = mi v Ci
es with
N i = I C iω i + ωi × I C iωi
forc
where ω i +1 = ω i + Ω i +1 = ω i + θ i +1 Zi +1
ω i +1 = ω i + ω i × Zi +1θ i+1 + θi +1 Zi +1
v i +1 = v i + ω i × pi +1 + ω i × (ω i × pi +1 ) + 2di +1ω i × Zi +1 + di +1 Zi +1
v C = v i +1 + ω i +1 × pC + ω i +1 × (ω i +1 × pC )
i +1 i +1 i +1

5
Outward iterations: i : 0 5
i +1
ω i +1 = i +i1Ri ω i + θ i +1 i +1 Zi +1
i +1
ω i +1 = i +i1Ri ω i + i +i1Ri ω i × i +1Zi +1θ i +1 + θ i +1 i +1 Zi +1
i +1
v i +1 = i +i1R( i ω i × i pi +1 + i ω i ×( i ω i × i pi +1 )+ i v i )
i +1
v Ci+1 = i +1 ω i +1 × i +1 pCi+1 + i +1 ω i +1 ×( i +1 ω i +1 × i +1 pCi+1 )+ i +1 v i +1
i +1
Fi +1 = mi +1 i +1 v Ci+1 Movie Segment
i +1
Ni +1 = Ci +1
Ii +1 ω i +1 + ω i +1 ×
i +1 i +1 Ci +1
Ii +1 ω i +1
i +1

Inward iterations: i : 6 1
i
fi = i +1iRi +1fi +1 + iFi Space Rover, EPFL, Switzerland,
i i i i +1
ni = Ni + R ni +1 + p Ci × Fi + p i +1 × R fi +1
i i i i i +1 ICRA 2000 video proceedings
i +1 i +1

Gravity: set v 0 = 1G
0
τ i = i niT i Zi

Lagrange Equations
d ∂L ∂L
( )− =τ
dt ∂ q ∂q
Lgrangian Kinetic Energy
L = K −U
Potential Energy
Since U = U (q)
d ∂ K ∂ K ∂U
( )− + =τ
dt ∂ q ∂q ∂q
Inertial forces Gravity vector

Lagrange Equations Inertial forces

d ∂K ∂K 1
d ∂K ∂K ∂U ( )− =τ −G K = qT M (q) q
( )− = τ − G; G = dt ∂ q ∂q 2
dt ∂ q ∂q ∂q ∂K ∂ 1 T
= [ q M ( q ) q ] = M (q) q
Inertial forces ∂ q ∂ q 2
d ∂K d
( )= ( M q ) = M q + M q
dt ∂ q dt
M (q)q + V (q, q ) = τ − G(q) ⎡ T
⎢ q
∂M ⎤
q
∂ q1 ⎥
d ∂K ∂K ⎢
−1⎢
⎥
( )− = M q + Mq #  + V ( q , q )
⎥= M q
dt ∂ q ∂q 2⎢
∂M ⎥
⎢ q T q ⎥
⎢⎣ ∂ qn ⎥⎦

6
 ∂K ∂ 1 Equations of Motion
= M q ⎡⎢ K = mx 2 ; ( mx 2 ) = mx ⎤⎥
1
∗
∂ q ⎣ 2 ∂ x 2 ⎦ ⎡ T ∂M ⎤
⎢ q q
∂ q1 ⎥
K=
1 T
q M (q)q d ∂K ∂K ⎢
−1⎢
⎥
2 ( )− = M q + Mq #  + V ( q , q )
⎥= M q
1 dt ∂ q ∂q 2⎢
∂M ⎥
v = M 1/ 2 q → K = v T v ⎢ q T q ⎥
2 ⎣⎢ ∂ qn ⎥⎦
∂ K ∂ K ∂v
= = M 1/ 2 v = Mq
∂ q ∂ v ∂ q M (q)q + V (q, q ) + G(q) = τ
1
∂ 1 T
( v v) = v M 1/ 2 M(q): K = qT M q M (q) ⇒ V (q, q)
∂v 2 2

Equations of Motion Kinetic Energy

ωi vci
Work done by external forces to
Link i bring the system from rest to its
Pci current state.

Total Kinetic Energy: K v F 1

m K = mv 2
2
1
K = ∑K Link i ≡ qT M q τ ω 1
2 K = ω T IC ω
IC 2

Equations of Motion Explicit Form Equations of Motion Explicit Form

ωi vci ωi vci

Link i Link i

Pci Pci
Generalized Coordinates q
1
K i = ( mi vCT i vCi + ω iT I C iω i ) Generalized Velocities q
2 Kinetic Energy 1
n Quadratic Form of K = qT M q
Total Kinetic Energy K = ∑Ki Generalized Velocities 2
1 T 1 n
q M q ≡ ∑ ( mi vCTi vCi + ω iT I C iω i )
i=1

2 2 i =1

7
Equations of Motion Explicit Form Equations of Motion Explicit Form
ωi vci ωi vci

Link i Link i

Pci Pci
vCi = J vi q
ωC = Jω q
1 ⎡ n ⎤
i i
1 T
1 T 1 n q M q = q T ⎢ ∑ ( mi J vT J v + J ωT I C i J ω ) ⎥ q
q M q = ∑ ( mi vCT i vCi + ωiT I Ci ωi ) 2 2 ⎣ i =1 i i i i
⎦
2 2 in=1
1 n
= ∑
2 i =1
( mi q T J vTi J vi q + q T J ωTi I C i J ωi q ) M = ∑ ( mi J vTi J vi + J ωTi I C i J ωi )
i =1

Equations of Motion Explicit Form

ωi vci

⎡ m11 m12 " m1n ⎤ Link i

⎢m m22 " m2 n ⎥⎥
Pci
vCi = J vi q
M ( q ) = ⎢ 21 ωC = Jω q
( n×n )
⎢ # # # # ⎥ i i

⎢ ⎥ ∂ pC ∂ pC ∂ pC
⎣⎢ mn1 mn 2 " mnn ⎦⎥ Jv = [ " 0 0 " 0]
i i i
i
∂ q1 ∂ q2 ∂ qi
Jω = [ε1 z1 ε 2 z2 " ε i zi 0 0 " 0]
i

∂m11
Vector V ( q, q ) Centrifugal & Coriolis Forces Vector V ( q, q ) ∂M
∂ q1
∂q1
LMq FG m m121 IJ q OP
Lq M q OP = FG m IJ
111

MM FH mm KP
T
T
m 12
 q − 1 M
V=M
11
q −
1 m221
2 Nq M q Q H m K IJ q P
q1 121
T
m 22
MNq GH m
m122
K PQ
q2 12 2 T 112

122 m222
m 11 = m111q1 + m112q2

⎛ m11 m12 ⎞ ⎡q1 ⎤ ⎡v1 ⎤ ⎡ g1 ⎤ ⎡τ1 ⎤ ⎡1 ⎤ ∂ m 22

1
⎢ 2 (m111 + m111 − m111 ) 2 (m122 + m122 − m221 ) ⎥ ⎡ q12 ⎤
⎜ ⎟⎢ ⎥ + ⎢ ⎥ + ⎢ ⎥ = ⎢ ⎥ V (q, q ) = ⎢ ⎥⎢ 2⎥ ∂ q2
⎣τ 2 ⎦
⎢ 1 (m + m − m ) 1 (m + m − m )⎥ ⎣ q2 ⎦
⎝ 12
m m22 ⎠⎣q2 ⎦ ⎣v2 ⎦ ⎣ g2 ⎦ ⎣⎢ 2
211 211 112
2
222 222 222
⎦⎥

+
LM m 112 + m121 − m121 OP q q
Nm 212 + m221 − m122 Q 1 2

8
 ∂mij
Christoffel Symbols ∂qk Potential Energy
1
bijk = ( mijk + mikj − m jki )
Ci
Link i mi
2
V=
LMb 111 b122 q OP LM OP LM
2
1
+
2 b112 OP
q q
pCi
hi
g

Nb 211 b222 q QN Q N
2
2
2 b212 1 2 Q U i = mi g0 hi + U 0
C (q ) B(q )
LMb 1,11 b1, 22 " b1,nn OP LMq OP
2
1 U i = mi ( − g T p Ci ); U = ∑ Ui
C(q)[q ] = M
MM #
b 2 2 ,11 b2, 22 " b2,nn
PP MMq PP
2
2
Gravity Vector
∂U n ∂p C i
# # #
PQ MNq# PQ Gj = = − ∑ ( mi g T ) i

F I
( n×n ) ( n ×1 )

Nb bn, 22 " bn,nn n2 ∂q j i =1 ∂q j m1g

G J
n ,11

LM2b 2 b1,13 " 2 b1,( n −1) n q1q2 OPLM OP

 ] = M
2b
1,12

2 b2 ,13 " 2 b2 ,( n −1) n q1q3

PPMM PP G = − J vT1 d J vT2 " J vTn iGG m# gJJ 2

GH m gJK
2 ,12
B(q ) [qq
( n×
( n −1 ) n
MM #( n−1 ) n
×1 )
# # # #
PQMN PQ
N2 b
) (

2 bn,13 " 2 bn,( n −1) n q( n −1) qn

2 2
n
n ,12

Gravity Vector

c3 g
m2
c2 IC2
cn
m3g
c1 l1
y0
m2g mng
m1
m1g d2
IC1
θ1
x0
G = − ( J vT1 ( m1 g ) + J vT2 ( m2 g ) +"+ J vTn ( m n g ))

Matrix M The matrices Jω and Jω 2 are given by

1

M = m J J v1 + J ω1 I C1 J ω1 + m2 J J v2 + J ω2 I C2 J ω2
T
1 v1
T T
v2
T
Jω 1 = ∈1 z1 0 = and Jω 2 = ∈1 z1 ∈2 z 2
Jv1 and Jv : direct differentiation of the vectors: Joint 1 is revolute and joint 2 is prismatic:
g
Ll c O Ld c O
2
m2
⎡0 0⎤
IC2

= Ml s P; and = Md s P
1 1 2 1
g
MM 0 PP MM 0 PP = ⎢⎢ 0 0 ⎥⎥
0 0 m2 l1
p C1 p C2
1 1 2 1 IC2 1
J ω1 = 1 J ω 2 y0I m1
N Q N Q l1
⎢⎣1 0 ⎥⎦ θ1
C1
d2

In frame {0}, these matrices are: y0 m1

d2
And x0
IC1
⎡−l1s1 0⎤ ⎡−d2s1 c1⎤ ⎡I 0⎤ ⎡I 0⎤
θ1 x0
1 T1
( Jω1 I C1 1
Jω1 ) = ⎢ zz1 ⎥ ; and ( 1JωT2 1IC2 1Jω2 ) = ⎢ zz 2 ⎥
0
Jv1 = ⎢⎢ lc ⎥ 0 ⎢ ⎥
1 1 0⎥; and Jv2 = ⎢ d2c1 s1⎥
⎣ 0 0 ⎦ ⎣ 0 0 ⎦
⎢⎣ 0 0⎥⎦ ⎢⎣ 0 0⎥⎦
This yields
Lm l 0OP; and m ( J
2
Lm d 2
OP
Finally,
M=
LMm l 2
11 + I zz1 + m2 d22 + I zz 2 OP 0
)=M )=M
0
N m Q
0 T 0 11 0 T 0 2 2
m1 ( J J v1 J v2
v1
N 0 0Q 2 v2
N 0 m2 Q 0 2

9
Centrifugal and Coriolis Vector V Vector V
LM2m d OP θ d + LM 0 0OP LMθ OP. 2
1
c h V=
2 2 1
bi , jk =
2
mijk + mikj − m jki
N 0 Q N− m d 0Q Nd Q
1 2
2 2
2
2
∂ m ij The Gravity Vector G
where m ijk = ; withbiii = 0 and biji = 0 for i > j
∂q k
G = − J vT1 m1g + J vT2 m2 g .
For this manipulator, only m11 is configuration dependent
- function of d2. This implies that only m112 is non-zero, In frame {0}, g
0
= 0 a −g 0 f T
and the gravity vector is
m112 = 2 m 2 d 2 .
OP LM− m0 gOP − LM− d s OP LM OP
g m2
Matrix B LM2b OP = LM2m d OP.
IC2
LM−l s l1c1 0 d2 c1 0
0

0Q M
MN 0 PPQ N c Q MMN PP
G=− − m2 g
0 1 1 2 1
B= l1
N0
112 2 2

N0Q N 0 Q y0I m1 0 1
1 s1 0
0 Q
Matrix C
C=M
L 0 b OP = LM 0 122 0OP θ C1
d2
and
Nb 0 Q N − m d 211 2 2 0 Q 1
.
x0 0
G=
LMbm l + m d ggc OP
11 2 2 1

Matrix V L2m d OP θ d + LM 0 0 O Lθ O

2
N m gs Q
0 PQ MNd PQ
V=M
2 2 1 2 1

N 0 Q N− m d 1 2
2 2
2
2
.

Equations of Motion

LMm l + I + m d + I 0 OP LMθ OP
2
11 zz1 2
2
2 zz 2 1

N 0 m Q Nd Q 2 2

+M
L2m d OP θ d + LM 0 0OP LMθ OP
2 2
2
1 g m2

N 0 Q N− m d 0Q Nd Q
1 2
IC2
2
2 2 2 l1

Lbm l + m d ggc OP = Lτ O. y0I m1

d2
+M
N m gs Q MNτ PQ
11 2 2 1 1 C1

2 1 2
θ1 x0

10