Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space
Peter Werner, Thomas Cohn*, Rebecca H Jiang*, Tim Seyde, Max Simchowitz, Russ Tedrake, and Daniela Rus
Embedded Files
TLDR Decomposing robot configuration spaces into collision-free polytopes enables the application of stronger motion-planning frameworks such as trajectory optimization with Graphs of Convex Sets and is currently a major roadblock in the adoption of these approaches. In this paper we aim to take a step towards alleviating this roadblock by introducing substantial improvements to IRIS-NP (Iterative Regional Inflation by Semidefinite & Nonlinear Programming). Our key insight is that finding near-by configuration-space obstacles using sampling is inexpensive and greatly accelerates region generation. We propose two algorithms using such samples to either employ nonlinear programming more efficiently (IRIS-NP2) or circumvent it altogether using a massively-parallel zero-order optimization strategy (IRIS-ZO). We also propose a termination condition that controls the probability of exceeding a user-specified permissible fraction-in-collision, eliminating a significant source of tuning difficulty in IRIS-NP. We compare performance across eight robot environments, showing that IRIS-ZO achieves an order-of-magnitude speed advantage over IRIS-NP. IRIS-NP2, also significantly faster than IRIS-NP, builds larger polytopes using fewer hyperplanes, enabling faster downstream computation.
Collision-free Polytopes in Robot Configuration Space
In our paper we compute probabilistically collision-free polytopes in robot configuration space. A visualization of such a polytope for a simple two degree of freedom (dof) system is shown above. Left: Two dof robot arm with a disk shaped obstacle. Two selected collision geometries of the system are highlighted in green and blue. Center: Configuration space of the system. The black regions correspond to collisions. Two of the configuration-space obstacles are highlighted in blue and green, corresponding to the configurations where the blue and green collision geometries of the robot intersect the disk obstacle. Right: Resulting polytope (red outline), when seeding IRIS-NP2 at the red dot. The configuration in left frame is shown by the blue dot.
Random Walks Inside of Generated Regions
Below a selection of 4 regions generated with IRIS-NP2 are animated. This is done by starting at the seed configuration and repeatedly picking a random direction and walking to the bounary of the region. In the animation we return to the seed configuration every 4 steps.
Click on the imaes below to open a full screen animation. Hit 'Open Controls' then 'Animation->Play' to play back the animation. Click on the 'drake' tab to visualize the collision geometries.
IRIS-ZO
The IRIS-ZO algorithm uses a simple parallelized zero-order optimization strategy to directly solve the SeparatingPlanes in Alg 1. in the paper (the polytope generation step). IRIS-ZO constructs a probabilistically collision-free polytope by using sampling and collision checking. Above is an illustration of how ZeroOrderSeparatingPlanes computes the polytope. The algorithm is fast and simple to implement, because it only requires a collision-checker and no gradient information, but produces slightly lower quality regions (smaller volume, more faces) as opposed to IRIS-NP2.
IRIS-NP2
The IRIS-NP2 algorithm improves IRIS-NP both in terms of computation time and region quality by increasing the volume and reducing the number of faces per region). The key update to the algorithm is that instead of looping over all collision pairs and solving for near-by configurations that cause this pair to collide, we use sampling strategies to decide which collision pairs to consider. In our paper we investigate two sampling strategies: "greedy" and "ray." Above is an illustration of using the ray sampling strategy to decide which collision pairs to consider. In the ray sampling strategy random configurations (blue dots) are sampled and a linesearch is performed until a collision is found (red dots). The red dots along with the corresponding collision pair are used to warmstart the nonlinear optimization that finds near-by collisions. Below is an illustration of how this nonlinear program is formulated for a given collision pair (A,B). On the left we see a sketch of the current polytope in configuration space. The remaning two frames show the task space of the robot along with the highlighted collision pair.
Note that this program remains unchanged from the original IRIS-NP.
Experiments
We benchmark our alogrithms on 8 robotic systems shown on the left. They range from 3 degrees of freedom (dof) to 15 dof. In each of these environments we hand-select 10 interesting seed configurations and compare the resulting regions obtained from the different algorithms. We use the original IRIS-NP algorithm as a baseline.
Statistics averaged over all 10 seed configurations. We consider two settings: A "fast" setting where the termination condition is more relaxed, allowing for a larger fraction of the polytope to be in collision and a more stringent "precise" setting. For the "fast" setting IRIS-ZO was on average around 15.5 times faster while requiring 1.4 times fewer hyperplanes than IRIS-NP. Iris-NP2 with the greedy strategy was 6.6 times faster while requiring 2.1 times fewer hyperplanes. The ray strategy was 4.2 times faster and required 2.3 times fewer hyperplanes. For the "precise" settings IRIS-ZO was 14 times faster than IRIS-NP while producing around the same number of hyperplanes, while IRIS-NP2 with the greedy strategy was 12.3 times faster with 2.4 fewer hyperplanes. With the ray strategy it was 8 times faster with 2.7 times fewer hyperplanes.
Acknowledgements
We gratefully acknowledge the support of several individuals and organizations in the completion of this work. Our thanks go to The Charles Stark Draper Laboratory, Inc. and Ravi Gondhalekar for their valuable feedback and for supporting Rebecca Jiang as a Draper Scholar. We also extend our appreciation to Amazon.com (PO No. 2D-06310236), the Toyota Research Institute, and the MIT Quest for Intelligence for their financial support. Finally, we thank Alexandre Amice for providing insightful feedback on the project.
Page updated
Google Sites
Report abuse