270

# Planning for Contact

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

## Zac Manchester

November 12, 2017

## Transcript

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

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

 µ
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

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