Dynamics of Articulated Robots

1. Dynamics of Articulated Robots Kris Hauser CS B659: Principles of Intelligent Robot Motion Spring 2013

2. Agenda • Basic elements of simulation • Derive the standard form of the dynamics of an articulated robot in joint space • Also works for humans, biological systems, non-actuated mechanical systems … • Featherstone algorithm: fast method for computing forward dynamics (torques to accelerations) and inverse dynamics (accelerations to torques) • Constrained dynamics

3. Rigid Body Dynamics • The following can be derived from first principles using Newton’s laws + rigidity assumption • Parameters • CM translation c(t) • CM velocity v(t) • Rotation R(t) • Angular velocity w(t) • Mass m, local inertia tensor HL

4. Rigid body ordinary differential equations • We will express forces and torques in terms of terms of H (a function of R), , and • Rearrange… • So knowing f(t) and τ(t), we can derive c(t), v(t), R(t), and w(t) by solving an ordinary differential equation (ODE) • dx/dt = f(x) • x(0) = x0 • With x=(c,v,R,w) the state of the system • Numerical integration, also known as simulation

5. Articulated body ODEs • We will express joint torques in terms of terms of and and external forces f • Rearrange… • An ODE in the state space x=() • Solve using numerical integration

6. Numerical integration of ODEs • If dx/dt = f(x) and x(0) are known, then given a step size h, • x(kh)  xk= xk-1 + h f’(xk-1) • gives an approximate trajectory for k 1 • Provided f is smooth • Accuracy depends on h • Known as Euler’s method • Better integration schemes are available • (e.g., Runge-Kutta methods, implicit integration, adaptive step sizes, etc) • Beyond the scope of this course

7. Dynamics of Rigid Bodies

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

9. Kinetic energy derivatives • Force (@CM) • H = [w]H – H[w] • Torque t = = [w] H w + H

10. Summary Gyroscopic “force”

11. Force off of COM F x

12. Force off of COM F x Consider infinitesimal virtual displacement generated by F. (we don’t know what this is, exactly) The virtual work performed by this displacement is FT

13. Generalized torque f Now consider the equivalent force f, torque τ at COM

14. Generalized torque f Now consider the equivalent force f, torque τ at COM And an infinitesimal virtual displacement of R.B. coordinates

15. Generalized torque f Now consider the equivalent force f, torque τ at COM And an infinitesimal virtual displacement of R.B. coordinates Virtual work in configuration space is [fT,τT]

16. Principle of virtual work F f [fT,τT]= FT Since we have [fT,τT]= FT

17. Principle of virtual work F f [fT,τT]= FT Since we have [fT,τT]= FT Since this holds no matter what is, we have [fT,τT]= FTJ(q), Or JT(q) F = f τ

18. Articulated Robot Dynamics

19. Robot Dynamics • Configuration , velocity  Rn • Generalized forces u  Rm • Joint torques and external forces • How does u relate to and ? • Use Langrangian mechanics to find a link between u and

20. Lagrangian Mechanics • The trajectory between two states , is the one that minimizes the “action” • is defined such that the path minimizing S is equivalent to the one produced by Newton’s laws, subject to the constraints that the system only moves along coordinates q Kinetic energy Potential energy

21. Lagrangian Mechanics • Minimum action condition => Euler-Lagrange equations of motion: • Note that P is independent of , so A system of n partial differential equations

22. Example: Point Mass • Coordinates q = (x,y) • Potential field P(x,y) • Lagrangian: • Equations of motion Sanity check: Newton’s laws

23. Kinetic energy for articulated robot • Velocity of i’th rigid body • Angular velocity of i’th rigid body Mass matrix:symmetric positive definite

24. Derivative of K.E. w.r.t

25. Derivative of K.E. w.r.t q

26. Potential energy for articulated robot in gravity field • G(q) Generalized gravity

27. Putting it all together Group these terms together

28. Final canonical form Generalized forces (joint torques + external forces) Generalized inertia Centrifugal/coriolis forces Generalized gravity

29. Forward/Inverse Dynamics • Given , , and , find • From torques to accelerations • Given , , and , find • From desired accelerations to necessary torques

30. Example: RP manipulator

31. Application: Effective Inertia • If a force is applied to a point on a robot, how much will accelerate?

32. Application: Effective Inertia • If a force is applied to a point on a robot, how much will accelerate? • Assume a stationary system, no acceleration when no force is applied • =0 • With the force:

33. Application: Effective Inertia • If a force is applied to a point on a robot, how much will accelerate? • Assume a stationary system, no acceleration when no force is applied • =0 • With the force: • The matrix is called the effective inertiamatrix • Can be infinite at singular configurations!

34. Application: Feedforward control • Feedback control: let torques be a function of the current error between actual and desired configuration • Problem: heavy arms require strong torques, requiring a stiff system • Stiff systems become unstable relatively quickly

35. Application: Feedforward control • Solution: include feedforward torques to reduce reliance on feedback • Estimate the torques that would compensate for gravity and coriolisforces, send those torques to the motors

36. Feedforward Torques • Given current , , desired • 1. Estimate B, C, G • 2. Compute u • 3. Apply torques u • How to compensate for errors in B,C,G? Combine feedforward with feedback. More in later classes…

37. Newton-Euler Method (Featherstone 1984) • Explicitly solves a linear system for joint constraint forces and accelerations, related via Newton’s equations • No matrix larger than 6x6 • Faster forward/inverse dynamics for large chains (O(n) vs O(n3) for direct matrix computations)

38. Forward Dynamics: Basic Intuition • Downward recursion: Starting from root, compute “articulated body inertia matrix” for each link • 6x6 matrix relating vectors to translational/angular accelerations respectively • Also need a “bias force” • Upward recursion: Starting from leaves, compute accelerations on links • Given acting on i’th link, compute acceleration of joint i and the joint constraint forces on the i-1’th link • includes external forces + joint constraint forces from downward links

39. Software • Both Lagrangian dynamics and Newton-Euler methods are implemented in KrisLibrary • Lagrangian form is usually most mathematically convenient representation

40. Constrained Dynamics

41. Constrained Systems • Suppose the system is constrained by • E.g., closed-chains, contact constraints, rolling constraints • A is a k x n matrix (k constraints) • How does evolve over time?

42. The Wrong Way • Suppose the system is constrained by • E.g., closed-chains, contact constraints, rolling constraints • A is a k x n matrix (k constraints) • How does evolve over time? • Wrong way: • Solve for as usual, then project it onto the subspace that satisfies this equation, obtaining • The correct answer will be a projection, but a very specific one!

43. The Right Way… • Constrained system of equations: • (1) • (2) • Lagrange multipliers have been introduced • can be thought of as constraint forces • Solve for n+k variables

44. Solving… • Constrained system of equations: • (1) • (2) • Solve for n+k variables • A solution must satisfy • (3) solve 1 for • (4) subst (3) in (2) (5) solve for in (4), use from (2) • (6) more manipulations.. • With

45. Back to Pseudoinverses • A pseudoinverse A# of the matrix A is a matrix such that • A = AA#A • A#= A#AA# • Generalizes the concept of inverse to non-square, noninvertible matrices • Such a matrix exists (in fact, there are infinitely many) • The Moore-Penrose pseudoinverse, denoted A+, can be derived as • A+ = (ATA)-1ATwhen ATA is invertible (overconstrained) • A+ = AT(AAT)-1 when AAT is invertible (underconstrained)

46. Properties • Note connection to least-squares formula • Ax=b => x = A+b • If system is overconstrained, this solution minimizes ||b-Ax||2 • If system is underconstrained, this solution minimizes ||x||2 • Note that (I-AA+)Ay = 0 is always satisfied • (I-AA+) is a projection matrix

47. Weighted Pseudoinverse • If (AAT)-1 exists, given any positive definite weighting matrix W, we can derive a new pseudoinverse • A# = W-1AT(AW-1AT)-1 • This is a weighted pseudoinverse • Has the property that x=A#b is a solution to Ax = b such that • x minimizes xTWx – a weighted norm

48. Weighted Pseudoinverse • If (AAT)-1 exists, given any positive definite weighting matrix W, we can derive a new pseudoinverse • A# = W-1AT(AW-1AT)-1 • This is a weighted pseudoinverse • Has the property that x=A#b is a solution to Ax = b such that • x minimizes xTWx – a weighted norm • Revisiting constrained dynamics… • The P projection matrix solves for such that is minimized • Constraint forces dissipate kinetic energy in a minimal fashion!

49. Rigid Body Simulators • Articulated robots are often simulated as a set of connected rigid bodies (Open Dynamics Engine, Bullet, etc) • Connections give rise to constraints in the dynamics • (1) • (2) • Solve for n+k variables • (1), (2) are sparse systems and are solved using specialized solvers • More on frictional contact later…

50. Next class • Feedback control • Principles App J