Rigid Body Dynamics

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

MA23

Formulations
Joint Space Dynamics
Newton-Euler Lagrange
Fi -ni+1
A(q )q + b(q, q ) + g ( x) = Ni
I Ci
q: Joint Coordinates -fi+1
-ni mi
Link i Kinetic Energy: Ki
A( q) : Kinetic Energy Matrix -fi Potential Energy i
b(q, q ) : Centrifugal and Coriolis forces Newton: m v C = F Generalized Coordinates
Euler: N i = I C  i + i I C i 1 T
g (q ) : Gravity forces i i
K = q M q
Eliminate Internal Forces 2
: Generalized forces
i =
RS n . Zi
T
i revolute M q + b + g =
Tf .Z
i
T
i prismatic

1
 f n f
n -fi+1
-ni+1

-n Link i
-f link 3
n3 Pi+1
3 f3
-f3 -n3 ni fi
n2
2 -f2 link 2
f1 f2
1 -n1 -n2
-f1 n1 link 1

Newton-Euler Algorithm
Newtons Law
F a particle
m
l erat
dI
ions an nertialfo
rce
F = ma
cce s
a
,a
ies
lo cit
Ve inertial
es
forc
Frame

a particle Angular Momentum

Newtons Law F F
m mv = F m
F = ma a take the moment /0 p
0 0
inertial v p m v = p F inertial mv
Frame Frame
N
=0

d
( p mv ) = p mv + v mv = p m v
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
angular momentum

2
 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   ) = ( p T p ) I 3 pp T
( pp
= mv = I
LM OP
x
V
z
I = [( p T p) I3 pp T ]dv
V
Newton Equation Euler Equation
MM PP ;p p = x + y + z
p= y T 2 2 2
F1 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
z 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

I + I = N L y + z xy xz OP
2 2

) = MM xy z + x
MN xz yz x + y PPQ
( pp yz 2 2

2 2

Inertia Tensor
LM I xx I xy I xz OP Parallel Axis theorem
I = M I I yy I yz PP
MN I
xy

I yz I zz Q {A}
p
m

zzz
xz

I xx = ( y 2 + z 2 )dxdydz
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 2 + y 2 )dxdydz I A = I C + m [(p C T p C ) I 3 p C p C T ]
I xy = zzz xydxdydz
I A zz = I C zz + m ( x C2 + y C2 )
Products of I xz = zzz xzdxdydz
I A xy = I C xy + m x C y C
zzz
Inertia
I yz = yzdxdydz

3
 a
Example 2 Newton-Euler Algorithm
I C zz = a ( x + y 2 ) dxdydz
2
{C}

and Inertial
tions
2
f or
I Czz =
1
a5; m = a 3 l era ces
cce
,a
6 ies
cit
a
ma 2 Ve
lo
I Cxx = I Cyy = I Czz =
es
6 forc
a
A
x c = yc = zc =
A A
{A} 2
ma2 2 2
I Axx = I Ayy = I Azz = ICzz + = ma
4 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 Z i +1
I C  + I C = N
b g
 i +1 =  i +  i +1 i Z i +1 +  i +1 Z i +1

Linear Acceleration Velocity and Acceleration

Z+1
Z at center of mass
p+1
x+1 +1 vCi+1
a
x vi+1 = vi +i pi+1 +Vi+1 {i+1}
vi+1 Ci+1
p Ci+1
Vi +1 = di +1 Z i +1
Pi +1 = ai x i + d i +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

4
 Dynamic forces on Link i fi +1
fi +1 ni +1
ICi i + i ICii 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 = m i v Ci
N i = I C i i + i I C i i

Newton-Euler Algorithm Recursive Equations

fi = Fi + fi +1
and Inertial
ni = N i + ni +1 + p Ci Fi + p i +1 fi +1
tions
,a
cce
l era
f or
ces
i =
RS
ni . Zi revolute

lo cit
ies T
fi . Zi prismatic
Ve Fi = mi v Ci
es with
N i = I C i i + i IC ii
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 Ci+1 = v i +1 +  i +1 pCi+1 + i +1 ( i +1 pCi+1 )

Outward iterations: i : 0 5 Lagrange Equations

i +1
i +1 = i +i1Ri i +  i +1 i +1 Zi +1
 i +1 = i +i1Ri  i + i +i1Ri i i +1Zi +1 i +1 +  i +1 i +1 Zi +1 d L L
=
i +1
( )
i +1
v i +1 = i +i1R( i  i i pi +1 + i i ( i i i pi +1 )+ i v i ) dt q q
i +1 Lgrangian Kinetic Energy
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
L = K U
Fi +1 = mi +1 i +1 v Ci+1 Potential Energy
i +1
Ni +1 = Ci+1 Ii +1 i +1 i +1 + i +1 i +1 Ci+1 Ii +1 i +1 i +1 Since U = U (q)
Inward iterations: i : 6 1 d K K U
i
fi = i +1iR i +1fi +1 + iFi ( ) + =
i
ni = iN i + i +1iR i +1ni +1 + i p Ci iFi + i p i +1 i +1iR i +1fi +1
dt q q q
Gravity: set v 0 = 1G
0 Gravity vector
i = i niT i Zi Inertial forces

5
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

K 1 Equations of Motion
= M q K = mx 2 ; ( mx 2 ) = mx
1

q 2 x 2 T M
q q
q1
1 T d K K
K= q M (q)q 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

K = mv 2
2
1
K = K Link i qT M q 1
2 K = T IC
IC 2

6
Equations of Motion Explicit Form Equations of Motion Explicit Form
i vci i vci

Link i Link i

Pci Pci
Generalized Coordinates q
1 Generalized Velocities q
K i = ( mi vCT i vCi + iT I C i i )
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

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 vCTi vCi + iT C I i i ) 2 2 i =1 i i i i

2 2 i n=1
1 n
= (mi q T J vTi J vi q + q T JTi I C i Ji q )
2 i =1
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
( nn )
# # # # 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

7
 m11
Vector V ( q, q ) Centrifugal & Coriolis Forces Vector V ( q, q ) M
q2
q1
LMq FG m m121 IJ q OP
LM OP FG
111

IJ MM FH mm KP
T

1 q Mq1 q m 11 m 12
T
1 m221
V = M q = q
Q H K IJ q P
121
T
N
2 q Mq2 q m 12 m 22
MNq GH m
m122
K PQ
2 T 112

122 m222
m 12 = m111 q1 + m112 q 2
m22
m11 m12 q1 v1 g1 1 LM q1
OP L O
 + + =
1 1
( m111 + m111 m111 ) ( m122 + m122 m221 ) q 2
12
m m22 q2 v2 g2 2 MM
V (q, q ) = 2
1
( m211 + m221 m112 )
2
1
1

( m222 + m222 m222 ) q2

2 PP MN PQ
N2
LM
2
m112 + m121 m121 OP Q
+ q q
N
m212 + m221 m122 1 2 Q

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 Q
2 b212 1 2
U i = mi g0 hi + U 0
C (q ) B(q )
LM b 1,11 b1,22 " b1,nn q12 OP LM OP U i = mi ( g T p Ci ); U = Ui
C (q )[q ] = M
MM #
b 2 2 ,11 b2,22 " b2,nn q 22
PP MM PP Gravity Vector
U n p C i
# # # #
PQ MN PQ Gj = = ( mi g T ) i

F I
( n n ) ( n 1 )

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

G J
n,11

LM2b OPLM q q OP
iGG m# gJJ
2 b1,13 " 2 b1,( n 1) n

 ] = M
2b
1,12

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

PPMM q #q PP G = JvT1 d JvT2 " JvTn

2

GH m gJK
2 ,12 1 3
B(q) [qq
( n
( n1 ) n
MM #
( n 1 ) n
1 )
# # #
PQMNq q PQ
N2 b
) (
2 2

n ,12 2 bn,13 " 2 bn ,( n 1) n ( n 1) n

n

Gravity Vector Gravity Vector

c3
c2
cn
m3g
c1
m2g mng

m1g

G = ( J vT1 ( m1 g ) + J vT2 ( m2 g ) +"+ J vTn ( m n g ))

G = ( J vT1 ( m1 g ) + J vT2 ( m2 g ) +"+ J vTn ( m n g ))

8
 Matrix M
M = m1 J vT1 J v1 + JT1 I C1 J1 + m2 J vT2 J v2 + JT2 I C2 J2
g
m2 Jv1 and Jv : direct differentiation of the vectors:
LM OP
LM OP
C 2
I2 l1c1 d 2 c1
l1 0
MM 0 PP
MM 0 PP
p C1 = l1s1 ; and 0 p C2 = d2 s1
y0 m1 N Q
N Q
d2 In frame {0}, these matrices are:
C
I1 Ll c 0 O L d s c O
J = M l c 0 P; and J = M d c s P
1 1 1 2 1 1

x0 MM 0 0PP
0
v1 MM 0 0 PP
1 1
0
v2 2 1 1

This yields
N Q N Q
m ( J J )=M
0 T 0 L m l 0O
P
2
; and m ( J J ) = M
11 Lm d 0 T 0 2
2
2 0 OP
1 v1
N v1
0 0 Q N 0 2 v2 v2
m2 Q

Centrifugal and Coriolis Vector V

The matrices J 1 and J 2 are given by
J 1 = 1 z1 0 = and J 2 = 1 z1 2 z 2 bi , jk =
1
2
c
mijk + mikj m jki h
Joint 1 is revolute and joint 2 is prismatic: m ij
where m ijk = ; withbiii = 0 and biji = 0 for i > j
qk
0 0
For this manipulator, only m11 is configuration dependent
1
J 1 = 1 J 2 = 0 0 - function of d2. This implies that only m112 is non-zero,
m112 = 2 m2 d2 .
And 1 0
I 0 I 0 Matrix B LM OP LM OP
B=
2 b112
=
2 m2 d 2
( 1JT11I C1 1J1 ) = zz1 ; and ( 1JT2 1IC2 1J2 ) = zz 2 N Q N Q 0 0
.

0 0 0 0 Matrix C L
C=M
0 b O L 0 0 OP
Nb 0 PQ = MNm d
122

LMm l OP Q
.
Finally, 0
2
+ I zz1 + m2 d + I zz 2 2
0 211 2 2

M=
N
11

0
2

m2 Q Matrix V L2m d OP  d + LM 0
V=M
2 2 OP LM OP
0  12
N 0 Q N m d 1 2
2 2 0 d 2QN Q
.
2

Vector V
LM2m d OP  d + LM 0 0OP LM OP. 2
Equations of Motion
V=
LMm l OP LM OP
2 2 1

N 0 Q N m d 0Q Nd Q
1 2
2 2
2 2
11 + I zz1 + m2 d22 + I zz 2 0 1

N m Q Nd Q
2

The Gravity Vector G 0 2 2

G = J vT1 m1g + J vT2 m2 g .

+
LM2m d OP  d
2 2 L 0 0OP LM OP
+M
2
1

In frame {0}, g
0
= 0 a g 0 f T
and the gravity vector is
N 0 Q 1 2
N m d 0Q Nd Q
2 2
2
2

LMbm l + m d ggc OP = L O.
LMl s OP LM m0 gOP LMd s 0
OP LM OP
N m gs Q MN PQ
1
l1c1 0 d2 c1 0 + 11 2 2 1

0Q M
MN 0 PPQ N c Q MMN PP
G= m2 g
0 1 1 2 1

N0 0 1
1 s1 0
0 Q
2 1 2

and

0
G=
LMbm l + m d ggc OP
11 2 2 1

N m gs Q 2 1