Dynamics of Articulated Robots

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

3. 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 T w v H 0 0 m I w v 1/2

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

5. Summary • f = m v’ • t = [w] H w + H w’

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

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 energy Potential 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 energy Potential energy

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

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’T /q1 B(q) q’ … q’T /qn B(q) q’ • /q K(q,q’) = ½

13. Potential energy for articulated robot in gravity field • P/q = Si Pi/q • Pi/q = mi (0,0,g)Tvi = mi (0,0,g)TJit(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 /q1 B(q) q’ … q’T /qn 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 /q1 B(q) q’ … q’T /qn B(q) q’

16. Final canonical form • B(q) q’’ + C(q,q’) + G(q) = u Mass matrix Centrifugal/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(n3)) • 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