Planning for Contact

6348c7765745abd70d165eb3e34eca1b?s=47 Zac Manchester
November 12, 2017

Planning for Contact

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

6348c7765745abd70d165eb3e34eca1b?s=128

Zac Manchester

November 12, 2017
Tweet

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