Dynamics of
Articulated Robots
Rigid Body Dynamics


The following can be derived from first principles
using Newton’s laws + rigidity assumption
Parameters
 CM
translation x(t)
 CM velocity v(t)
 Rotation R(t)
 Angular velocity w(t)
 Mass m, local inertia tensor HL
Kinetic energy for rigid body

Rigid body with velocity v, angular velocity
w
 KE

= ½ (m vTv + wT H w)
World-space inertia tensor H = R HL RT
1/2
w
v
T
H 0
0 mI
w
v
Kinetic energy derivatives
KE/v = m v
 Force (@CM) f = d/dt (KE/v) = m v’

KE/w = H w
 d/dt H = [w]H – H[w]
 Torque t = d/dt (KE/w) = [w] H w + H w’

Summary
f = m v’
 t = [w] H w + H w’

Robot Dynamics
Configuration q, velocity q’  Rn
 Generalized forces u  Rm

 Joint
torques/forces
 If m < n we say robot is underactuated

How does u relate to q and q’?
Articulated Robots
Treat each link as a rigid body
 Use Langrangian mechanics to
determine dynamics of q, q’ as a function
of generalized forces u
 (Derivation: principle of virtual work)

Lagrangian Mechanics

L(q,q’) = K(q,q’) – P(q)
Kinetic energy

Potential energy
Lagrangian equations of motion:
d/dt L/q’ - L/q = u
Lagrangian Approach

L(q,q’) = K(q,q’) – P(q)
Kinetic energy
Potential energy

Lagrangian equations of motion:
d/dt L/q’ - L/q = u

L/q’ = K/q’
L/q = K/q - P/q

Kinetic energy for articulated
robot
K(q,q’) = Si Ki(q,q’)
 Velocity of i’th rigid body

 vi

= Jit(q) q’
Angular velocity of i’th rigid body
 wi
= Jir(q) q’
Ki = ½ q’T(miJit(q)TJit(q) + Jir(q)THi(q)Jir(q))q’
 K(q,q’) = ½ q’T B(q) q’

Mass matrix
Derivative of K.E. w.r.t q’
/q’ K(q,q’) = B(q) q’
 d/dt (/q’ K(q,q’)) = B(q) q’’ + d/dt B(q) q’

Derivative of K.E. w.r.t q

/q K(q,q’) = ½
q’T /q1 B(q) q’
…
q’T /qn B(q) q’
Potential energy for articulated
robot in gravity field
P/q = Si Pi/q
 Pi/q = mi (0,0,g)T vi = mi (0,0,g)T Jit(q) q’
 -G(q)

Generalized gravity
Putting it all together


d/dt K/q’ - K/q - P/q = u
B(q) q’’ + d/dt B(q) q’ – ½
q’T /q1 B(q) q’
…
q’T /qn B(q) q’
+ G(q) = u
Putting it all together


d/dt K/q’ - K/q - P/q = u
B(q) q’’ + d/dt B(q) q’ – ½
q’T /q1 B(q) q’
…
q’T /qn B(q) q’
+ G(q) = u
Final canonical form

B(q) q’’ + C(q,q’) + G(q) = u
Mass matrix
Centrifugal/
coriolis forces
Generalized
gravity
Generalized forces
Forward/Inverse Dynamics

Given u, find q’’
 q’’

= M(q)-1 (u - C(q,q’) - G(q) )
Given q,q’,q’’, find u
u
= M(q) q’’ + C(q,q’) + G(q)
Newton-Euler Method
(Featherstone 1984)
Explicitly solves a linear system for joint
constraint forces and accelerations,
related via Newton’s equations
 Faster forward/inverse dynamics for large
chains (O(n) vs O(n3))
 Lagrangian form still mathematically handy

Software

Both Lagrangian dynamics and NewtonEuler methods are implemented in
KrisLibrary
Application: Feedforward control



Joint PID loops do not follow
joint trajectories accurately
Include feedforward torques
to reduce reliance on feedback
Estimate the torques that
would compensate for gravity
and coriolis forces
Application: Feedforward control



Joint PID loops do not follow
joint trajectories accurately
Include feedforward torques
to reduce reliance on feedback
Estimate the torques that
would compensate for gravity
and coriolis forces
Feedforward Torques
Given q,q’,q’’ of trajectory
 1. Estimate M, C, G
 2. Compute u

u

= M(q) q’’ + C(q,q’) + G(q)
3. Add u into joint PID loops