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 H L

Kinetic energy for rigid body Rigid body with velocity v, angular velocity   KE = ½ (m v T v +  T H  ) World-space inertia tensor H = R H L R T vv T vv H 0 0 m I 1/2

Kinetic energy derivatives  KE/  v = m v Force 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’  R n Generalized forces u  R m  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 energyPotential 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 energyPotential energy

Kinetic energy for articulated robot K(q,q’) =  i K i (q,q’) Velocity of i’th rigid body  v i = J i t (q) q’ Angular velocity of i’th rigid body   i = J i r (q) q’ K i = ½ q’ T (m i J i t (q) T J i t (q) + J i r (q) T H i (q)J i r (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  /  q 1 B(q) q’ … q’ T  /  q n B(q) q’

Potential energy for articulated robot in gravity field  P/  q =  i  P i /  q  P i /  q = m i (0,0,g) T v i = m i (0,0,g) T J i t (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) = u q’ T  /  q 1 B(q) q’ … q’ T  /  q n 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) = u q’ T  /  q 1 B(q) q’ … q’ T  /  q n B(q) q’

Final canonical form B(q) q’’ + C(q,q’) + G(q) = u Mass matrixCentrifugal/ 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(n 3 )) 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