finding locally optimal, collision-free
TRANSCRIPT
![Page 1: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/1.jpg)
Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization
John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow and Pieter Abbeel
Pranay Pasula, Eugene Vinitsky
![Page 2: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/2.jpg)
Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization
The world’s fastest introduction to penalty methods and collision checking
Pranay Pasula, Eugene Vinitsky
![Page 3: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/3.jpg)
Problem
![Page 4: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/4.jpg)
Problem Statement
● Given object A and obstacles ● Find a trajectory that takes object A from to with
○ Minimal computation time ○ Minimal likelihood of collision○ Minimal cost
Figure from Xanthidis et al. 2019 “Navigation in the Presence of…”
![Page 5: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/5.jpg)
Key Insights● Mesh methods are really effective● Convex-convex collision checking is “fast”● Convex collision checking can be turned into a constraint● Trust region methods can be used to efficiently solve constrained optimization
problems
![Page 6: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/6.jpg)
Progress● Fast signed distance checking● Collision constraints● Solving the constrained problem
![Page 7: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/7.jpg)
Overview: Collision avoidance
Discrete-time● Signed distance (SD) between objects● SD constraints and penalty formulation● Linearizing SD to enable optimization
Continuous-time● Case 1: Translation only● Case 2: Translation + rotation
![Page 8: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/8.jpg)
Preliminaries
are labels for rigid objects/obstacles
are sets of points occupied by
![Page 9: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/9.jpg)
Penalizing collisions ideallyCollision penalty based on minimum translation distance between objects
![Page 10: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/10.jpg)
Penalizing collisions (kind of) practically
No collision:
Collision: No room for error!
Introduce safety margin
![Page 11: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/11.jpg)
But where do signed distances come from?
![Page 12: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/12.jpg)
GJKTwo objects intersect if their Minkowski difference contains the origin
Source: “Real Time Collision Detection”
![Page 13: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/13.jpg)
GJK basic idea: simple● Form Minkowski
difference● Iteratively find
closest points● Either gives
closure or distance
● NOT THE FASTEST WAY
Source: “Real Time Collision Detection”
![Page 14: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/14.jpg)
Expanding Polytope Method● Pick a polytope● Find closest point in
polytope● If point on edge, done● Else, expand polytope
to include support vector
Source: https://www.youtube.com/watch?v=6rgiPrzqt9w
![Page 15: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/15.jpg)
Expanding Polytope Method● Pick a polytope● Find closest point in
polytope● If point on edge,
done● Else, expand polytope
to include support vector
Source: https://www.youtube.com/watch?v=6rgiPrzqt9w
![Page 16: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/16.jpg)
Expanding Polytope Method● Pick a polytope● Find closest point in
polytope● If point on edge, done● Else, expand
polytope to include support vector
Source: https://www.youtube.com/watch?v=6rgiPrzqt9w
![Page 17: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/17.jpg)
Expanding Polytope Method● Pick a polytope● Find closest point in
polytope● If point on edge,
done● Else, expand polytope
to include support vector
Source: https://www.youtube.com/watch?v=6rgiPrzqt9w
![Page 18: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/18.jpg)
Signed distance constraints and reformulation
: set of robot links
: set of obstacles
What do we want?
Reformulate for our method
![Page 19: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/19.jpg)
Penalizing collisions practically
Enumerating over all combinations is prohibitive
Introduce
Compute cost only for pairs with
We have
![Page 20: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/20.jpg)
Progress● Fast signed distance checking● Collision constraints● Solving the constrained problem
![Page 21: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/21.jpg)
Making signed distance work for usWe have
But is non-linear!
1. Reformulate as maximin problem
2. Approx with first-order Taylor expansion wrt
3. Replace corresponding cost term with approx
4. Repeat for all pairs with distance
![Page 22: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/22.jpg)
Continuous-Time Collision Avoidance (Translation only)
![Page 23: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/23.jpg)
Continuous-Time Collision Avoidance (Translation + Rotation)
![Page 24: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/24.jpg)
Recap: Collision avoidance
Discrete-time● Signed distance (SD) between objects● SD constraints and penalty formulation● Linearizing SD to enable optimization
Continuous-time● Case 1: Translation only● Case 2: Translation + rotation
![Page 25: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/25.jpg)
Progress● Fast signed distance checking● Collision constraint● Solving the constrained problem
![Page 26: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/26.jpg)
Penalty Optimization● Start with a constrained problem
● Move the constraints into the cost with a penalty coefficient
● Optimize away
![Page 27: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/27.jpg)
Penalty Optimization
● But wait, won’t this change the optimum?
![Page 28: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/28.jpg)
Penalty OptimizationBut wait, won’t this change the optimum? Yep!
![Page 29: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/29.jpg)
Penalty Optimization
If you just make the penalty large enough, we’ll find the constrained local minimum1
Note, this isn’t necessarily true if we used quadratic penalties
Nocedal and Wright1
![Page 30: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/30.jpg)
Sequential Quadratic Optimization w/ Trust Region● Non-convex problem
![Page 31: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/31.jpg)
Sequential Quadratic Optimization w/ Trust Region● Expand to second order around your point
![Page 32: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/32.jpg)
Sequential Quadratic Optimization w/ Trust Region● Apply a trust region
![Page 33: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/33.jpg)
Sequential Quadratic Optimization w/ Trust Region● Apply a quadratic program
solver like IPOPT
![Page 34: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/34.jpg)
Trust Region Scaling● Apply a quadratic program
solver like IPOPT● Improved on the true problem?
● Didn’t?
![Page 35: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/35.jpg)
Penalty Scaling● Constraints unsatisfied?
![Page 36: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/36.jpg)
Progress● Fast signed distance checking● Collision constraints● Solving the constrained problem
![Page 37: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/37.jpg)
What’ve we got● A way to compute signed distance
constraints● A way to solve the optimization
formula
![Page 38: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/38.jpg)
CHOMP V. TrajOpt
CHOMP● Projected gradient
descent● Distance fields
TrajOpt● Sequential
Quadratic Programs
● Convex-Convex collision checking
![Page 39: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/39.jpg)
What does trajopt help with? CHOMP
![Page 40: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/40.jpg)
What does trajopt help with? CHOMP
Conflicting costs on body points
![Page 41: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/41.jpg)
What does trajopt help with? CHOMP
Just compute minimal translation
![Page 42: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/42.jpg)
What does trajopt help with? CHOMP
Fast distance checking using
spheres
![Page 43: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/43.jpg)
What does trajopt help with? CHOMP
Spheres over-approximate
convex objects
![Page 44: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/44.jpg)
What does trajopt help with? CHOMP
Don’t need to approximate for convex objects
![Page 45: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/45.jpg)
Comparison with other motion planning algorithms
198 arm planning problems with PR2 (7 DOF)
96 full body planning problems with PR2 (18 DOF)
![Page 46: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/46.jpg)
Pros + Cons+ Typically returns high quality path+ Works in high dimensions+ Faster than comparable optimizers+ Highly customizable- Highly customizable
- Must specific objective function, gradient descent step size, D_safe, etc.
- Not complete- Not optimal- Neglects structure of the problem during optimization- Initialization dependent- More complicated than many sampling based or graph search based methods
![Page 47: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/47.jpg)
Failure Modes
![Page 50: Finding Locally Optimal, Collision-Free](https://reader030.vdocuments.net/reader030/viewer/2022012516/618fdcfbb8400076297fc92d/html5/thumbnails/50.jpg)
Points for discussion● Other optimization schemes?● Ideas for tackling failure modes● GPU acceleration● Where does the speedup come from?
How to further speed up?