Ch.

6: Dynamics

Dynamics Overview

• We want to come up with equations of motion for any nDOF system

– In general, this will consist of n coupled second order ODEs
• These systems may be:
– Linear or nonlinear
– Conservative or nonconservative
• We want to develop an expression of the form: q = f ( q, t )
• Once we have this, we can use it to choose an appropriate controller
that will put our dynamical system in a desired state (configuration)

2
 Terminology

• Definitions
– Generalized coordinates: any minimal set of independent
parameters sufficient to completely specify the configuration of
a system
– Vector norm: measure of the magnitude of a vector
• 2-norm:
x 2 = x = x12 + x2 2 + ... + xn 2 , x ∈ R n
= xT x
– Inner product:
• Dot product for arbitrary vector spaces
x,y = y T x x, y ∈ R n
• Note that: 2
x,x = xT x = x
• Note also that the inner product is zero if the two vectors
are orthogonal
3

Euler-Lagrange Equations

• We can derive the equations of motion for any nDOF system by

using energy methods
– All we need to know are the conservative (kinetic and potential)
and non-conservative (dissipative) terms
• This is a shortcut to describing the motion of each particle in a rigid
body along with the constraints that form rigid motions
• For this, we need to first use virtual displacements subject to
holonomic constraints, then use the principle of virtual work, then
finally use D’Alembert’s Principle to derive the Euler-Lagrange
equations of motion
• But first, an example…

4
 Euler-Lagrange Equations

• To illustrate, we derive the equations of motion for a 1DOF system

– Consider a particle of mass m
– Using Newton’s second law:
my = f − mg
– Now define the kinetic and potential energies:
1
K = my 2 P = mgy
2
– Rewrite the above differential equation
• Left side:
d d ∂ ⎛ 1 2 ⎞ d ∂K
my = ( my ) = ⎜ my ⎟ =
dt dt ∂y ⎝ 2 ⎠ dt ∂y
• Right side:
∂ ∂P
mg = ( mgy ) =
∂y ∂y
5

Euler-Lagrange Equations

• Thus we can rewrite the initial equation:

d ∂K ∂P
= f −
dt ∂y ∂y
• Now we make the following definition:
L=K −P
• L is called the Lagrangian
– We can rewrite our equation of
motion again:
d ∂L ∂L
− = f
dt ∂y ∂y
• Thus, to define the equation of motion for this system, all we need
is a description of the potential and kinetic energies

6
 Euler-Lagrange Equations

• If we represent the variables of the system as generalized

coordinates, then we can write the equations of motion for an
nDOF system as:
d ∂L ∂L
− = τi
dt ∂qi ∂qi
• We will come back to this, but it is important to recognize the form
of the above equation:
– The left side contains the conservative terms
– The right side contains the non-conservative terms
• This formulation leads to a set of n coupled 2nd order differential
equations

Ex: 1DOF system

• Single link, single motor coupled by a drive shaft

– θm and θl are the angular displacements of the shaft and the link
respectively, related by a gear ratio, r:
θ m = rθl
– Start by determining the kinetic and potential energies:
1 1
K = J mθm 2 + J lθl 2
2 2
1 2
( )
= r J m + J l θl 2
2
MgL
P= (1 − cosθl )
2
– Jm and Jl are the motor/shaft and link inertias respectively. M and
L are the mass and length of the link respectively

8
 Ex: 1DOF system

• Let the total inertia, J, be defined by:

J = r 2 J m + Jl
• Now write the Lagrangian:
1 MgL
L = Jθl 2 − (1 − cosθl )
2 2
• Thus we can write the equation of motion for this 1DOF system as:
MgL
Jθl + sin θl = τ l
2
• The right side contains the non-conservative terms such as:
– The input motor torque: u = rτ m
– Damping torques: B = rBm + Bl
• Therefore we can rewrite the equation of motion:
MgL
Jθl + Bθl + sin θl = u
2 9

Holonomic constraints

• Motivation: reduce the number of DOFs of the system

– i.e. reduce the number of coordinates necessary to fully describe
a system of particles
• Consider a set of k independent particles r1, r2, …, rk:
– Requires 3k generalized coordinates to fully describe the set

• If we add constraints, the number of DOFs (and hence generalized

coordinates) is reduced
10
 Holonomic constraints

• Without constraints, we can simply write: mri = fi

– i.e. k uncoupled differential equations
• If there are constraints on the motion of the particles, we must also
consider constraint forces
• Example, consider a system of two particles with positions r1, r2
– If there are no constraints, we need 6 parameters to fully
describe this system
– Consider the following constraint: the distance between the
particles is constant
r1 − r2 = ( r1 − r2 )T ( r1 − r2 ) = L
– Now we can write the equations by finding all the forces that will
ensure the constraints are met

11

Holonomic constraints

• Instead, we want a method of deriving the equations of motion

without having to explicitly know the constraint forces
• Definition: a constraint on r1, r2, …, rk is holonomic if it can be
represented as:
gi ( r1 , r2 ,..., rk ) = 0, for i = 1,..., m
– In this case there are m holonomic constraints
– Differentiating gives:
k
∂g
∑ ∂r i drj = 0
j =1 j

• If there are m holonomic constraints, the system has m fewer

degrees of freedom (than the unconstrained case)
– Now we can fully represent the coordinates of the particles using
the generalized coordinates: q1, q2, …, qn

12
 Holonomic constraints

• Thus, the holonomic constraints, gi ( r1, r2 ,..., rk ) = 0, for i = 1,..., m

can be rewritten as: ri = ri ( q1 ,..., qn ) , i = 1,..., k
– i.e. the particles can be fully expressed by the n independent
generalized coordinates
• Ex: consider a rigid object, i.e. an infinite number of particles
– To specify the coordinates of each particle, we would require an
infinite # of terms
– However, there are constraints imposed by the rigid body: the
distance between any two points is fixed
– Therefore, there are a maximum of 6 independent (generalized)
coordinates needed to fully specify all the particles in a rigid
body

13

Virtual displacements

• Definition: for a system of k particles with m holonomic constraints,

the set of virtual displacements is the set: δr1,..., δrk
– These displacements are also consistent with the holonomic
constraints
• Ex: consider a system of two particles that has a fixed distance
( r1 − r2 )T ( r1 − r2 ) = L2
– Now supposed that both r1 and r2 are perturbed by the virtual
displacements δr1 and δr2
– Thus, the same constraint has to be satisfied:
( r1 + δ r1 − r2 − δ r2 )T ( r1 + δ r1 − r2 − δ r2 ) = L2
– Rearranging this gives:
( r1 − r2 + δ r1 − δ r2 )T ( r1 − r2 + δ r1 − δ r2 ) = L2
⇒ ( r1 − r2 ) ( r1 − r2 ) + 2 ( r1 − r2 ) (δ r1 − δ r2 ) + (δ r1 − δ r2 ) (δ r1 − δ r2 ) = L2
T T T

14
 Virtual displacements

• If we ignore second order terms in the virtual displacements:

( r1 − r2 )T ( r1 − r2 ) + 2 ( r1 − r2 )T (δ r1 − δ r2 ) = L2
• Now our initial constraint is:
( r1 − r2 )T ( r1 − r2 ) = L2
• Therefore:
2 ( r1 − r2 ) (δ r1 − δ r2 ) = 0
T

• This says that in order for the virtual displacements to satisfy the
holonomic constraints, they must be orthogonal to the constraint
forces.
− For example:

15

Principle of virtual work

• Due to the holonomic constraints on a system of particles, the co-

ordinates of the particles can be fully defined by a set of generalized
coordinates, then we can write the set of virtual displacements as:
n
∂r
δ ri = ∑ i δ q j , i = 1,..., k
j =1 ∂q j
– There are no constraints on the generalized coordinates, so there
are no constraints on the virtual displacements of the generalized
coordinates
• Now suppose each particle is in equilibrium
– The net force on each particle is zero
– This implies that the work done by each set of virtual
displacements is zero
k
∑ FiT δ ri = 0
i =1
• Fi is the total force acting on particle i 16
 Principle of virtual work

• At the beginning of this discussion, we said that a system with

holonomic constraints has two sets of forces acting on it:
– External forces, fi
– Forces that impose the constraints, fia
• If the work done by the constraint force is zero, we can write:
k
∑ fiaT δ ri = 0
i =1
– This is true whenever the constraint force between particles is
directed along the vector connecting the two particles
• i.e. the virtual displacements are orthogonal to the constraint
forces
• Thus when the work done by the constraint forces is zero, we can
also say that: k
∑ fiT δ ri = 0
i =1
17

Principle of virtual work

• This is called the principle of virtual work:

– The work done by external forces corresponding to any set of
virtual displacements is zero!
– Only true if the work done by the constraint forces is zero
• Which is only true if the constraint forces are directed along
the vector connecting two particles
• In this case the virtual displacement vector will be orthogonal
to the constraint force.
Ex: consider two particles that are constrained: ( r1 − r2 ) ( r1 − r2 ) = L
T 2
•
– The constraint force must be along the vector connecting the two
– Assume that there is a rigid wire connecting the two
– The constraint force acting on the first particle is: f1a = c ( r1 − r2 )
– The force on the second particle is: f 2a = −c ( r1 − r2 )

18
 Principle of virtual work

• Thus we can write the work done by the constraint forces as:
f1aT δ r1 + f 2aT δ r2 = c ( r1 − r2 ) δ r1 − c ( r1 − r2 ) δ r2
T T

= c ( r1 − r2 ) (δ r1 − δ r2 )
T

• But we already showed that this is zero since the virtual

displacements are also consistent with the holonomic constraints
– Thus the work done by the constraint forces is zero
• Thus what we have shown is that for a system of points in which the
points are subjected to holonomic constraints, the principle of virtual
work says that the work done by the forces that maintain the
holonomic constraints is zero
• We can represent a rigid body as an infinite set of particles with an
infinite set of holonomic constraints
– The principle of virtual work also holds in the case of rigid motion

19

D’Alembert’s principle

• The previous discussion assumed that the system of particles was

in equilibrium
• To make this more general, we assume that the system is not in
equilibrium
– Thus adding an additional force will put the more general
system in equilibrium (D’Alembert’s Principle)
– This additional force is: − p i
• Where pi is the momentum of the ith particle
– And we can include this as follows:
k
∑ ( Fi − p i ) δ ri = 0
T

i =1
– We can again use the principle of virtual work to remove the
constraint forces from Fi and this becomes:
k k
∑ fi T
δ ri − ∑ p iδ ri = 0
20
i =1 i =1
 D’Alembert’s principle

• Note that the virtual displacements are not independent

– But the generalized coordinates are independent
• Thus we want to express this relationship in terms of the generalized
coordinates instead of the virtual displacements
– First, remember that:
n
∂r
δ ri = ∑ i δ q j , i = 1,..., k
j =1 ∂q j
– Substituting this in gives (for the first term):
T ∂ri
k k n n
∑ i i ∑∑ i ∂q j ∑ψ jδ q j
f T
δ r = f δ q =
i =1 i =1 j =1 j j =1

– Where the jth generalized force ψj is:

k
∂r
ψ j = ∑ f iT i
i =1 ∂q j
21

D’Alembert’s principle

• The momentum of particle i is: pi = mi ri

• Therefore we can write the second term as:
k k k n
∂ri
∑ p iT δ ri = ∑ mi riT δ ri = ∑∑ mi riT ∂q j
δqj
i =1 i =1 i =1 j =1
• Note that:
d ⎡ T ∂ri ⎤ ∂r d ⎡ ∂r ⎤
⎢ mi ri ⎥ = mi 
riT i + mi riT ⎢ i ⎥
dt ⎢⎣ ∂q j ⎥⎦ ∂q j dt ⎢⎣ ∂q j ⎥⎦
• Rearranging gives:
∂r d ⎡ ∂r ⎤ d ⎡ ∂r ⎤
riT i = ⎢ mi riT i ⎥ − mi riT ⎢ i ⎥
mi 
∂q j dt ⎣⎢ ∂q j ⎦⎥ dt ⎣⎢ ∂q j ⎦⎥
• Remember that the holonomic constraints allows us to write the
coordinates of the particles as:
ri = ri ( q1 ,..., qn ) , i = 1,..., k
22
 D’Alembert’s principle
n
∂ri
• Differentiating this gives: vi ≡ ri = ∑ q j
j =1 ∂q j

• Note that this implies: ∂vi ∂r

= i
∂q j ∂q j

• Now the last term that we need to identify is:

d ⎡ ∂ri ⎤ n ∂ 2 ri ∂ n
∂r ∂vi
⎢ ⎥=∑ qu = ∑ ∂qi qu =
dt ⎢⎣ ∂q j ⎥⎦ u =1 ∂q j ∂qu ∂q j u =1 u ∂q j

• Recall the following

k ⎧ ⎡ ∂ri ⎤ ⎫⎪
k
∂ri ⎪ d ⎡ T ∂ri ⎤ T d
∑ i i ∂q ∑ ⎨ dt ⎢ i i ∂q ⎥ i i dt ⎢ ∂q ⎥ ⎬
m 
rT
= m 
r − m 
r
i =1 j ⎪ ⎣⎢
i =1 ⎩ ⎥
j⎦ ⎢⎣ j ⎦⎥ ⎭⎪

vi
23

D’Alembert’s principle

• Now that we know each term, we can rewrite this as follows:

k ⎧ ⎫
k
∂ri ⎪d ⎡ T ∂vi
⎤ T ∂vi ⎪
∑ i i ∂q ∑ dt i i ∂q
m 
r T
= ⎨ ⎢ m v ⎥ − m v
i i
∂
⎬
i =1 j ⎪ ⎣
i =1 ⎩ ⎢ j ⎥
⎦ q j ⎭⎪
• Now define the kinetic energy of a system of particles:
k
1
K = ∑ mi viT vi
i =1 2
• Take the partial derivative of the kinetic energy with respect to the
generalized coordinates and their derivatives:
∂K k
∂v ∂K k
∂v
= ∑ mi viT i = ∑ mi viT i
∂q j i =1 ∂q j ∂q j i =1 ∂q j
• Therefore we can rewrite once more:
k
∂r d ∂K ∂K
∑ mi riT ∂qi = dt ∂q − ∂q
i =1 j j j
24
 D’Alembert’s principle

• Putting this pack into our first equation for the work from the fictitious
force: n ⎧
k
⎪ d ∂K ∂K ⎫⎪
∑ i i ∑ ⎨ dt ∂q − ∂q ⎬δ q j
p T
δ r =
i =1 ⎪
j =1 ⎩ j j⎭⎪
• Finally, combining this with the external forces:
n ⎧⎪ d ∂K ∂K ⎫⎪
∑ ⎨ dt ∂q −
∂q j
−ψ j ⎬δ q j = 0
j =1 ⎪
⎩ j ⎪⎭

• Since the virtual displacements of the generalized coordinates are

independent, we can say that each coefficient of δqj is zero:
d ∂K ∂K
− = ψ j , j = 1,..., n
dt ∂q j ∂q j

25

Euler-Lagrange Equations

• If we assume that the generalized force ψj is the sum of an external

force and a force due to potential energy, we can simplify further
– Assume that there exists a potential energy function P(q) and
external forces τj:
∂P
ψj =− +τ j
∂q j
– Then remembering the definition of the Lagrangian: L = K – P:
d ∂L ∂L
− =τ j
dt ∂q j ∂q j

– This is the Euler-Lagrange equation and it allows us to derive the

equations of motion for an nDOF system subject to holonomic
constraints

26
 Euler-Lagrange Equations

• We can derive the equations of motion for any nDOF system by

using energy methods
– All we need to know are the conservative (kinetic and potential)
and non-conservative (dissipative) terms
• Ideally, the terms in the Euler-Lagrange equation are functions of
the generalized coordinates
• This is a shortcut to describing the motion of each particle in a rigid
body along with the constraints that form rigid motions
• For this, we need to first use virtual displacements subject to
holonomic constraints, then use the principle of virtual work, then
finally use D’Alembert’s Principle to derive the Euler-Lagrange
equations of motion

27

Kinetic Energy

• For an arbitrary rigid body, the kinetic energy is:

1 1
K = mvT v + ω T Ιω
2 2
– v is the linear velocity of the center of mass
– ω is the angular velocity
– I is the inertia tensor expressed in the inertial (base) frame

28
 Inertia

• First, we need to express the inertia in the body-attached frame

– Note that the rotation between the inertial frame and the body
attached frame is just R
– Note also the relationship between the angular velocity ω (in the
inertial frame) and the rotational transformation between the body
attached frame and the inertial frame:
S (ω ) = RR
 T
– So the inertia in the inertial frame is related to the inertia in the
body frame by a similarity transform:
Ι = RIRT
• I is the inertia in the body attached frame
– The inertia in the inertial frame is dependant on configuration
– The inertia in the body attached frame is independent of
configuration
29

Inertia

• Inertia is an intrinsic property of a rigid body

– In the body frame, it is a constant 3x3 matrix:
⎡ I xx I xy I xz ⎤
I = ⎡⎣ I ij ⎤⎦ = ⎢ I yx I yy I yz ⎥
⎢ ⎥
⎢⎣ I zx I zy I zz ⎥⎦

– The diagonal elements are called the principal moments of

inertia and are a representation of the mass distribution of a
body with respect to an axis of rotation:

I ii = ∫ r 2 dm = ∫∫∫ r 2 ρ ( x, y, z ) dV = ∫∫∫ r 2 ρ ( x, y, z ) dxdydz

V V V

• r is the distance from the axis of rotation to the particle

30
 Inertia

• The elements are defined by:

( )
I xx = ∫∫∫ y 2 + z 2 ρ ( x, y, z ) dxdydz
principal
I yy = ∫∫∫ ( x 2
+z 2
)ρ ( x, y, z ) dxdydz moments
of inertia
I zz = ∫∫∫ ( x 2
+ y2 )ρ ( x, y, z ) dxdydz
I xy = I yx = − ∫∫∫ xyρ ( x, y, z ) dxdydz
cross
I xz = I zx = − ∫∫∫ xzρ ( x, y, z ) dxdydz products
I yz = I zy = − ∫∫∫ yzρ ( x, y, z ) dxdydz of inertia

ρ(x,y,z) is the density

31

Ex: uniform rectangular block

• Consider a uniform solid block

with the reference frame in the
geometric center

( y + z ) ρ dxdydz
c /2 b /2 a /2
I xx = ∫
− c /2 ∫− b /2 ∫− a /2
2 2

= ∫ ∫ ( y 2 + z 2 ) x − a /2 ρ dydz
c /2 b /2 a /2
− c /2 − b /2

= ρ a ∫ ∫ ( y 2 + z 2 ) dydz
c /2 b /2 c /2 b /2 a /2
− c /2 − b /2 I xy = − ∫ ∫ ∫ xy ρ dxdydz
− c /2 − b /2 − a /2
b /2
⎛ y3
c /2 ⎞ yx 2
a /2
= ρa∫ ⎜ + z2 y ⎟
c /2 b /2
− c /2 3
dz = −∫ ∫ ρ dydz = 0
⎝ ⎠ −b /2 − c /2 − b /2 2
− a /2
c /2 ⎛ b ⎞
2
= ρ ab ∫ ⎜ + z 2 ⎟ dz
− c /2 12
⎝ ⎠
I yy =
m 2 2
12
(
a +c )
=ρ
abc 2 2
12
(
b +c =
m 2 2
12
b +c ) ( ) I zz =
m 2
12
(
a + b2 )
32
 Ex: uniform cylinder

• Consider a uniform (amorphous) solid cylinder with the reference

frame in the geometric center
2π
( x, y, z ) rdrdθ dz
h /2 r1 2
I zz = ∫ ∫ ∫ r ρ
− h /2 0 0
4 r1
h /2 2π r h /2 2π r14
=∫ ∫ ρ dθ dz =ρ ∫ ∫ dθ dz
− h /2 0 4 − h /2 0 4
0
2π
ρπ r14 h /2
r14 1
( )
h /2
= ρ∫ θ dz = z − h /2 = ρπ r12 h r12
− h /2 4 2 2
0
1
= mr12
2

33

Kinetic energy of an n-link manipulator

• We said that the kinetic energy of a rigid body is the sum of two
terms:
1 1
K = mvT v + ω T Ιω
2 2
• Noting the relation between the inertia tensor (inertial frame) and
the inertia matrix (body-attached frame), this becomes:
1 1
K = mvT v + ω T RIRT ω
2 2
• Similar to a set of particles, the total kinetic energy for an n-link
manipulator is the sum of the kinetic energy of each link:
n
⎧1 1 ⎫
K = ∑ ⎨ mi viT vi + ωiT Ri I i RiT ωi ⎬
i =1 ⎩ 2 2 ⎭

34
 Kinetic energy of an n-link manipulator

• Note that the rotational matrices are functions of the generalized

coordinates. i.e. functions of the joint variables:
n
⎧1 1 ⎫
K = ∑ ⎨ mi viT vi + ωiT Ri ( q ) I i Ri ( q ) ωi ⎬
T

i =1 ⎩ 2 2 ⎭
• Lastly, we need a relation between the joint velocities and the
linear and angular velocity of the CM of each link
– This is simply the Jacobian:
vi = J vi ( q ) q ωi = Jωi ( q ) q
• Thus we can rewrite the kinetic energy as a function of the joint
variables:
n
⎧1 1 ⎫
K = ∑ ⎨ mi q T J vi ( q ) J vi ( q ) q + q T Jωi ( q ) Ri ( q ) I i Ri ( q ) Jωi ( q ) q ⎬
T T T

i =1 ⎩ 2 2 ⎭

35

Kinetic energy of an n-link manipulator

• Simplifying this gives:

K=
1 T⎡n
2 ⎣ i =1
{
q ⎢ ∑ mi J vi ( q ) J vi ( q ) + Jωi ( q ) Ri ( q ) I i Ri ( q ) Jωi ( q )
T T T
}⎦⎤⎥ q = 12 q D ( q ) q
T

Inertia matrix

• Properties of the inertia matrix:

– nxn
– Symmetric
– Positive-definite

36
 Potential energy of an n-link manipulator

• For a rigid manipulator, only potential energy is from gravity

• The potential energy of the ith link is found by assuming all mass is
concentrated at the center of mass
– let rci be the position of the center of mass of the ith link (in the
inertial frame)
– let g be the gravity vector in the inertial frame
n n
P = ∑ Pi = ∑ mi g T rci
i =1 i =1

• Potential energy is independent of the joint velocities

37

Equations of motion

• Now we are finally ready to use the Euler-Lagrange formulation

d ∂L ∂L
− =τ j
dt ∂q j ∂q j

• We can write the Lagrangian as:

1
L = K − P = q T D ( q ) q − P ( q )
2
1 n n
= ∑∑ dij ( q )qi q j − P ( q )
2 j =1 i =1
• Take the partial derivatives of the Lagrangian:
∂L ∂ ⎛1 n n ⎞ ∂
= ⎜ ∑∑ dij ( q )qi q j ⎟ − P(q)
∂qk ∂qk ⎜ 2 j =1 i =1 ⎟ ∂qk
⎝ ⎠
n
= ∑ d kj ( q )q j
j =1 38
 Equations of motion

• Now take the partial derivatives of the Lagrangian:

d ∂L n n
d
= ∑ d kj q j + ∑ d kj q j
dt ∂qk j =1 j =1 dt
n n n ∂d
= ∑ d kj q j + ∑∑
kj
qi q j
j =1 j =1 i =1 ∂qi

• Partial with respect to the positions:

∂L 1 n n ∂dij ∂P
= ∑∑ qi q j −
∂qk 2 j =1 i =1 ∂qk ∂qk

• Combining these back into the Euler-Lagrange equation:

n n ⎧ ∂d
n
1 ∂dij ⎫ ∂P
∑ kj j ∑∑ ⎨ ∂q 2 ∂q ⎬qi q j + ∂q = τ k
+
kj
d 
q −
j =1 j =1 i =1 ⎩ i k ⎭ k

39

Equations of motion

• Lastly, we do a trick to simplify the second term

n n ⎧ ∂d ⎫
1 n n ⎪⎧ ∂d kj ∂d ki ⎪⎫ Simply because
∑∑ ⎨ ∂q ⎬ i j 2 ∑∑ ⎨ ∂q + ∂q ⎬qi q j
kj
q 
q = of the symmetry
j =1 i =1 ⎩ i ⎭ ⎪ i
j =1 i =1 ⎩ j ⎭⎪ of the inertia matrix

• Therefore the second term can be rewritten as:

n n ⎧ ∂d
1 ∂dij ⎫ 1 n n ⎪⎧ ∂d kj ∂d ki ∂dij ⎪⎫
∑∑ ⎨ ∂q − 2 ∂q ⎬qi q j = 2 ∑∑ ⎨ ∂q + ∂q − ∂q ⎬qi q j
kj

j =1 i =1 ⎩ i k ⎭ j =1 i =1 ⎪
⎩ i j k ⎪ ⎭
• For simplicity, we make the following definition:
1 ⎛ ∂d kj ∂d ki ∂dij ⎞
cijk ≡ ⎜ + − ⎟⎟
2 ⎝⎜ ∂qi ∂q j ∂qk ⎠
– cijk are the Christoffel symbols
∂P
• Define the gravity force on the kth link as: g k =
∂qk
40
 Equations of motion

• The Euler-Lagrange equations for an n-link manipulator:

n n n
∑ dkj ( q ) qj +∑∑ cijk ( q )qi q j + g k ( q ) = τ k
j =1 j =1 i =1
if i = j , then qi 2 (centrifugal)
• Commonly, this is written as:
if i ≠ j , then qi q j (coriolis)
D ( q ) q + C ( q, q ) q + g ( q ) = τ
– D(q) is the inertia matrix n
– The C matrix is given as: kj ∑ cijk ( q ) qi
c =
i =1

– And the gravity vector is: g ( q ) = ⎡⎣ q1 ( q ) q2 ( q ) ⋅⋅⋅ qn ( q ) ⎤⎦

T

• This holds for any n-link manipulator that has kinetic energy defined
by two terms and potential energy that is independent of velocity

41

Summary

1. If we know the configuration, we can calculate the Jacobian

• If we know the Jacobian, we know the velocities of each CM
2. Once we know the mass and inertia properties of each link and
the linear and angular velocities of each link, we can formulate the
kinetic energy (for the whole system)
3. If we know the forward kinematics and the mass of each link, we
can find the position of the CM
• We can calculate the potential energy
4. Once we have K and P, we can easily form L
5. To obtain the equations of motion we can:
• Take the partial derivatives with respect to the joint positions
and velocities (and time derivatives) and plug into the Euler-
Lagrange formulation
• Or determine D, C, and g
42
 Example 1: two-link cartesian manipulator

• m1 and m2 are the masses of the two links

• Generalized coordinates are the prismatic displacements
• By inspection:
⎡0 0 ⎤  ⎡0 0 ⎤ 
⎡ q ⎤ ⎡q ⎤
v1 = J v1 q = ⎢ 0 0 ⎥ ⎢ ⎥ v2 = J v2 q = ⎢ 0 1 ⎥ ⎢ 1 ⎥
1
⎢1 0 ⎥ ⎣ q2 ⎦ ⎢1 0 ⎥ ⎣ q2 ⎦
⎣ ⎦ ⎣ ⎦
• Thus, the kinetic energy is:
1 T⎧n ⎫
K = q ⎨∑ mi J vi T J vi ⎬ q
2 ⎩ i =1 ⎭
1
{ }
= q T m1 J v1T J v1 + m2 J v2 T J v2 q
2
• And the inertia matrix is:
⎡1 0 ⎤ ⎡1 0 ⎤ ⎡ m1 + m2 0⎤
D ( q ) = m1 ⎢ + m2 ⎢ =
⎥
⎣0 0 ⎦ ⎣0 1 ⎥⎦ ⎢⎣ 0 m2 ⎥⎦ 43

Example 1: two-link cartesian manipulator

• The potential energy of the system is:

n
P ( q ) = ∑ Pi = g ( m1 + m2 ) q1
i =1
• Note that the inertia matrix is not a function of q
– Therefore the partial derivatives are zero
– Thus the Christoffel symbols are zero
• We can write the equations of motions as:
Dq + g ( q ) = f
– Where the gravity vector is the partial of P:
∂P ∂P
g1 = = g ( m1 + m2 ) , g 2 = =0
∂q1 ∂q2
• The equations of motion will be:
( m1 + m2 ) q1 + g ( m1 + m2 ) = f1
( m2 ) q2 = f 2 44
 Example 2: PR planar manipulator

• PR planar manipulator with

massless link:

– The kinetic energy:

1
K1 = m1q12
2
1 1
K 2 = m2 x22 + m2 y 22
2 2
2 2
1 ⎛d ⎞ 1 ⎛d ⎞
= m2 ⎜ ( q1 + l cos q2 ) ⎟ + m2 ⎜ (l sin q2 ) ⎟
2 ⎝ dt ⎠ 2 ⎝ dt ⎠
1
(
= m2 q12 + l 2 q22 − 2lq1q2 sin q2
2
)
– The potential energy:
P = m2 gl sin q2
45

Example 2: PR planar manipulator

• PR planar manipulator with

massless link:

1
K1 = m1q12
2
1
(
K 2 = m2 q12 + l 2 q22 − 2lq1q2 sin q2
2
)
K = K1 + K 2

P = m2 gl sin q2

• The equations of motion will be:

(m1 + m2 )q1 − m2lq2 sin q2 − m2lq22 cos q2 = τ1
m2l 2 q1 − m2lq1 sin q2 + m2 gl cos q2 = τ 2
46
 Example 3: planar polar manipulator

• A planar polar manipulator with

massless link and a massive point:

– The kinetic energy:

– The potential energy:

– The equations of motion will be:

47

Example 3: planar polar manipulator

• A planar polar manipulator with

massless link and a massive point:

– The kinetic energy:

1 1
K = m2 x22 + m2 y 22
2 2
2 2
1 ⎛d ⎞ 1 ⎛d
⎠ 2 ⎝ dt
⎞
⎠
1
(
= m2 ⎜ ( q1 cos q2 ) ⎟ + m2 ⎜ ( q1 sin q2 ) ⎟ = m2 q12 + q12 q22
2 ⎝ dt 2
)
– The potential energy:
V = m2 gq1 sin q2

– The equations of motion will be:

mq1 − mq1q22 + mg sin q2 = τ1
mq12 q2 + 2mq1q1q2 + mgq1 cos q2 = τ 2
48
 Example 4: one-link manipulator
y0
• A one-link manipulator with
massive link and a massive point:
⎡ 0.5l cos θ ⎤ ⎡l cosθ ⎤
r1 = ⎢ 0.5l sin θ ⎥ , r2 = ⎢ l sin θ ⎥ τ1
⎢
⎣ 0 ⎥
⎦
⎢ 0 ⎥
⎣ ⎦
•
x0

⎡ −0.5l sin θ ⎤ ⎡ −l sin θ ⎤

v1 = J v1θ = r1 = ⎢ 0.5l cos θ ⎥ θ, v2 = J v2θ = r2 = ⎢ l cos θ ⎥ θ
⎢ ⎥ ⎢ 0 ⎥
⎣ 0 ⎦ ⎣ ⎦
– The kinetic energy:

– The potential energy:

49

Example 4: one-link manipulator

y0
• A one-link manipulator with
massive link and a massive point:
⎡ 0.5l cos θ ⎤ ⎡l cosθ ⎤
r11 = ⎢ 0.5l sin θ ⎥ , r12 = ⎢ l sin θ ⎥ τ1
⎢
⎣ 0 ⎥
⎦
⎢ 0 ⎥
⎣ ⎦
•
x0

⎡ −0.5l sin θ ⎤ ⎡ −l sin θ ⎤

v11 = J v11θ = r11 = ⎢ 0.5l cos θ ⎥ θ, v12 = J v12θ = r12 = ⎢ l cos θ ⎥ θ
⎢ ⎥ ⎢ 0 ⎥
⎣ 0 ⎦ ⎣ ⎦
– The kinetic energy:
1 1 1
( )
T
K = m1v11 2
+ m2 v12
2
+ ω T R10 I R10 ω
2 2 2
– The potential energy:
P = m1 gy1 + mgy2 = 0.5m1 gl sin θ + m2 gl sin θ
50
 Example 4: one-link manipulator
y0
• A one-link manipulator with
massive link and a massive point:

– The kinetic energy: τ1 •

x0
1 1 1
( )
T
K = m1v12 + m2v22 + ω T R10 I R10 ω
2 2 2
– The potential energy:
P = m1 gy1 + mgy2 = 0.5m1 gl sin θ + m2 gl sin θ

– The equations of motion will be:

⎛1 2 ⎞  ⎛ 1 ⎞
⎜ m1l + m2l + I zz ⎟θ + ⎜ m1 + m2 ⎟ gl cos θ = τ1
2
⎝4 ⎠ ⎝2 ⎠

51

Example 5: two-link elbow manipulator

• A two-link manipulator with

massive link and massless point :

⎡ − Lc1 sin q1 0 ⎤
⎡ q ⎤
v1 = J v1 q = Lc1 cos q1 0 ⎥ ⎢ 1 ⎥
⎢
⎢ q
⎣ 0 0 ⎥⎦ ⎣ 2 ⎦

v2 = J v2 q
⎡ − L1 sin q1 − Lc 2 sin ( q1 + q2 ) − Lc 2 sin ( q1 + q2 ) ⎤
⎡ q ⎤
= ⎢ L1 cos q1 + Lc 2 cos ( q1 + q2 ) Lc 2 cos ( q1 + q2 ) ⎥ ⎢ 1 ⎥
⎢ ⎥ ⎣ q2 ⎦
⎢⎣ 0 0 ⎥⎦
52
 Example 5: two-link elbow manipulator

• Now the angular velocity of each link:

ωiT Ιiωi = q T Jωi ( q ) Ri ( q ) I i Ri ( q ) Jωi ( q ) q
T T

⎡ cos q1 − sin q1 0 ⎤ ⎡ cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 ⎤

R1 ( q ) = ⎢ sin q1 cos q1 0 ⎥ R2 ( q ) = ⎢ sin ( q1 + q2 ) cos ( q1 + q2 ) 0 ⎥
⎢ 0 ⎢ ⎥
⎣ 0 1 ⎥⎦ ⎣⎢ 0 0 1 ⎥⎦
⎡0 0 0 ⎤ ⎡0 0 0 ⎤
⎢
I1 = 0 I yy ,1 0 ⎥ I 2 = ⎢ 0 I yy ,2 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣⎢0 0 I zz ,1 ⎦⎥ ⎣⎢ 0 0 I ⎥
zz ,2 ⎦

⎡0 0 ⎤  ⎡0 0 ⎤ 
⎡q ⎤ ⎡q ⎤
ω1 = Jω1 q = ⎢0 0 ⎥ ⎢ 1 ⎥ ω2 = Jω2 q = ⎢ 0 0 ⎥ ⎢ 1 ⎥
⎢1 0 ⎥ ⎣ q2 ⎦ ⎢1 1 ⎥ ⎣ q2 ⎦
⎣ ⎦ ⎣ ⎦
53

Example 5: two-link elbow manipulator

• Thus the rotational component of the kinetic energy is:

1 ⎧2 ⎫
K rot = q T ⎨∑ J ωi ( q ) Ri ( q ) I i Ri ( q ) Jωi ( q ) ⎬ q
T T
2 ⎩ i =1 ⎭
⎧ ⎡ cos q1 − sin q1 0 ⎤ ⎡0 0 0 ⎤ ⎡ cos q1 sin q1 0 ⎤ ⎡0 0 ⎤ ⎫
1 T ⎪ ⎡0 0 1 ⎤ ⎢ ⎪
= q ⎨ ⎢ ⎥ sin q1 cos q1 0 ⎥ ⎢ 0 I yy ,1 0 ⎥ ⎢ − sin q1 cos q1 0 ⎥ ⎢0 0 ⎥ ⎬ q
2 ⎪ ⎣0 0 0⎦ ⎢ 0 ⎢ ⎥⎢
⎣ 0 1 ⎥⎦ ⎢0 0 I ⎥ ⎣ 0 0 1 ⎥⎦ ⎢⎣1 0 ⎥⎦ ⎪
⎩ ⎣ zz ,1 ⎦ ⎭
⎧ ⎡cos ( q1 + q2 ) − sin ( q1 + q2 ) 0 ⎤ ⎡0 0 0 ⎤ ⎫
⎪ ⎡0 0 1⎤ ⎢ ⎢ ⎪
⎥
⎪ ⎢⎣0 0 1⎥⎦ ⎢ sin ( q1 + q2 ) cos ( q1 + q2 ) 0 ⎥ ⎢0 I yy ,2 0 ⎥ x⎪
⎥
1 T⎪ ⎢
⎣ 0 0 1 ⎦⎥ ⎣⎢0 0 I zz ,2 ⎦⎥ ⎪
+ q ⎨ ⎬ q
2 ⎪ ⎡ cos ( q1 + q2 ) sin ( q1 + q2 ) 0 ⎤ ⎡0 0 ⎤ ⎪
⎪ x ⎢ − sin ( q1 + q2 ) cos ( q1 + q2 ) 0 ⎥ ⎢0 0 ⎥ ⎪
⎪ ⎢ ⎥⎢ ⎥
1 ⎥⎦ ⎣1 1 ⎦ ⎪
⎩ ⎢⎣
0 0
⎭
1 ⎧⎡ I 0 ⎤ ⎡ I zz ,2 I zz ,2 ⎤ ⎫
= q T ⎨ ⎢ zz ,1 + q
2 ⎩ ⎣ 0 0 ⎥⎦ ⎣⎢ I zz ,2 I zz ,2 ⎦⎥ ⎬⎭
54
 Example 5: two-link elbow manipulator

• Thus, the total kinetic energy of the system is:

1 T⎧n ⎫
K = q ⎨∑ mi J vi T J vi + J ωi T I i Jωi ⎬ q
2 ⎩ i =1 ⎭
1 ⎧ ⎡1 0 ⎤ ⎡1 1⎤ ⎫ 
= q T ⎨ m1 J v1T J v1 + m2 J v2 T J v2 + I zz ,1 ⎢ + I ⎢⎣1 1⎥⎦ ⎬⎭ q
⎣ 0 0 ⎥⎦
zz , 2
2 ⎩
• And the inertia matrix can be written as:
⎡d d12 ⎤
D ( q ) = ⎢ 11
⎣ d 21 d 22 ⎥⎦
– Where:
⎛ L12 + Lc 2 2 + L1Lc 2 sin ( q1 ) sin(q1 + q2 ) + ⎞
d11 = m1Lc1 + m2 ⎜
2
⎟ + I zz ,1 + I zz ,2
⎝ + L1 Lc 2 cos ( q1 ) cos( q1 + q2 ) ⎠
( )
d12 = m2 Lc 2 + L1Lc 2 sin ( q1 ) sin( q1 + q2 ) + L1Lc 2 cos ( q1 ) cos( q1 + q2 ) + I zz ,2
2

d 22 = m2 Lc 2 2 + I zz ,2
55

Example 5: two-link elbow manipulator

• Since cosαcosβ + sinαsinβ = cos(α - β), we can rewrite the elements

of the inertia matrix:
( )
d11 = m1Lc12 + m2 L12 + Lc 2 2 + 2 L1Lc 2 cos ( q2 ) + I zz ,1 + I zz ,2

( )
d12 = m2 Lc 2 2 + L1Lc 2 cos ( q2 ) + I zz ,2
d 22 = m2 Lc 2 2 + I zz ,2
• We next determine the Christoffel symbols:
1 ⎧ ∂d11 ∂d11 ∂d11 ⎫ 1 ∂d11
c111 = ⎨ + − ⎬= =0
2 ⎩ ∂q1 ∂q1 ∂q1 ⎭ 2 ∂q1
1 ⎧ ∂d ∂d ∂d ⎫ 1 ∂d11
c121 = ⎨ 12 + 11 − 12 ⎬ = = − m2 L1Lc 2 sin q2 for k = 1
2 ⎩ ∂q1 ∂q2 ∂q1 ⎭ 2 ∂q2
1 ⎧ ∂d ∂d ∂d ⎫
c221 = ⎨ 12 + 12 − 22 ⎬ = − m2 L1Lc 2 sin q2
2 ⎩ ∂q2 ∂q2 ∂q1 ⎭
56
 Example 5: two-link elbow manipulator

• We next determine the Christoffel symbols:

1 ⎧ ∂d 21 ∂d 21 ∂d11 ⎫ 1 ∂d11
c112 = ⎨ + − ⎬= = m2 L1Lc 2 sin q2
2 ⎩ ∂q1 ∂q1 ∂q2 ⎭ 2 ∂q1
1 ⎧ ∂d ∂d ∂d ⎫ 1 ∂d 22
c122 = ⎨ 22 + 21 − 12 ⎬ = =0 for k = 2
2 ⎩ ∂q1 ∂q2 ∂q2 ⎭ 2 ∂q1
1 ⎧ ∂d ∂d ∂d ⎫ 1 ∂d 22
c222 = ⎨ 22 + 22 − 22 ⎬ = =0
2 ⎩ ∂q2 ∂q2 ∂q2 ⎭ 2 ∂q2

• Thus we can write the C matrix:

⎡c c ⎤ ⎡c c ⎤
C ( q, q ) = ⎢ 111 121 ⎥ + ⎢ 112 122 ⎥
⎣c121 c221 ⎦ ⎣c122 c222 ⎦
⎡( − m2 L1Lc 2 sin q2 ) q2 − m2 L1Lc 2 sin q2 ( q1 + q 2 ) ⎤
=⎢ ⎥
⎣ ( m2 L1Lc 2 sin q2 ) q1 0 ⎦
57

Example 5: two-link elbow manipulator

• We next determine the potential energy:

P1 = m1 gLc1 sin q1
P2 = m2 g ( L1 sin q1 + Lc 2 sin ( q1 + q2 ) )
• And the gravity vector:
∂P
g1 = = ( m1Lc1 + m2 L1 ) g cos q1 + m2 Lc 2 g cos ( q1 + q2 )
∂q1
∂P
g2 = = m2 Lc 2 g cos ( q1 + q2 )
∂q2

• Combining each of these terms gives us the equations of motion:

d11q1 + d12 q2 + c121q1q2 + c211q1q2 + c221q2 2 + g1 = τ1
d 21q1 + d 22 q2 + c112 q12 + g 2 = τ 2

58
 Example 6: General RR planar manipulator

• A two-link manipulator with massive link and a massive point:

τ1 τ2

59

Example 6: General RR planar manipulator

⎡c1 cosθ1 ⎤ ⎡l1 cosθ1 ⎤

r11 = c1 sin θ1 , r12 = ⎢ l1 sin θ1 ⎥
⎢ ⎥
⎢ 0 ⎥ ⎢ 0 ⎥
⎣ ⎦ ⎣ ⎦
⎡l1 cosθ1 + c2 cos(θ1 + θ 2 ) ⎤
r21 = ⎢ l1 sin θ1 + c2 sin(θ1 + θ 2 ) ⎥ ,
⎢ ⎥
⎣ 0 ⎦
⎡l1 cosθ1 + l2 cos(θ1 + θ 2 ) ⎤ τ2
r22 = ⎢ l1 sin θ1 + l2 sin(θ1 + θ 2 ) ⎥ τ1
⎢ ⎥
⎣ 0 ⎦

60
 Example 7: planar five-bar

• Fivebar linkage has 2DOF

– Remember Gruebler’s formular
• This is a simplified configuration
– Opposite links are always parallel
• Since this is a closed chain, we cannot use the standard Jacobians

⎡ xc1 ⎤ ⎡ Lc1 cos q1 ⎤

⎢⎣ yc1 ⎥⎦ = ⎢⎣ Lc1 sin q1 ⎥⎦
⎡ xc 2 ⎤ ⎡ Lc 2 cos q2 ⎤
⎢⎣ yc 2 ⎥⎦ = ⎢⎣ Lc 2 sin q2 ⎥⎦
⎡ xc 3 ⎤ ⎡ L2 cos q2 ⎤ ⎡ Lc 3 cos q1 ⎤
⎢⎣ yc 3 ⎥⎦ = ⎢⎣ L2 sin q2 ⎥⎦ + ⎢⎣ Lc 3 sin q1 ⎥⎦
⎡ xc 4 ⎤ ⎡ Lc1 cos q1 ⎤ ⎡ Lc 4 cos q2 ⎤
⎢⎣ yc 4 ⎥⎦ = ⎢⎣ Lc1 sin q1 ⎥⎦ − ⎢⎣ Lc 4 sin q2 ⎥⎦

61

Example 7: planar five-bar

• Next we calculate the linear velocities:

⎡ − Lc1 sin q1 0⎤ ⎡ 0 − Lc 2 sin q2 ⎤

vc1 = ⎢ Lc1 cos q1 0 ⎥ q , vc 2 = ⎢ 0 Lc 2 cos q2 ⎥ q
⎢ ⎥ ⎢0 ⎥
⎣ 0 0⎦ ⎣ 0 ⎦
⎡ − Lc 3 sin q1 − L2 sin q2 ⎤ −
⎡ 1 L sin q1 − Lc 4 sin q2 ⎤
vc 3 = ⎢ Lc 3 cos q1 L2 cos q2 ⎥ q , vc 4 = ⎢ L1 cos q1 Lc 4 cos q2 ⎥ q
⎢ ⎥ ⎢ ⎥
⎣ 0 0 ⎦ ⎣ 0 0 ⎦
• And the angular velocities:
– By inspection, it is clear that the angular velocities are simply:
ω1 = ω3 = q1kˆ
ω = ω = q kˆ
2 4 2

62
 Example 7: planar five-bar

• Therefore we can write the inertia matrix as:

⎡I + I 0 ⎤
4
D ( q ) = ∑ mi J vi ( q ) J vi ( q ) + ⎢ 1 3
T

i =1 ⎣ 0 I 2 + I 4 ⎥⎦
⎡ m1Lc12 + m3 Lc 32 + m4 L12 + I1 + I 3 ( m3 L2 Lc 3 − m4 L1Lc 4 ) cos ( q2 − q1 ) ⎤
=⎢ ⎥
⎢⎣( m3 L2 Lc 3 − m4 L1Lc 4 ) cos ( q2 − q1 ) m2 Lc 2 + m3 L2 + m4 Lc 4 + I 2 + I 4 ⎥⎦
2 2 2

• Lets make the assumption that: m3 L2 Lc 3 = m4 L1Lc 4

– Therefore the inertia matrix is diagonal and constant
– Therefore the partial derivatives are zero
– Therefore the Christoffel symbols are zero

63

Example 7: planar five-bar

• The potential energy is given by:

4 4
P = ∑ Pi =g ∑ mi yi
i =1 i =1
= gm1Lc1 sin q1 + gm2 Lc 2 sin q2
+ gm3 ( L2 sin q2 + Lc 3 sin q1 ) + gm4 ( L1 sin q1 − Lc 4 sin q2 )

• Taking the partial derivatives gives the gravity vector:

g1 = g cos q1 ( m1Lc1 + m3 Lc 3 + m4 L1 )
g 2 = g cos q2 ( m2 Lc 2 + m3 L2 − m4 Lc 4 )

• Finally, the equations of motion are given by:

d11q1 + g1 ( q1 ) = τ1
d 22 q2 + g 2 ( q2 ) = τ 2

64