Planning for Contact

This is work presented at ISRR 2017 on motion planning for robots that experience contact.

Transcript

1. Planning for Contact Zac Manchester and Scott Kuindersma Harvard Agile Robotics Lab
2. minimize x(t),u(t) J(x(t), u(t)) = Z T 0 L(x(t), u(t)) dt Trajectory Optimization 2 ˙ x = f(x, u) subject to: umin  u  umax
3. minimize x(t),u(t) J(x(t), u(t)) = Z T 0 L(x(t), u(t)) dt Trajectory Optimization 3 ˙ x = f(x, u) subject to: umin  u  umax Model of your robot
4. minimize x(t),u(t) J(x(t), u(t)) = Z T 0 L(x(t), u(t)) dt Trajectory Optimization 4 ˙ x = f(x, u) subject to: umin  u  umax Model of your robot What you want it to do
5. minimize x1:N ,u1:N J(x1:N , u1:N ) = N X k=1 L(xk, uk) xk+1 = f(xk, uk) Direct Transcription 5 subject to: umin  u  umax
6. minimize x1:N ,u1:N J(x1:N , u1:N ) = N X k=1 L(xk, uk) xk+1 = f(xk, uk) Direct Transcription 6 subject to: umin  u  umax Must be smooth
7. How Do We Deal With Contact? 7
8. How Do We Deal With Contact? 8 Hybrid Approach: • Pre-specify contact sequence • Optimize over smooth segments of the trajectory • Pro: can have high integration accuracy • Con: contact mode pre-specification is impractical for complex robots/motions Smooth Segments Mode Switches
9. How Do We Deal With Contact? 9 Hybrid Approach: • Pre-specify contact sequence • Optimize over smooth segments of the trajectory • Pro: can have high integration accuracy • Con: contact mode pre-specification is impractical for complex robots/motions Contact-Implicit Approach: • Include contact dynamics as nonlinear constraints • Optimize over entire trajectory and compute contact forces • Pro: can generate gaits/complex contact sequences • Con: typically poor (1st order) integration accuracy
10. How Do We Deal With Contact? 10 Hybrid Approach: • Pre-specify contact sequence • Optimize over smooth segments of the trajectory • Pro: can have high integration accuracy • Con: contact mode pre-specification is impractical for complex robots/motions Contact-Implicit Approach: • Include contact dynamics as nonlinear constraints • Optimize over entire trajectory and compute contact forces • Pro: can generate gaits/complex contact sequences • Con: typically poor (1st order) integration accuracy Can we get the best of both worlds?
11. Existing Contact-Implicit Methods 11 (+ contact constraints) =) xk+1 ⇡ xk + f(xk, uk) t M ¨ q + C(q, ˙ q) + G = Bu + JT
12. Existing Contact-Implicit Methods 12 minimize q(t) S = Z tf t0 L (q(t), ˙ q(t)) dt =) xk+1 ⇡ xk + t f(xk, uk) (+ contact constraints) =) Least-Action Principle M ¨ q + C(q, ˙ q) + G = Bu + JT
13. Least-Action Principle 13 minimize q(t) S = Z tf t0 L (q(t), ˙ q(t)) dt
14. Discrete Mechanics 14 minimize q1:N S ⇡ N X k=0 L ✓ qk + qk+1 2 , qk+1 qk t ◆ t
15. Add Contact Constraints… 15 minimize q1:N S ⇡ N X k=0 L ✓ qk + qk+1 2 , qk+1 qk t ◆ t subject to (qk+1) 0 @ @q (q)
16. Discrete Euler-Lagrange Equation 16 Complementarity Conditions @ @qk  L ✓ qk 1 + qk 2 , qk qk 1 h ◆ + L ✓ qk + qk+1 2 , qk+1 qk h ◆ + k @ @qk+1 = 0 k 0 (qk+1) 0 k (qk+1) = 0
17. Coulomb Friction 17 minimize b ˙ E subject to kbk  µ
18. Trajectory Optimization Problem 18 minimize h, Q, U, C J(h, Q, U, C) subject to f(h, qi 1, qi, qi+1, i, i, ⌘i) = 0 g(qi+1, i, i, ⌘i, si) 0 umin  ui  umax hmin  h  hmax
19. Trajectory Optimization Problem 19 minimize h, Q, U, C J(h, Q, U, C) subject to f(h, qi 1, qi, qi+1, i, i, ⌘i) = 0 g(qi+1, i, i, ⌘i, si) 0 umin  ui  umax hmin  h  hmax Cost Function
20. Trajectory Optimization Problem 20 minimize h, Q, U, C J(h, Q, U, C) subject to f(h, qi 1, qi, qi+1, i, i, ⌘i) = 0 g(qi+1, i, i, ⌘i, si) 0 umin  ui  umax hmin  h  hmax Discrete Dynamics Cost Function
21. Trajectory Optimization Problem 21 minimize h, Q, U, C J(h, Q, U, C) subject to f(h, qi 1, qi, qi+1, i, i, ⌘i) = 0 g(qi+1, i, i, ⌘i, si) 0 umin  ui  umax hmin  h  hmax Discrete Dynamics Contact Stuff Cost Function
22. Accuracy 22 ational Contact-Implicit Trajectory Optimization 2 4 6 8 10 12 14 16 0 0.5 1 1.5 2 2.5 Knot Points RMS Error First Order Variational al Contact-Implicit Trajectory Optimization 13 1.5 2 2.5 First Order Variational
23. Spring Flamingo 23 ≈ • 18-states • Passive spring ankles • Heel/toe contacts • Minimize energy
24. Spring Flamingo 24
25. Little Dog 25
26. Little Dog on Stairs 26
27. Limping Dog 27
28. Summary 28 • Applying standard trajectory optimization ideas to the Least Action Principle results in high-order methods for simulating rigid body dynamics with contact. • We can use these ideas to build trajectory optimization algorithms for motion planning with contact. • The new algorithms achieve better accuracy with smaller problem sizes than state-of-the-art first-order methods. zmanchester@seas.harvard.edu http://agile.seas.harvard.edu