1. Dynamics of Articulated Robots

2. 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

3. 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

4. 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  ’

5. Summary f = m v’  = [  ] H  H  ’

6. 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’?

7. 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)

8. Lagrangian Mechanics L(q,q’) = K(q,q’) – P(q) Lagrangian equations of motion: d/dt  L/  q’ -  L/  q = u Kinetic energyPotential energy

9. 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

10. 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

11. 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’

12. 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’

13. 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

14. 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’

15. 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’

16. Final canonical form B(q) q’’ + C(q,q’) + G(q) = u Mass matrixCentrifugal/ coriolis forces Generalized gravity Generalized forces

17. 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)

18. 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

19. Software Both Lagrangian dynamics and Newton- Euler methods are implemented in KrisLibrary

20. 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

21. 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

22. 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