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

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 Newton-Euler
Ni
-ni

Lagrange

Fi

-ni+1 -fi+1

I Ci
mi
Link i

-fi Newton: m v C = F Euler: N i = I C i + i I C i i i Eliminate Internal Forces

Ki Kinetic Energy: Potential Energy i Generalized Coordinates

3 2 1

-f n3 -f3

-n link 3 f3 -n3 link 2 f2

K =

i =

R n .Z Sf .Z T
T i T i i

1 T q Mq 2

revolute prismatic

M q +V + G =

-n1 -f1

f1 n1

n2 -f2

-n2 link 1

-fi+1
Ni Fi

Newton-Euler Algorithm -ni+1

, ies cit

Link i
mi

I Ci
Pi+1 Recursive Equations fi
fi = Fi + fi +1
Ve lo

a eler acc

s and Inertial f tion or

ces

ni

es forc

ni = N i + ni +1 + p Ci Fi + p i +1 fi +1 ni . Zi revolute i = fi . Zi prismatic

R S T

Newtons Law

F
0 inertial Frame

F = ma

a particle m a v

Angular Momentum

mv = F
take the moment /0

F
p
0 inertial Frame

p mv = p F
N

mv

d ( mv ) = F dt
Linear Momentum

rate of change of the linear momentum is equal to the applied force

d (p mv ) = p mv + v mv = p mv dt
d (p m v ) = N dt

=0

= mv

angular momentum

= p mv

applied moment

Rigid Body
Rotational Motion

vi = pi
mi

pi

p ( p ) = p( p )

p ( p ) dv

Angular Momentum =

p
i

mi v i

= [ ppdv ]
V

= mi p i ( p i )

Inertia Tensor

mi dv

( : density )

= I

p ( p ) dv

Inertia Tensor
Linear Momentum Angular Momentum

= mv
Newton Equation

= I

d ( mv ) = F dt

Euler Equation d ( I ) = N dt

=F
ma = F

=N I + I = N

LMx OP I = z [(p p) I pp ]dv p = y ;p p = x + y + z F1 MM z PP (p p) I = ( x + y + z )G 0 NQ GH 0 LM x xy xzOP LM x OP pp = y a x y z f = M xy y yz P MM z PP MN xz yz z PQ NQ LMy + z xy xz OP yz ( pp ) = M xy z + x MN xz yz x + y PPQ

V
T T 3 V
T 2 2 2

I = ppdv

( pp ) = ( p T p ) I3 pp T

0 0 1 0 0

I JJ 1K

Inertia Tensor

LM I I = M I MN I

xx xy xz

I xy I yy I yz

I xx = I yy = Izz = I xy =

Moments of Inertia

Products of Inertia

I xz = I yz =

zzz zzz zzz zzz zzz zzz

I xz I yz I zz

( y 2 + z 2 )dxdydz ( z 2 + x 2 )dxdydz ( x + y )dxdydz

2 2

OP PP Q

Parallel Axis theorem

I = ppdv
V

{A}

( pp ) = ( p T p ) I3 pp T

m
pc

Rx |y S |z T

c c c

U | V | W

I A = I C + m [(p C p C ) I 3 p C p C T ]
T
2 2 I A zz = I C zz + m ( x C + y C )

xydxdydz xzdxdydz yzdxdydz

I A xy = I C xy + m x C y C

Example
{C}

I C zz =

a ( x
2

a 2

+ y 2 ) dxdydz

Newton-Euler Algorithm
el acc
erat
nd Inertia l fo ions a

I Czz =
a

1 a5; 6

m = a 3

I Cxx = I Cyy = I Czz =

A

{A}

a x c = yc = z c = 2
A A

ma 2 6

Ve

lo

, ies cit

rce s

es forc

I Axx = I Ayy = I Azz = ICzz +

I A xy = I A xz

ma2 2 2 = ma 2 32 ma = I A yz = 4

Newton-Euler Equations
Translational Motion
m. v C = F

Angular Acceleration
+1
m

mvC = F
Rotational Motion

IC

{i}

i +1 = i + i +1

I C + I C = N

i + 1 = i + i + 1 i Z i + 1 + i + 1 Z i +1

i +1 = i +1 Z i + 1

Linear Acceleration
Z x Z+1 p+1 a

Velocity and Acceleration at center of mass

+1 {i+1}

vi+1 = vi +i pi+1 +Vi+1

x+1

vi+1

vCi+1

Vi +1 = di +1 Zi +1 Pi +1 = ai x i + di +1 Z i +1
v i +1 = v i + i p i +1 + i p i +1 + Vi +1
v i +1 = v i + i p i +1 + i ( i p i +1 )

p Ci+1

Ci+1

v Ci +1 = v i +1 + i +1 p Ci +1
v Ci +1 = v i +1 + i +1 p Ci +1 + i +1 ( i +1 p Ci +1 )

+ 2 di +1 i Z i +1 + di +1 Z i +1

Dynamic forces on Link i

ICii + i ICii
mi v Ci = forces

fi +1
Link i

fi +1 ni +1
mi vCi
Fi = fi fi +1

ni +1

fi I C i i + i I C i i = moments / ci
Fi = mi v Ci N i = I C i i + i I C i i

ni

fi

p i +1

Fi

pCi
ni

Ni

Inertial forces/moments

N i = ni ni +1 + ( p Ci ) fi + (p i +1 p Ci ) ( fi +1 )

Newton-Euler Algorithm
lera cce
s and Inertial f tion or

Recursive Equations
fi = Fi + fi +1 ni = N i + ni +1 + p Ci Fi + p i +1 fi +1 ni . Zi revolute i = fi . Zi prismatic
with where

Ve

lo

,a es iti c

ces

es forc

N i = I C ii + i I C ii i + 1 = i + i +1 = i + i + 1 Z i + 1

Fi = mi v Ci

R S T

i +1 = i + i Zi +1 i+1 + i +1 Zi +1
vi +1 = vi + i pi +1 + i ( i pi +1 ) + 2di +1 i Zi +1 + di +1 Zi +1 v Ci+1 = vi +1 + i +1 pCi+1 + i +1 ( i +1 pCi+1 )

Outward iterations: i : 0
i +1

i +1 = i +i1Ri i + i +1 i +1 Zi +1

i +1 i +1 i +1 i +1 i +1

i +1 = i +i1Ri i + i +i1Ri i i +1Zi +1 i +1 + i +1 i +1 Zi +1

v i +1 = i +i1R( i i i pi +1 + i i ( i i i pi +1 )+ i v i ) 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 Fi +1 = mi +1 i +1 v Ci+1 Ni +1 =
Ci +1

Movie Segment
Space Rover, EPFL, Switzerland, ICRA 2000 video proceedings

Ii +1 i +1 + i +1
i +1 i +1

Ci +1

Ii +1 i +1
i +1

Inward iterations: i : 6
i i

1
i i i +1 i +1

fi = i +1iRi +1fi +1 + iFi ni = Ni + R ni +1 + p Ci Fi + p i +1 R fi +1

i i i +1 i i i +1

i = i niT i Zi

Gravity: set v 0 = 1G
0

Lagrange Equations d L L ( ) = q dt q
Lgrangian Kinetic Energy

L = K U

Since

U = U (q)

Potential Energy

d K K U ( ) + = q q dt q
Inertial forces Gravity vector

Lagrange Equations

U d K K ( ) = G; G = q q dt q
Inertial forces

Inertial forces d K K ( ) = G dt q q

1 K = qT M (q) q 2

M (q)q + V (q, q) = G(q)

K 1 T [ q M ( q ) q ] = M (q) q = q q 2 d K d ( )= (Mq) = M q + Mq dt q dt
d K K ( ) = q dt q
T q 1 M q + Mq 2 qT = M q + V (q, q ) M q qn

M q q1

K 1 1 = M q K = mx 2 ; ( mx 2 ) = mx 2 x 2 q
1 T q M (q)q 2 1 v = M 1/ 2 q K = v T v 2 K=

Equations of Motion
d K K ( ) = q dt q
T q 1 M q + Mq 2 qT = M q + V (q, q ) M q qn

M q q1

K K v = M 1/ 2 v = Mq = q v q
1 T ( v v) = v v 2

M (q)q + V (q, q) + G(q) =

1 M(q): K = qT M q 2
M (q) V (q, q)

M 1/ 2

Equations of Motion
i
Link i

Kinetic Energy
vci

Pci

Work done by external forces to bring the system from rest to its current state.

Total Kinetic Energy:

v
m

K =

1 K = K Link i qT M q 2

IC

1 mv 2 2 1 K = T IC 2

Equations of Motion
i
Link i

Explicit Form
vci

Equations of Motion
i
Link i

Explicit Form
vci

Pci

Pci

1 T K i = ( mi vCi vCi + iT I C i i ) 2
Total Kinetic Energy

Generalized Coordinates q Generalized Velocities q

K = Ki
i=1

1 T 1 n T q M q ( mi vCi vCi + iT I C i i ) 2 2 i =1

Kinetic Energy K Quadratic Form of Generalized Velocities

1 = qT M q 2

Equations of Motion
i
Link i

Explicit Form
vci

Equations of Motion
i
Link i

Explicit Form
vci

Pci

vCi = J vi q
C = J q
i i

Pci

1 T 1 T q M q = ( mi vCi vCi + iT I Ci i ) 2 2 in=1

=

1 T 1 n T q M q = q T ( mi J vT J v + J I C i J 2 i =1 2
i i i

) q

1 (mi qT J vTi J vi q + qT JTi I C i Ji q ) 2 i =1

T M = ( mi J vTi J vi + J i I C i J i ) i =1

Equations of Motion
i
vci
Link i

Explicit Form

m11 m M ( q ) = 21 ( nn ) mn1

m12 m22 mn 2

m1n m2 n mnn

Pci

vCi = J vi q
C = J q
i i
i i

pC pC Jv = [ q1 q2 J = [1 z1 2 z2
i i
i

pC 0 0 qi i zi 0 0

0]
0]

Vector V ( q, q )

Centrifugal & Coriolis Forces

Vector V ( q, q ) M
q1
1 Lq M q O F m V = Mq M P=G 2 Nq M q Q H m
T q1 T q2 11

m12 m22

12

m11 = m111q1 + m112

LMq FG m IJ q 1 M H m K 2 Mq FG m MN H m q
T T

m11 q1
m121 m221 m122 m222

111 121

112 122

IJ q OP KP IJ qP K PQ

m11 m12 q1 v1 g1 1 + = + m12 m22 q2 v2 g2 2

1 1 2 (m111 + m111 m111 ) 2 (m122 + m122 m221 ) q12 V (q, q) = 2 1 (m + m m ) 1 (m + m m ) q2 211 211 112 222 222 222 2 2

m 22 q2

LM m Nm

112

+ m121 m121 + m221 m122

212

OP q q Q

1 2

Christoffel Symbols
V=

LMb Nb
2
( n 1 )

111 211

b122 q b222 q
1,11 2 ,11

OP LM OP LM QN Q N
2 1 2 2

bijk =

1 ( mijk + mikj m jki ) 2 2 b112 qq + 2 b212 1 2

mij qk

Potential Energy
Link i
Ci

OP Q

mi

pCi

g
hi

( n

LMb b C(q)[q ] = M MM Nb LM2b 2b B(q ) [qq] = M MM N2 b

( nn )
( n 1 ) n ( n1 ) n ) ( 1 ) 2 2

C (q )

B(q )
b1, 22 b2, 22 bn, 22 2 b1,13 2 b2 ,13
2 bn,13

U i = mi g0 hi + U 0
Gravity Vector

b1,nn b2,nn

n ,11
1,12 2 ,12

2 bn,nn n 2 b1,( n 1) n 2 b2 ,( n 1) n

OP LMq OP PP MMq PP PQ MNq PQ

2 1 2 2

n ,12

2 bn,( n 1) n

OPLM q q OP PPMM q q PP PQMNq q PQ

1 2 1 3 ( n 1) n

U i = mi ( g T p Ci ); U = Ui i n p C U Gj = = ( mi g T ) m1g q j q j i =1
i

G = J vT1

J vT2

J vTn

F I G J iGG m gJJ GH m gJK

2 n

Gravity Vector
c3 c2 c1 m2g m1g m3g cn mng g

IC2

m2

l1
1

y0

m1

IC1

d2

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

x0

Matrix M

M = m J J v1 + J 1 I C1 J 1 + m2 J J v2 + J 2 I C2 J 2 Jv1 and Jv : direct differentiation of the vectors:

T 1 v1
2

The matrices J and

1

T v2

J 2 are given by
0 = and J 2 = z1 1
g

J 1 = z 1 1

z2 2
IC2
m2

p C1

In frame {0}, these matrices are:

Ll c O = Ml s P; and MM 0 PP N Q
1 1 1 1

p C2

Ld c O = Md s P MM 0 PP N Q
2 1 2 1

Joint 1 is revolute and joint 2 is prismatic:

IC2
m1

m2

l1

J 1 = 1 J 2
1 T1 1

This yields

l1s1 0 d2s1 c1 0 Jv1 = lc1 0; and 0Jv2 = d2c1 s1 1 0 0 0 0

0 T 0 v1

y0

1
0 T 0 v2

IC1

d2

And

0 0 = 0 0 1 0

l1

y0I

m1

d2

C 1

x0
2 2 2

( J1 I C1
0 m2

0 0 I I T J1 ) = zz1 ; and ( 1J2 1IC2 1J2 ) = zz 2 0 0 0 0

x0

m1 ( J

J v1

Lm l 0OP; and m ( J )=M N 0 0Q

2 11 2

J v2

Lm d )=M N 0

OP Q

Finally,

M=

LMm l N

2 11

2 + I zz1 + m2 d2 + I zz 2 0

OP m Q
0
2

Centrifugal and Coriolis Vector V

Vector V

bi , jk =
where m ijk

For this manipulator, only m11 is configuration dependent - function of d2. This implies that only m112 is non-zero, Matrix B

1 mijk + mikj m jki 2 m ij = ; withbiii = 0 and biji = 0 for i > j q k

V=

The Gravity Vector G

LM2m d OP d + LM 0 0OP LM OP. N 0 Q N m d 0 Q Nd Q

2 2 1 2 2 2 2 1 2 2

G = J vT1 m1g + J vT2 m2 g .

In frame {0}, g
0

LM2b OP = LM2m d OP. N0Q N 0 Q L 0 b OP = LM 0 Matrix C C=M Nb 0 Q N m d Matrix V L2m d OP d + LM 0 V=M N 0 Q N m d

B=
112 2 2

m112 = 2 m 2 d 2 .
l1

= 0
1 1

g
0

0
1

and the gravity vector is

2 1

IC2
m1

m2
0

122

211

0 . 0

1 2

OP Q 1 0 O L O 0 P Md P Q N Q.
2 1 2 2

y0I

G=

d2

C 1

x0

and
0

LMl s N0

l1c1 0

0 OP LM m gOP LM d s P c 0Q M NM 0 QP N
1

d2 c1 s1

0 0 m2 g 0 0

OP LM Q MNM

OP PP Q

G=

LMbm l + m d ggc OP N m gs Q
11 2 2 1 2 1

Equations of Motion

LMm l + I + m d + I 0 OP LM OP 0 m Q Nd Q N L2m d OP d + LM 0 0OP LM OP +M N 0 Q N m d 0 Q Nd Q

2 11 zz1 2 2 2 zz 2 1 2 2 2 2 1 2 2 2 2 1 2 2

g
l1

IC2
m1

m2

Lbm l + m d ggc OP = L O. +M N m gs Q MN PQ
11 2 2 1 1 2 2 1

y0I

d2

C 1

x0

10