Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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, Daniela Rus

TL;DR

This paper proposes two novel algorithms for constructing convex collision-free polytopes in robot configuration space and proposes 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.

Abstract

We propose two novel algorithms for constructing convex collision-free polytopes in robot configuration space. Finding these polytopes enables the application of stronger motion-planning frameworks such as trajectory optimization with Graphs of Convex Sets [1] and is currently a major roadblock in the adoption of these approaches. In this paper, we build upon IRIS-NP (Iterative Regional Inflation by Semidefinite & Nonlinear Programming) [2] to significantly improve tunability, runtimes, and scaling to complex environments. IRIS-NP uses nonlinear programming paired with uniform random initialization to find configurations on the boundary of the free configuration space. 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. IRISNP2, also significantly faster than IRIS-NP, builds larger polytopes using fewer hyperplanes, enabling faster downstream computation. Website: https://sites.google.com/view/fastiris

Faster Algorithms for Growing Collision-Free Convex Polytopes in Robot Configuration Space

TL;DR

This paper proposes two novel algorithms for constructing convex collision-free polytopes in robot configuration space and proposes 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.

Abstract

We propose two novel algorithms for constructing convex collision-free polytopes in robot configuration space. Finding these polytopes enables the application of stronger motion-planning frameworks such as trajectory optimization with Graphs of Convex Sets [1] and is currently a major roadblock in the adoption of these approaches. In this paper, we build upon IRIS-NP (Iterative Regional Inflation by Semidefinite & Nonlinear Programming) [2] to significantly improve tunability, runtimes, and scaling to complex environments. IRIS-NP uses nonlinear programming paired with uniform random initialization to find configurations on the boundary of the free configuration space. 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. IRISNP2, also significantly faster than IRIS-NP, builds larger polytopes using fewer hyperplanes, enabling faster downstream computation. Website: https://sites.google.com/view/fastiris

Paper Structure

This paper contains 13 sections, 2 theorems, 10 equations, 6 figures, 2 tables, 3 algorithms.

Table of Contents

  1. Introduction
  2. Problem Formulation
  3. Related Works
  4. Background on IRIS Algorithms
  5. IRIS with Convex Obstacles
  6. Computing Separating Planes in Robot Configuration Space
  7. Improving the Separating Hyperplanes Routine
  8. Termination Condition for the Separating Planes Step
  9. The IRIS-ZO Algorithm
  10. The IRIS-NP2 Algorithm
  11. Experiments
  12. Discussion
  13. Acknowledgements

Key Result

theorem 1

Let $\varepsilon_{tr}\geq \varepsilon\geq 0$, $(X_i)_{M\geq i\geq1}\sim\mathrm{Bernoulli}(\varepsilon_{tr})$, with $M \geq 2 \log(1/\delta)/(\varepsilon \tau^2)$, for any fixed parameter $\tau>0$, and $\delta\in(0,1]$. Define $\bar{X}_M:=\frac{1}{M}\sum_{i=1}^MX_i$. It holds that

Figures (6)

  • Figure 1: The nonlinear closest-point-in-obstacle program seeks the closest configuration $q$, under the ellipsoidal metric $\mathcal{E}$, that lies inside of the current polytope $\mathcal{P}$ and causes the collision pair $\mathcal{A},~ \mathcal{B}$ to collide. Instead of constraining $q\in\mathcal{O}^{\mathcal{AB}}$ directly, we encode the collision constraint in task space with an auxiliary point $t$ that is constrained to lie in the in the intersection of both collision geometries. The picture on the right shows a feasible solution. A locally optimal solution to the program is indicated by the cross in the cartoon on the left.
  • Figure 2: A visualization of an IRIS-NP region for a simple two dof system. 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 intersect the disk obstacle. Right: Resulting region (red outline), when seeding at the red dot. The configuration in left frame is shown by the blue dot.
  • Figure 3: One iteration of ZeroOrderSeparatingPlanes. Left: Configuration space with grey configuration-space obstacles. First, a batch of samples is drawn uniformly in the current polytope and checked for collisions. Center: The found collisions $\mathcal{S}_\text{col}$, shown in red, are brought closer to the center of the ellipsoid using bisection. The updated candidate points $\mathcal{S}_\text{col}^\star$(red stars), are sorted by their weighted squared distance to the center of the ellipsoid under the ellipsoidal metric. Right - Hyperplanes are iteratively added for the non-redundant candidates in order. Note that the second candidate is made redundant by the first hyperplane, and therefore only four hyperplanes are added to $\mathcal{P}$.
  • Figure 4: Execution of NP2-SeparatingPlanes using the ray collision finder for GetConfigInCollision. Blue dots represent uniform samples in the polytope. Red dots represent the results of the corresponding discrete line searches. Stars represent the results of local nonlinear searches (\ref{['opt:closestcollision']}). On the right, the corresponding hyperplanes (green) form the updated polytope.
  • Figure 5: The systems we use to benchmark our algorithms, along with the number of degrees of freedom, number of collision pairs, and the ratio of the volume of the free configuration space relative to the entire configuration space $\mathcal{C}$. The ratio $\text{vol}(\mathcal{C}^\mathrm{free})/ \text{vol}(\mathcal{C})$ is estimated with $10^7$ samples uniformly drawn from $\mathcal{C}$.
  • ...and 1 more figures

Theorems & Definitions (3)

  • theorem 1: Sample Bound
  • definition 1: Unadaptive Test
  • corollary 1: Controlling False Accept