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 (t) Mass m, local inertia tensor HL

Kinetic energy for rigid body

Rigid body with velocity v, angular velocity KE = ½ (m vTv + T H )

World-space inertia tensor H = R HL RT

v

T

v

H 0 0 m I

1/2

Kinetic energy derivatives

KE/v = m v Force (@CM) f = d/dt (KE/v) = m v’

KE/ = H d/dt H = []H – H[] Torque = d/dt (KE/) = [] H H ’

Summary

f = m v’ = [] H H ’

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)

Lagrangian equations of motion:d/dt L/q’ - L/q = u

Kinetic energy Potential energy

Lagrangian Approach

L(q,q’) = K(q,q’) – P(q)

Lagrangian equations of motion:d/dt L/q’ - L/q = u

L/q’ = K/q’ L/q = K/q - P/q

Kinetic energy Potential energy

Kinetic energy for articulated robot K(q,q’) = i Ki(q,q’) Velocity of i’th rigid body

vi = Jit(q) q’

Angular velocity of i’th rigid bodyi = Ji

r(q) q’ Ki = ½ q’T(miJi

t(q)TJit(q) + Ji

r(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 = i 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’ – ½ + G(q) = uq’T /q1 B(q) q’

…q’T /qn B(q) q’

Putting it all together

d/dt K/q’ - K/q - P/q = u

B(q) q’’ + d/dt B(q) q’ – ½ + G(q) = uq’T /q1 B(q) q’

…q’T /qn B(q) q’

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 uu = 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 Newton-Euler 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