Faster algorithms for growing collision-free convex polytopes in robot configuration space

Abstract

We propose two novel algorithms for constructing convex probabilistically 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 (Marcucci et al., 2023) and is currently a major roadblock in the adoption of these approaches. In this paper, we build upon the IRIS-NP algorithm (Iterative Regional Inflation by Semidefinite & Nonlinear Programming) of Petersen and Tedrake (2023) 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 nearby 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). Both algorithms employ a novel 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 further present an approach for applying both algorithms in parametrized configuration spaces. We compare the performance across eight robot environments, showing that IRIS-ZO achieves an order-of-magnitude speed advantage over IRIS-NP, which is extended roughly by an additional order of magnitude by parallelizing it with a GPU. IRIS-NP2, also significantly faster than IRIS-NP, builds larger polytopes using fewer hyperplanes which has the additional benefit of accelerating downstream motion planning. Website: https://sites.google.com/view/fastiris.

Keywords

collision-free motion planning robot configuration space convex decomposition

1. Introduction

A major challenge in robot motion planning is the need to simultaneously consider task space, the world in which the robot physically resides, and configuration space, $C$ , the set of all possible robot configurations. The planner must produce a trajectory in the configuration space, but many constraints are formulated in the task space. Collision avoidance is particularly challenging, because even geometrically simple obstacles in task space can have intractably complicated descriptions when transformed into configuration space through the robot’s inverse kinematics. While there is work on constructing explicit configuration-space representations of workspace obstacles (Bajaj and Kim, 1990; Lozano-Pérez, 1990; Pan and Manocha, 2015), these methods are intractable for the high degree-of-freedom (dof) robotic systems being used today.

The unavailability of obstacles’ configuration-space descriptions has not prevented the development of a rich literature of motion planning algorithms. Many approaches approximate the set of collision-free configurations $(C^{f r e e})$ without explicitly constructing the individual obstacles. Interval analysis and cell decompositions can approximate $C^{f r e e}$ as the union of boxes (Jaulin, 2001; Latombe, 2012, §5.6), but these methods are computationally intractable for high-dimensional configuration spaces. Perhaps the most widely used technique has been sampling-based planning, in which samples are drawn from $C^{f r e e}$ and connected into a graph structure.

These representations of $C^{f r e e}$ are popular due to their simplicity and versatility. However, they all struggle with the “curse of dimensionality”—the memory use of the representation may grow exponentially with the dimension of the configuration space. This has led the motion planning community to explore volumetric approximations of $C^{f r e e}$ such as the union of spheres used by Yang and LaValle (2004) or the polytopes constructed by Deits and Tedrake (2015). In contrast to the grid- or sampling-based approaches, the individual sets used in these approaches describe free-space regions rather than points. Although these sets are harder to construct, they often enable a more concise approximation of $C^{f r e e}$ and yield convex (probabilistic) collision-avoidance constraints.

Modern decomposition-based motion planning algorithms (DBMPs) leverage these representations to quickly produce high-quality, collision-free trajectories and can scale to high-dimensional robotic systems (Chen et al., 2016; Chew Chia et al., 2024; Liu et al., 2017; Marcucci et al., 2023, 2024; Morozov et al., 2024, 2025; Natarajan et al., 2024; Wu et al., 2024). Furthermore, by nature of their use of optimization, these planners can handle a variety of costs and constraints. However, the performance of these planners is highly dependent on the properties of the convex sets. It is desirable for these sets to have large volumes in order to reduce the number of sets required to approximate $C^{f r e e}$ , while retaining simple descriptions to make downstream planning more efficient. While creating perfectly collision-free sets is very costly, practical algorithms should provide a straightforward way to trade off between precision and runtime.

Recent results for planning on Graph of Convex Sets (GCSTrajOpt) leverage the IRIS-NP algorithm (Cohn et al., 2024; Marcucci et al., 2023; Morozov et al., 2024, 2025). IRIS-NP takes in a collision-free seed configuration and attempts to construct a convex, collision-free polytope containing it. IRIS-NP, however, falls short of meeting the aforementioned criteria in practice. Its runtime is substantial and trading off between runtime and correctness (how much of the region is collision-free) proves challenging. IRIS-NP only terminates after failing to solve a user-specified number of nonlinear programs in succession, a time-consuming process dependent on this user-specified parameter acting as a proxy for correctness, when the actual relationship is unclear. Furthermore, these nonlinear programs must be run separately for every object in the scene, causing IRIS-NP to scale poorly with environment complexity.

In this paper, we improve upon IRIS-NP, focusing on a key subroutine that constructs hyperplanes to separate the seed point from obstacles. Our improvements leverage the fact that we can evaluate thousands of configurations for collisions in the time it takes to solve a single nonlinear optimization problem. We employ random sampling and collision-checking to estimate the proportion of the polytope that is collision-free, providing a rigorous probabilistic certificate and enabling intuitive tradeoffs between region correctness and computation times. We further present two algorithms that utilize sampling and collision-checking to improve polytope generation: IRIS-ZO rapidly generates polytopes using a simple parallelized zero-order optimization strategy that requires no gradient computations and is easy to implement. IRIS-NP2 uses sampled collisions to seed nonlinear optimizations, increasing search success and dramatically reducing the required number of programs. We demonstrate that both algorithms outperform IRIS-NP in terms of computation time and region quality.

This paper further extends the initial conference paper (Werner et al., 2024b) in several ways. We demonstrate that our algorithmic improvements accelerate downstream motion planning and lead to lower-cost plans, and show that IRIS-ZO can be further sped up by a factor of 10 by leveraging GPU-parallelism. Finally, we show how both IRIS-ZO and IRIS-NP2 can be used to grow regions under any smooth parametrization of configuration space. This allows us to grow regions for kinematically constrained systems (e.g., for constrained bimanual manipulation), even when the feasible set is measure-zero, as in (Cohn et al., 2024). An open-source implementation of our method is made available as part of the Drake modeling and simulation framework (Tedrake and the Drake Development Team, 2019).

2. Problem formulation

In this section, we introduce the problem formulation and discuss the required inputs and provided outputs of our proposed algorithms.

We aim to generate polytopes¹ in configuration space whose volume is large, and whose fraction in collision is less than a user-provided constant. More precisely, let λ denote the Lebesgue measure in the free configuration space $C^{f r e e}$ . Given a user-specified admissible fraction in collision ɛ ∈ (0, 1) and confidence δ ∈ (0, 1), we will compute positive-volume polytopes $R \subseteq C$ such that

\begin{matrix} \Pr [\frac{λ (R \ C^{f r e e})}{λ (R)} > ε] \leq δ . \end{matrix}

(1)

Since obtaining a closed-form description of $C^{f r e e}$ is intractable for general robotic systems according to (LaValle, 2006, §4.3.3) and (Latombe, 2012, §3), our algorithms utilize a task-space description. Such descriptions are readily provided via common robot description formats such as URDFs (http://ros.org/ROSorg, 2024) or SDFs (Open Source Robotics Foundation, 2024).

We expect the robotic system to be described as M sets $G_{i} \subseteq R^{N_{ts}}$ representing collision geometries in task space. Here, N_ts ∈ {2, 3} is the dimension of the task space, and $q \in C$ is the configuration. The sets $G_{i}$ are represented in their body frame B_i. Using monogram notation (Tedrake, 2023, §3.1), each $G_{i}$ is paired with a configuration-dependent rigid-body ${transformation}^{W} X^{B_{i}} (q) \in S E (N_{ts})$ that defines the forward kinematics of $G_{i}$ in the world frame W. We use the shorthand $G_{i} (q)$ for $G_{i}$ expressed in the world frame:

G_{i} (q) = {p_{W} \in R^{N_{ts}} | p_{W} =^{W} X^{B_{i}} (q) \cdot p_{B_{i}}, p_{B_{i}} \in G_{i}} .

We let $G (q) = {G_{1} (q), G_{2} (q), \dots, G_{M} (q)}$ be the set of all collision geometries.

For a configuration q, the system is in collision if there exists a valid² pair of collision geometries $A (q), B (q) \in G (q)$ such that $A (q) \cap B (q) \neq \emptyset$ . For a valid collision pair $A (q), B (q) \in G (q)$ , the corresponding configuration-space obstacle $O^{A B} (q)$ is implicitly defined as

\begin{align} O^{A B} (q) : = \{q | A (q) \cap B (q) \neq \emptyset\} \end{align}

(2)

We avoid explicitly describing $O^{A B}$ by using non-convex task-space constraints, as in the work of Petersen and Tedrake (2023).

3. Related works

Algorithms for computing positive-volume subsets of $C^{f r e e}$ can be divided into two categories: those which require explicit descriptions of the obstacles in configuration space, and those that can use implicit descriptions. Explicit descriptions of obstacles are generally only available for robots with simple kinematics (e.g., only prismatic joints).

If descriptions of all obstacles are given as convex sets, the original IRIS algorithm can construct large collision-free polytopes about a seed points using a series of convex optimizations (Deits and Tedrake, 2015). Such descriptions can be obtained from arbitrary meshes via approximate convex decomposition techniques (Mamou and Ghorbel, 2009). Wu et al. (2024) use a similar approach to grow polytopes around an existing trajectory, towards producing a shorter, collision-free path.

Due to the complex kinematics of robotic manipulators, obstacles in configuration space are frequently given by implicit descriptions. Yang and LaValle (2004) leveraged the kinematic Jacobian to relate motion in configuration-space and task space, allowing the construction of collision-free ellipsoids. Unfortunately, large numbers of ellipsoids are required to approximate even simple, low-dimensional configuration spaces.

The original IRIS algorithm has been extended to handle such implicit descriptions in two ways. IRIS-NP (Petersen and Tedrake, 2023) uses nonlinear programming to find multiple locally separating hyperplanes to each obstacle, until the program becomes infeasible—an imprecise termination condition that often leaves some obstacle volume in regions. Jaitly and Farzan (2024) modified IRIS-NP to use a nonuniform sampling strategy to seed the collision search program. The other option is to use a rational parametrization of the kinematics to construct regions that are rigorously certified to be collision-free with sums-of-squares programming (Dai et al., 2024). However, such optimizations are computationally expensive, and the regions are grown in a stereographic projection of configuration space, which distorts distances.

Alternatively, Sarmientoy et al. (2005) directly decompose three-dimensional spaces into polytopes only using sample-based collision-checking. Unfortunately, this approach requires dense sampling of the configuration space to produce large sets which is intractable in all but the simplest cases.

4. Background on IRIS algorithms

In this section, we review the key algorithms our approach builds upon. The IRIS algorithm (Deits and Tedrake, 2015) takes in a list of convex obstacles and produces a collision-free polytope around a seed point in the same space as these obstacles. IRIS-NP (Petersen and Tedrake, 2023) generalizes IRIS to operate in configuration space using nonlinear programming.

4.1. IRIS with convex obstacles

IRIS takes as input N_o convex obstacles $O_{i}$ and a collision-free seed point $s \in C^{f r e e}$ . IRIS then computes a large collision-free polytope by alternating between two convex optimizations: the SeparatingPlanes step, that produces a polytope conditioned on an initial ellipsoid, and the InscribedEllipsoid step, which updates the ellipsoid to the maximum-volume inscribed ellipsoid (MVIE) in the current polytope.³ These alternations guarantee the containment of the ellipsoid, and hence, guarantee that the volume of the MVIE is monotonically increasing across alternations. To understand the upcoming modifications to IRIS-NP, we review the SeparatingPlanes step in detail.

The SeparatingPlanes step starts with a collision-free ellipsoid $E$ ,

E = {x | {(x - c)}^{T} E (x - c) \leq 1, E ≻ 0},

(3)

that is centered at c with a symmetric, positive-definite matrix E. For each of the N_o obstacles, IRIS computes a hyperplane that separates c from

O_{i}

and passes through the point in

O_{i}

that lies closest to the ellipsoid center c in the metric of the ellipsoid. This is done by solving

\begin{matrix} minimize ‖ x_{i} - c ‖_{E}^{2} subject to x_{i} \in O_{i}, \end{matrix}

(4)

where

‖ x ‖_{E}^{2} : = x^{T} E x

. Given the optimum

x_{i}^{⋆}

, the hyperplane

\begin{matrix} H_{i} = {x | a_{i}^{T} x - b_{i} = 0}, a_{i} = E (x_{i}^{⋆} - c), b_{i} = a_{i}^{T} x_{i}^{⋆}, \end{matrix}

separates c from

O_{i}

and does not intersect

E

(Deits and Tedrake, 2015, §3.5). This step is performed for each obstacle. Finally, the intersection of the halfspaces

a_{i}^{T} x \leq b_{i}

yields an updated collision-free polytope.

4.2. Computing separating planes in robot configuration space

IRIS requires convex obstacles to ensure the convexity of (4). However, configuration-space obstacles are generally non-convex. IRIS-NP generalizes the closest-point-in-obstacle program (4) for the configuration-space obstacle $O^{A B}$ . To avoid intractable descriptions of $O^{A B}$ , the collision constraint is encoded in task space. Optimization variables represent a point in each of the collision geometries, $t_{A} \in A$ , $t_{B} \in B$ , in their body frames, constrained ${by}^{W} X^{B_{A}} t_{A} =^{W} X^{B_{B}} t_{B}$ , to coincide in task space when transformed through the forward kinematics. For clarity, in this paper, we write this as requiring a single point t to lie in the intersection of the associated pair of collision geometries in task space. This is shown in Figure 1. In order to ensure that new hyperplanes address obstacles still relevant to the current polytope $P$ , IRIS-NP requires the found points in collision to lie inside $P$ . The full nonlinear program then reads:

\begin{align} \underset{q, t}{minimize} & ‖ q - c ‖_{E}^{2} \end{align}

(5a)

\begin{align} subject to & t \in A (q) \cap B (q), \end{align}

(5b)

\begin{align} q \in P . \end{align}

(5c)

Figure 1.

The nonlinear closest-point-in-obstacle program seeks the closest configuration q, under the ellipsoidal metric $E$ , that lies inside of the current polytope $P$ and causes the collision pair $A, B$ to collide. Instead of constraining $q \in O^{A B}$ 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.

Because configuration-space obstacles are generally non-convex, multiple hyperplanes may be necessary to separate c from $O^{A B}$ .

To construct the separating hyperplanes for the collision pair $(A, B)$ , IRIS-NP repeatedly finds locally optimal solutions q^⋆ to (5), adding the hyperplane

\begin{matrix} H_{i} = {x | a_{i}^{T} q^{⋆} - b_{i} + Δ = 0}, \\ a_{i} = E (q^{⋆} - c), b_{i} = a_{i}^{T} q^{⋆} \end{matrix}

P

for each of the found points q^⋆ in collision. More precisely, the polytope

P

is updated by intersecting

P

with the halfspace

{q | a_{i}^{T} q \leq b_{i} - Δ}

, where Δ > 0 is a user-specified stepback. This stepback ensures that the collision configuration q^⋆ is excluded from

P

, and prevents the need for an infinite number of hyperplanes to exclude a non-convex obstacle. This is repeated until a fixed number of attempted solves of (5) fail; IRIS-NP interprets this failure as a suggestion that

P

is sufficiently collision-free. However, note that due to the local nature of nonlinear programming, a single failed solve often provides inconclusive information about the feasibility of the overall program.

To summarize, the IRIS-NP strategy to approximating SeparatingPlanes in robot configuration space is to create a list of all valid collision pairs ordered by their task-space distance, and, for each collision pair, repeatedly find locally optimal solutions to (5) and update $P$ until a user-specified number of consecutive solve attempts fail. Through this procedure, IRIS-NP strives to make $P$ collision-free with respect to all valid collision pairs. See Figure 2 for an illustration.

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.

Looping over all collision pairs in a fixed order is a major inefficiency leading to longer computation times and polytopes with needlessly many hyperplanes. In the following, we propose two ways of using sampling and collision-checking in order to improve the SeparatingPlanes-step and eliminate this fixed-order traversal of all collision pairs from the algorithm, making our approaches significantly more performant.

5. Improving the separating hyperplanes routine

This section discusses how we leverage sampling to produce a rigorous termination condition for SeparatingPlanes in robot configuration space (see sec. 4.2), and two improved approaches for solving the step, yielding the new algorithms IRIS-ZO and IRIS-NP2, whose parameters are given in Table 1. Both of these new formulations follow the same alternation scheme as IRIS-NP, given in Alg. 1.

Table 1.

Glossary of algorithm parameters.

Parameter	Description	IRIS-ZO	IRIS-NP2
ɛ	Admissible fraction of the region in collision	✓	✓
δ	Max admissible uncertainty	✓	✓
Δ	Configuration margin, that is, “step back”	✓	✓
T	Termination condition (as in Petersen and Tedrake (2023))	✓	✓
N _p	Number of optimized particles per inner iteration	✓	✓
N _b	Number of bisection steps	✓	✗
N _f	Max number of hyperplanes added per inner iteration	✓	✓

In particular, we still initialize the ellipsoid with a small ball of radius r_start and employ the same overall termination conditions for the alternations (Petersen and Tedrake, 2023, §II.D): the seed point s no longer being contained in $P$ , reaching a maximum number of alternations, or achieving convergence of the ellipsoid volume. Our proposed termination condition for SeparatingPlanes decides when $P$ meets a correctness criterion based on polytope fraction in collision, not when to terminate the alternations. We discuss the proposed termination condition in Sec. 5.1, before discussing IRIS-ZO in Sec. 5.2, and IRIS-NP2 in Sec. 5.3.

5.1. Termination condition for the separating planes step

In this section, we discuss a termination condition that allows a user to specify a desired bound on the fraction of the volume of the polytope $P$ in collision. In the following, let $ε_{t r} : = λ (P \ C^{f r e e}) / λ (P)$ denote the true fraction in collision of $P$ , where λ is the Lebesgue measure over $C^{f r e e}$ , and ɛ is the specified admissible fraction in collision. A naive approach may be to sample uniformly in the polytope $P$ , estimate the fraction of $P$ in collision $\hat{ε}$ to be equal to the fraction of these samples that are in collision, and terminate if $\hat{ε} \leq ε$ . However, each time we perform this check, there is some chance of underestimating ɛ_tr such that we incorrectly terminate with a polytope with ɛ_tr > ɛ. The probability of false termination accumulates over the multiple evaluations of this condition.

Instead, we propose a statistical test that controls the probability of falsely claiming a polytope is sufficiently collision-free and terminating. For some user-specified uncertainty δ, a correct termination condition allows a region with ɛ_tr > ɛ to be returned with probability at most δ. To accomplish this, we pair union bounds with a simple statistical test based on a Chernoff bound.

We sample a batch of M points $q_{i} \in P$ uniformly. We then assign X_i = 0 if $q_{i} \in C^{f r e e}$ and X_i = 1 otherwise. Note that X_i ∼Bernoulli(ɛ_tr). The employed bound reads as follows.

Theorem 1. (Sample Bound). Let ɛ_tr ≥ ɛ ≥ 0, ${(X_{i})}_{M \geq i \geq 1} \sim B e r n o u l l i (ε_{t r})$ , with M ≥ 2 log(1/δ)/(ɛτ²), for any fixed parameter τ > 0, and δ ∈ (0, 1]. Define ${\bar{X}}_{M} ≔ 1 / M \sum_{i = 1}^{M} X_{i}$ . It holds that

\Pr [{\bar{X}}_{M} \leq (1 - τ) ε] \leq δ .

(6)

Proof. Observe that

\Pr [{\bar{X}}_{M} \leq (1 - τ) ε] \leq Prob [{\bar{X}}_{M} \leq (1 - τ) ε_{t r}] .

If τ > 1 both sides evaluate to 0. Otherwise, it holds that (1 − τ)ɛ ≤ (1 − τ)ɛ_tr. Next, we use the standard multiplicative Chernoff bound

\Pr [{\bar{X}}_{M} \leq (1 - τ) ε_{t r}] \leq e^{- M ε_{t r} τ^{2} / 2}

(Alon and Spencer 2016, §A, Thm A.1.15), and use the fact that $e^{- M ε_{t r} τ^{2} / 2} \leq e^{- M ε τ^{2} / 2}$ . We have now shown that

\Pr [{\bar{X}}_{M} \leq (1 - τ) ε_{t r}] \leq e^{- M ε τ^{2} / 2},

and the final result follows from evaluating

M = 2 log (1 / δ) / (ε τ^{2}) . □

Definition 1

(Unadaptive Test). $U n a d a p t i v e T e s t (δ, ε, τ)$ collects M = ⌈2 log(1/δ)/(ɛτ²)⌉ samples ${(X_{i})}_{1 \leq i \leq M}$ . $U n a d a p t i v e T e s t$ returns reject if ${\bar{X}}_{M} > (1 - τ) ε$ , and accept otherwise.

Corollary 1

(Controlling False Accept). Suppose ɛ_tr ≥ ɛ, that is,. the true fraction in collision is higher than allowed, and we have ${(X_{i})}_{1 \leq i \leq M} \sim B e r n o u l l i (ε_{t r})$ . Then, $U n a d a p t i v e T e s t (δ, ε, τ)$ rejects with probability at least 1 − δ.

Thus, our test behaves as follows: Assume we are given a polytope $P$ , an admissible fraction in collision ɛ, an admissible target uncertainty δ, and a batch of M uniform samples in $P$ with M = ⌈2 log(1/δ)/(ɛτ²)⌉. If it holds that $P$ does not meet the specifications, that is, ɛ_tr ≥ɛ, then $U n a d a p t i v e T e s t (δ, ε, τ)$ rejects $P$ with probability greater than 1 − δ; the test only falsely accepts $P$ with probability less than δ. Observe that if τ decreases, we require many samples and the test becomes expensive. Conversely, if we increase τ, the number of samples decreases, but the test starts rejecting every $P$ . In practice, we choose τ = 0.5 to balance power and computational cost of the test.

Within SeparatingPlanes, for both algorithms, hyperplanes are iteratively added to $P$ until sufficient separation from the configuration-space obstacles is achieved. We refer to these iterations as inner iterations. In order to decide whether the polytope is sufficiently collision-free at the k-th inner iteration, we run $U n a d a p t i v e T e s t (δ_{k}, ε, τ)$ . We can bound the probability of SeparatingPlanes terminating falsely after K iterations by the union bound

\begin{matrix} \Pr (“false accept” if λ (P \ C^{f r e e}) / λ (P) > ε) \leq \sum_{k = 1}^{K} δ_{k} . \end{matrix}

(7)

In order to control the probability of falsely accepting, that is, terminating, after an unknown number of inner iterations, in the case where a single outer iteration is to be run, we simply select δ_k according to the sequence δ_k = 6δ/(π²k²) for the k-th test, as this sequence sums to δ as k → ∞. If an unknown number of outer iterations (i.e., alternations between updating the polytope and the ellipsoid) are to be run, we must account for the additional UnadaptiveTest queries. In order for the probability of falsely accepting to sum to δ over infinite inner and outer iterations, we run $U n a d a p t i v e T e s t (δ_{i, k}, ε, τ)$ , in the i-th outer iteration, after the k-th inner iteration, with δ_i,k ≔ 36δ/(π⁴i²k²).

Note that this termination condition assumes uniform samples in $P$ . In practice, we generate approximately uniform samples using the hit-and-run sampling method of Lovász (1999), as alternatives such as rejection sampling rapidly become intractable in high-dimensional spaces. Hit-and-run sampling may require a high (but still polynomial in dimension and geometric factors) number of mixing steps per sample in order to closely match a uniform distribution in the worst case (Lovász and Vempala, 2004). For our experiments, we found a lower number of mixing steps can still achieve the desired results, while also greatly speeding up the algorithm.

5.2. The IRIS-ZO algorithm

The IRIS-ZO algorithm uses a simple parallelized zero-order optimization strategy to directly solve SeparatingPlanes in Alg. 1 for all collision pairs simultaneously. We call this subroutine ZeroOrderSeparatingPlanes and summarize it in Alg. 2. Figure 3 illustrates how ZeroOrderSeparatingPlanes optimizes hyperplanes.

Figure 3.

One iteration of ZeroOrderSeparatingPlanes. Left: Configuration space with gray configuration-space obstacles. First, a batch of samples is drawn uniformly in the current polytope and checked for collisions. Center: The found collisions $S_{col}$ , shown in red, are brought closer to the center of the ellipsoid using bisection. The updated candidate points $S_{col}^{⋆}$ (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 $P$ .

The ZeroOrderSeparatingPlanes step constructs a probabilistically collision-free polytope $P$ by repeating 5 steps until the statistical test passes. We take as input the domain $D$ , the current ellipsoid $E$ , the current outer iteration i and options that are summarized in Tab. 1. Here, we assume that the center of the current ellipsoid is collision-free. We then initialize $P$ with the domain $D = {q | A_{D} q \leq b_{D}}$ (typically a polytope describing the joint limits).

The first step is to uniformly sample a batch of configurations $S$ in $P$ via hit-and-run sampling (Lovász, 1999) with a batch size of max{M, N_p}. Then, we find all configurations $S_{col} \subseteq S$ that are in collision by running a collision checker. These samples are used to check the termination condition by running the unadaptive test, returning $P$ if it accepts. More precisely, counting the collisions $| S_{col}^{M} |$ in the first M samples in $S$ and accepting if $| S_{col}^{M} | \leq M (1 - τ) ε$ .

If the probabilistic test fails, we then add hyperplanes to the polytope. We produce candidate solutions for the closest point in collision program (5) based on the first N_p configurations q in $S_{col}$ . For each q, we use bisection search to step toward the center c of the ellipsoid, while ensuring that q is still in collision. In practice, we bisect a fixed number N_b of times and pick the configuration q^⋆ that is closest to c and still in collision. This yields our updated batch $S^{⋆}$ . This strategy is guaranteed to converge to the boundary of an obstacle for each configuration q, but not necessarily the obstacle containing the original q or the one that is closest to c. Because this optimization procedure is gradient-free and only relies on a collision checker, it is highly parallel and its performance scales with available computational resources. Finally, we order all candidates $q^{⋆} \in S_{col}^{⋆}$ by ellipsoidal metric in ascending order. We then repeatedly select the closest candidate q^⋆ inside the current polytope and add a hyperplane at that configuration tangent to the ellipsoid, until we hit N_f non-redundant hyperplanes or run out of candidates. We remark that this is a computationally cheap way of guaranteeing that the currently placed hyperplane is not redundant, which avoids solving any redundancy checks via linear programming but discards candidates outside of the current polytope, which would produce non-redundant hyperplanes and potentially reduce the number of iterations required.

5.3. The IRIS-NP2 algorithm

The IRIS-NP2 algorithm (Alg. 3) places hyperplanes more carefully than IRIS-ZO, increasing runtime in favor of larger regions with fewer hyperplanes, while still improving upon IRIS-NP runtimes. Like IRIS-NP, IRIS-NP2 uses nonlinear programming, solving (5) to find separating hyperplanes. In contrast to IRIS-NP, IRIS-NP2 always initializes NLP searches with feasible initial guesses q₀, prioritizing initial guesses with lower ellipsoid metric $‖ q_{0} - c ‖_{E}^{2}$ . These initial guesses, along with the corresponding pair of bodies in collision, $(A, B)$ such that $A (q) \cap B (q) \neq \emptyset$ , are obtained via a subroutine GetConfigInCollision, two options for which are outlined at the end of this section.

This strategy of always using a feasible initial guess presents a major advantage over IRIS-NP, as IRIS-NP spends substantial time in NLP solves that return infeasible. In part, this property of IRIS-NP is due to initializing NLP searches with uniform samples q₀, in the hopes that the nonlinear program solver can pull those q₀ not in collision into collision to return feasible optimized solutions q^⋆. In practice, this frequently fails even when the polytope does contain collisions. Furthermore, as discussed in Sec. 4.2, IRIS-NP requires that many consecutive NLP solves return infeasible in order to terminate. Assessing termination readiness via the Bernoulli trials is faster, allowing the NLP, which is slow to solve, to be used only to construct hyperplanes. Another advantage of this strategy is it scales better with environment complexity. IRIS-NP must solve a minimum number of NLPs for every valid collision pair. This quantity grows quickly with scene complexity, while many of these pairs never actually collide, resulting in infeasible NLPs and wasted computation time. IRIS-NP2 only considers pairs known to collide, and hence solves only feasible NLPs. We present two options for GetConfigInCollision, with differing costs and benefits: the greedy collision finder, which is hyperparameter-free, and the ray collision finder, which can be tuned for higher performance.

For the Greedy Collision Finder, all samples from the Bernoulli trial that are in collision are aggregated and sorted in ascending order of distances to the center point of the ellipsoid, w.r.t. the ellipsoidal metric. (In effect, N_p is dynamically set to equal the number of collision particles.) When queried, this subroutine returns the sample with the next-lowest ellipsoid metric, until all in-collision samples from the Bernoulli trial have been exhausted. If a sample is not in the polytope (due to new hyperplanes from an earlier sample), it would no longer be a feasible initial guess for Equation (5), so None is returned.

The Ray Collision Finder (Figure 4) prioritizes the quality of the constructed hyperplanes via an inexpensive line search to find configurations in collision near the ellipsoid center. The ray collision finder takes a subset of the samples drawn for the Bernoulli trial, and, for each sample, steps outward in discrete steps along the ray from the ellipsoid center through the sample. The first step in collision is returned. If a step has exited the polytope, None is returned. This discrete search serves a purpose similar to the objective of (5): to limit the hyperplanes needed, we desire to add hyperplanes at the closest (in the ellipsoid metric) points in collision. Because the discrete search is not restricted to a specific collision pair, it may find configuration-space obstacles closer than (5). In contrast to the bisection search performed by IRIS-ZO, this line search sweeps monotonically outward from the ellipsoid center in an attempt to find the closest collision; the IRIS-ZO bisection search steps inward from the sample, finding a point on a boundary of a configuration-space obstacle. (The ray may pass through several such boundaries).

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 (). On the right, the corresponding hyperplanes (green) form the updated polytope.

6. Growing polytopes in parametrized configuration spaces

For collision-free motion planning, constructing convex subsets of $C^{f r e e}$ is tractable because $C^{f r e e}$ itself is positive-measure. This is not the case for systems like mechanical linkages, which physically must satisfy (nonlinear) equality constraints. Even when there are no physical constraints, satisfying such constraints may still be necessary for task completion. For example, consider a bimanual robot moving a rigid object with both hands. In this case, the two grasps define a “virtual linkage” (Williams and Khatib, 1993), and the set of kinematically valid configurations $(C^{v a l i d})$ is a measure-zero subset of the full configuration space $C$ . We refer to this set as the constraint manifold (although it is only truly a manifold if we excise singularities).

To plan along constraint manifolds, sampling-based planners must use specialized techniques to draw samples (Kingston et al., 2018). Trajectory optimizers must use similar techniques (Bordalba et al., 2022) or accept slight constraint violations away from the control points of the trajectory (Bonalli et al., 2019). But GCSTrajOpt, and DBMPs in general, were wholly inapplicable, as any convex subset of $C^{v a l i d}$ would have an empty interior and relative interior.

6.1. Prior methods

Recent work, using analytic inverse kinematics as a parametrization of the constraint manifold, enabled trajectory optimization intrinsic to $C^{v a l i d}$ (Cohn et al., 2024). The key ingredient is a smooth mapping from some other space to $C$ , the image of which is a subset of $C^{v a l i d}$ . This mapping effectively defines (minimal) nonlinear coordinates on the constraint manifold, which are used to represent the trajectories. Costs and constraints can then be imposed both in these nonlinear coordinates as well as in $C$ by precomposing the parametrization. Using this approach guarantees, by construction, that trajectories obey the kinematic constraints up to floating-point error.

Cohn et al. (2024) propose growing convex sets in terms of the nonlinear coordinates in order to use DBMPs restricted to $C^{v a l i d}$ . To this end, they modified IRIS-NP to operate through the parametrization: given a smooth mapping $ϕ : D \to C$ that parametrizes $C^{v a l i d}$ , the algorithm finds hyperplanes to

(1) avoid function domain violations,

(2) avoid joint limit violations, and

(3) avoid non-reachable configurations

(in that order), before finally searching across all possible collision pairs. While this approach enables planning intrinsic to $C^{v a l i d}$ using DBMPs, users were confronted with the same challenges with IRIS-NP as discussed in Sec. 1 and 4.2, which are exacerbated due to the greater problem complexity. Ultimately, the regions had a large number of hyperplanes, and the fraction of the region in collision (or violating a constraint) was difficult to control as a user, even with careful tuning of the number of consecutive solve failures for each constraint and collision pair.

In the following, we show that we can circumvent these challenges by adapting both IRIS-ZO and IRIS-NP2 to operate in parametrized configuration spaces.

6.2. IRIS-ZO and IRIS-NP2 on a manifold

Suppose we have a smooth function $ϕ : D \to C$ , where $\forall \tilde{q} \in D$ , $ϕ (q) \in C^{v a l i d}$ , and $D$ can be defined as some constrained subset of $R^{m}$ ,

D = \{\tilde{q} \in R^{m} | D_{k} (\tilde{q}) \leq 0, k = 1, \dots, K\},

where

D_{k} : R^{m} \to R

. Then we grow regions in

R^{m}

, within a new user-defined domain (which should encompass

D

). The constructed region

P

must be collision-free, in the sense that for each

\tilde{q} \in P

ϕ (\tilde{q})

is collision-free. In addition, it must satisfy the domain constraints, that is,

P \subseteq D

. See Figure 5 for an illustration.

Figure 5.

In order to grow polytopes on a measure-zero subset of $C$ , Cohn et al. (2024) propose both constructing the polytopes and performing the planning using a smooth parametrization ϕ of $C$ . We demonstrate that both IRIS-NP2 as well as IRIS-ZO can be modified work in the parametrized space as well. The red region is an obstacle in configuration space $C$ (right) and the parametrized space $D$ (left). The light blue surface is $C^{v a l i d}$ , the green region is the domain (left) and image (right) of the parametrization ϕ, and the dark blue region is the polytope produced by our algorithm.

The bisection search of IRIS-ZO adapts very naturally to this formulation, except instead of directly checking the sample for a collision, the algorithm checks for any constraint violations before applying the parametrization to check for collisions in the full configuration space. IRIS-NP2 identifies which constraint was violated or which geometries were in collision and then solves the appropriate nonlinear program. Since constraints are imposed in $D$ , we can use the same approach as in equation (6) of Petersen and Tedrake (2023). For collisions, we modify (5) to use the parametrization function:

\begin{align} \underset{\tilde{q}, t}{minimize} & ‖ \tilde{q} - c ‖_{E}^{2} \end{align}

(8a)

\begin{align} subject to & t \in A (ϕ (\tilde{q})) \cap B (ϕ (\tilde{q})), \end{align}

(8b)

\begin{align} \tilde{q} \in P . \end{align}

(8c)

Because $\tilde{q}$ now lives in the parametrized space, the mapping ϕ must be applied to obtain the full robot configuration, before verifying that the pair $A B$ is in collision. Because ϕ is compatible with automatic differentiation, this program can be solved with generic nonlinear optimizers like SNOPT (Gill et al., 2005) and IPOPT (Wächter and Biegler, 2006).

7. Experiments

7.1. Polytope construction benchmark

To evaluate our proposed changes to the IRIS-NP algorithm, we put forth a comprehensive benchmark. This benchmark includes eight different robotic systems, described in Figure 6. For each benchmark, we select 10 seed configurations manually, placing the robot in a variety of configurations in its environment. Each experiment is run for a single outer iteration, to isolate performance differences in the SeparatingPlanes steps.

Figure 6.

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 $C$ . The ratio $vol (C^{f r e e}) / vol (C)$ is estimated with 10⁷ samples uniformly drawn from $C$ .

We compare the approaches for two different settings: a “fast” setting, where we request the region to be 90% collision-free, with 90% confidence, and a “precise” setting, where we request the region to be 99% collision-free with 95% confidence. IRIS-NP cannot take these arguments directly, so we hand-tune the required number of consecutive infeasible solves to roughly match the average collision fractions achieved by our algorithms. For IRIS-NP2, we tuned the algorithm hyperparameters of Table 1 on a per-experiment basis.

To mitigate the effects of randomness of the algorithms in our comparisons, we construct regions around each seed point 10 times. This yields 100 trials per robot environment, or 800 total, for each algorithm. For the parallelized components of the algorithms, an Intel Core i9-10850K (10 cores, 20 threads) is used. All convex programs are solved using MOSEK (MOSEK ApS, 2025) and the nonlinear programs are solved using SNOPT (Gill et al., 2005). The volume of the maximum-volume inscribed ellipsoid is used as a proxy for polytope volume, as actual volume is intractable to compute in higher dimensions. We report these volumes normalized by the averaged IRIS-NP results.

Compared to the conference version of this paper (Werner et al., 2024b), we have rerun these benchmarks using the publicly available implementations of IRIS-ZO and IRIS-NP2 in Drake (Tedrake and the Drake Development Team, 2019).

For each algorithm and robotic system, we include the runtime, number of hyperplanes, volume, and proportion of the region in collision, averaged over all trials on all seed points in Table 2. Region volume and number of hyperplanes are both dependent on the seed configuration—if the robot is in a narrow passage in

C^{f r e e}

, the region may be small, and if it is near non-convex obstacles, many hyperplanes may be required. In Figure 7, we visualize the runtime and hyperplane data across the 10 trials for the 8 corresponding seed configurations shown in Figure 6. Our algorithms show significant improvements relative to IRIS-NP in runtime and number of hyperplanes. In particular, IRIS-ZO gains approximately an order-of-magnitude time advantage with a comparable number of hyperplanes. While IRIS-NP2 is slower than IRIS-ZO, it generally remains faster than IRIS-NP and uses significantly fewer hyperplanes.

Table 2.

Statistics averaged over all 10 seed points. “Rel Volume” corresponds to the volumes of the MVIE, normalized by that of IRIS-NP. For the “Fast” settings, on average, IRIS-ZO was 11.9 times faster than IRIS-NP with 1.4 times fewer hyperplanes. IRIS-NP2 with the greedy strategy was 6.55 times faster with 2.1 times fewer hyperplanes, and with the ray strategy was 4.2 times faster with 2.3 times fewer hyperplanes. For the “Precise” settings, IRIS-ZO was 13.4 times faster than IRIS-NP with around the same number of hyperplanes. IRIS-NP2 with the greedy strategy was 33.9 times faster with 2.4 times fewer hyperplanes, and with the ray strategy was 13.6 times faster with 2.6 times fewer hyperplanes.

	Environment	Time [s]				Num hyperplanes				Rel volume				Frac in collision
	Environment	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray
Fast	Flipper	0.157	0.034	0.032	0.066	22.6	15.8	14.2	13.7	1.000	0.542	1.190	1.083	0.036	0.013	0.012	0.009
	UR3	0.890	0.048	0.068	0.100	41.1	27.4	25.8	23	1.000	0.679	2.112	2.074	0.043	0.031	0.019	0.035
	UR3Wrist	1.132	0.063	0.096	0.163	50.9	33.6	29.8	27.7	1.000	0.348	3.524	3.355	0.025	0.032	0.024	0.036
	IIWAShelf	1.219	0.108	0.349	0.378	75.7	47.9	33.8	30.3	1.000	0.443	2.256	1.994	0.078	0.034	0.031	0.044
	4Shelves	1.226	0.113	0.206	0.315	63.1	47.1	36.5	31.6	1.000	0.280	2.821	3.325	0.108	0.038	0.023	0.036
	IIWABins	1.401	0.094	0.468	0.400	84.2	45.7	32.4	30.7	1.000	0.600	3.759	2.361	0.083	0.034	0.032	0.034
	2IIWAs	18.752	2.039	1.463	1.625	239	237	72.6	63.1	1.000	0.010	75.225	41.233	0.048	0.058	0.059	0.101
	Allegro	2.193	0.276	0.123	0.150	75.4	81.3	41.4	39.6	1.000	0.096	7.179	7.531	0.001	0.062	0.021	0.030
Precise	Flipper	0.988	0.169	0.077	0.144	26	17.5	14.3	14.1	1.000	0.565	1.093	1.119	0.003	0.001	0.001	0.001
	UR3	5.253	0.267	0.134	0.368	52.8	39.8	29.4	28.3	1.000	0.616	1.584	1.848	0.001	0.002	0.001	0.003
	UR3Wrist	4.517	0.312	0.165	0.432	62.4	45.5	33.1	31.6	1.000	0.462	3.565	4.023	0.002	0.003	0.002	0.003
	IIWAShelf	10.800	0.991	0.596	1.178	126	77.3	41.7	39	1.000	0.498	1.849	1.839	0.006	0.003	0.002	0.004
	4Shelves	24.054	0.818	0.392	1.002	86.6	69.6	40.1	35.9	1.000	0.270	2.735	3.398	0.007	0.003	0.002	0.003
	IIWABins	15.278	0.821	0.710	0.971	129	88.3	43.3	38.1	1.000	0.584	2.072	1.995	0.003	0.003	0.003	0.004
	2IIWAs	214.238	27.455	2.642	9.191	396	512	94.8	86	1.000	0.002	15.758	13.382	0.004	0.005	0.003	0.006
	Allegro	2.214	3.703	0.214	0.462	75.4	210	45.1	42.9	1.000	0.011	2.604	3.181	0.001	0.005	0.001	0.003

Figure 7.

The mean and standard deviation of the runtime and hyperplane counts across ten trials per environment for the 8 seed configurations shown in Figure 6 (These plots use a log-scale y-axis. Exact numbers are shown in Table 3).

Table 3.

Statistics specifically for the seed points shown in Figure 6 (Numbers correspond to the plots in Figure 7.).

	Environment	Time [s]				Num hyperplanes
		NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray
Fast	Flipper	0.155 ± 0.042	0.034 ± 0.006	0.058 ± 0.053	0.059 ± 0.015	23.300 ± 1.616	16.300 ± 1.616	15.900 ± 1.513	13.800 ± 0.872
	UR3	0.996 ± 0.123	0.078 ± 0.009	0.095 ± 0.015	0.121 ± 0.037	44.700 ± 4.713	45.500 ± 6.103	32.100 ± 1.814	27.100 ± 1.136
	UR3Wrist	0.793 ± 0.099	0.070 ± 0.012	0.118 ± 0.069	0.096 ± 0.027	46.000 ± 4.837	35.300 ± 4.755	31.500 ± 2.202	24.200 ± 1.327
	IIWAShelf	0.779 ± 0.243	0.074 ± 0.020	0.209 ± 0.131	0.148 ± 0.071	47.300 ± 8.718	34.700 ± 8.626	30.000 ± 4.561	28.600 ± 2.973
	4Shelves	0.948 ± 0.142	0.119 ± 0.012	0.221 ± 0.075	0.404 ± 0.320	57.100 ± 5.629	50.200 ± 4.490	42.700 ± 4.173	36.000 ± 1.949
	IIWABins	1.823 ± 0.492	0.081 ± 0.014	0.411 ± 0.288	0.167 ± 0.068	101.700 ± 26.926	40.900 ± 5.147	32.800 ± 3.763	26.300 ± 1.552
	2IIWAs	18.092 ± 2.487	1.789 ± 0.225	1.276 ± 0.243	1.651 ± 0.228	235.600 ± 33.254	196.300 ± 20.611	72.200 ± 7.440	65.800 ± 7.666
	Allegro	2.168 ± 0.276	0.218 ± 0.028	0.111 ± 0.020	0.103 ± 0.015	61.800 ± 1.778	65.900 ± 7.905	39.200 ± 2.522	37.200 ± 1.077
Precise	Flipper	0.907 ± 0.063	0.167 ± 0.025	0.079 ± 0.009	0.131 ± 0.026	26.700 ± 0.900	17.800 ± 1.887	16.300 ± 1.847	14.100 ± 1.044
	UR3	5.978 ± 0.464	0.565 ± 0.057	0.185 ± 0.022	0.650 ± 0.526	47.200 ± 1.990	69.900 ± 5.009	39.300 ± 1.792	32.200 ± 1.536
	UR3Wrist	3.189 ± 0.423	0.379 ± 0.038	0.198 ± 0.054	0.551 ± 0.329	56.600 ± 2.577	45.100 ± 4.549	31.600 ± 1.625	29.300 ± 2.759
	IIWAShelf	4.304 ± 0.544	0.744 ± 0.253	0.337 ± 0.162	0.664 ± 0.255	69.700 ± 5.900	65.800 ± 18.088	43.600 ± 6.216	34.700 ± 2.648
	4Shelves	18.665 ± 0.683	0.804 ± 0.105	0.285 ± 0.084	0.857 ± 0.274	78.800 ± 3.311	70.600 ± 7.552	45.000 ± 3.950	39.200 ± 2.315
	IIWABins	16.473 ± 1.849	1.259 ± 0.219	0.337 ± 0.245	0.540 ± 0.161	135.600 ± 10.288	131.000 ± 16.310	40.400 ± 4.695	33.000 ± 1.897
	2IIWAs	132.775 ± 25.754	20.288 ± 2.614	2.199 ± 0.317	11.174 ± 6.893	366.200 ± 47.421	355.100 ± 37.289	91.200 ± 7.534	80.300 ± 1.792
	Allegro	2.189 ± 0.279	0.794 ± 0.245	0.212 ± 0.037	0.411 ± 0.189	61.800 ± 1.778	97.000 ± 19.621	44.200 ± 0.980	40.700 ± 1.487

7.2. Downstream motion planning improvements

A key motivation behind this work is to enable and improve motion planning through the computed regions. In this section, we characterize this performance on a three-DOF environment, FLIPPER, pictured in Figure 6, where we automatically position the sets using the VCC algorithm (Werner et al., 2024a). While different motion planning algorithms that utilize IRIS regions are formulated differently, all require solving optimization problems involving the regions. To isolate challenges related to this shared task, we fix a sequence of regions and solve a single convex program to plan a shortest path through it.

A challenge in comparing regions generated for motion planning is that, for all IRIS variants, regions initialized with haphazardly chosen ellipsoids may grow in unpredicted ways; two regions initialized with the same such ellipsoid may cover significantly different parts of $C^{f r e e}$ . To facilitate generating analogous regions across the IRIS variants, we leverage prior work that generates ellipsoids that represent a natural decomposition of $C^{f r e e}$ (Werner et al., 2024a). These ellipsoids are intended to guide region growth to capture certain areas of $C^{f r e e}$ . We hand-select a sequence of four ellipsoids expected to generate regions that will intersect sequentially. We use these ellipsoids to initialize IRIS-NP, IRIS-ZO, and IRIS-NP2 (Greedy and Ray) to each generate a sequence of sequentially intersecting regions. Subsequently, we calculate the shortest path passing through the sequence from the center of the first ellipsoid to the center of the last using Gurobi 10.03 (Gurobi Optimization, LLC, 2024).

Figure 8 shows statistics for this experiment. As previously seen, IRIS-NP is significantly slower than our proposed algorithms. It also yields the largest number of hyperplanes per polytope on average. The IRIS-NP2 algorithms improve the computation time for calculating the shortest path as well as reduce the overall path length. The latter result aligns with our observations in Sec. 7.1: on average, IRIS-NP2 produced polytopes where the MVIE had a larger volume than those produced by IRIS-NP. These results confirm the intended impacts of our proposed algorithms: both IRIS-ZO and IRIS-NP2 dramatically speed up the calculation of free-space regions, while IRIS-NP2 additionally yields improved motion plans, calculated faster.

Figure 8.

Statistics for the motion planning experiment. Top-left: Total time for the IRIS algorithm to compute the polytopes. Bottom-left: Number of hyperplanes per polytope, averaged over the computed polytopes. Top-right: Time for the optimizer to compute the shortest path through the sequence of polytopes, averaged over 100 optimization trials. Bottom-right: Length of the optimized path. All results are averaged over 10 trials of growing the polytopes. The standard deviations over these trials are shown in error bars.

7.3. Accelerating IRIS-ZO using GPUs

The ZeroOrderSeparatingPlanes subroutine (alg. 2) is highly parallelizeable. In this section, we briefly investigate the benefits of using a GPU to perform the sampling and collision-checking within this subroutine. To this end, we build IRIS-ZO off of the existing infrastructure in the CSDECOMP software package Werner et al. (2025), which implements forward kinematics and collision-checking for box and sphere geometries directly in CUDA. In order to compare our CPU implementation of IRIS-ZO, which supports a wider variety of shape primitives such as meshes, capsules, and cylinders, to the GPU implementation, we employ the 2IIWAs environment from the benchmarks, where the collision geometries have been converted to spheres and boxes. The new environment is comprised of 101 collision geometries and has 4125 collision pairs. For each of the ten seed configurations, we grow each polytope ten times while capping the algorithm at a single outer iteration in order to simplify the comparison between the implementations.

For the fast settings, which uses N_p = 100, the GPU implementation takes 0.162 seconds on average for all 100 regions, whereas the CPU version took 1.326 seconds on average. This corresponds to an 8.17-times speedup on average. For the precise settings, which uses N_p = 800, the mean computation times per polytope are 1.114 seconds for the GPU, and 12.415 seconds for the CPU implementation, respectively. This corresponds to an 11.45-times speedup on average.

A further benefit of the GPU implementation is that we can increase the number of particles N_p used for the bisection search significantly, while only marginally increasing the computation time per inner iteration. This has the benefit of decreasing the number of hyperplanes. We demonstrate this by rerunning the above benchmark task with the precise settings for N_p = 50, 500, 5000, 50000. The results are shown in Figure 9. We find that the GPU implementation only begins to slow down noticeably at N_p = 50000 where the time for one inner iteration is around 81 ms on average. The CPU implementation already begins to slow down noticeably at N_p = 500, where the time for one inner iteration is 92 ms, on average. We then, per seed point, compute the number of hyperplanes relative to the average number of hyperplanes computed with N_p = 50 for the GPU implementation. We find that increasing N_p from 50 to 50000 reduces the number of faces of the final polytopes by around 41% on average. For N_p = 50 the regions had an average of 492.2 hyperplanes. For N_p = 50000 the regions had 289.42 hyperplanes on average.

Figure 9.

Comparison of the GPU to the CPU implementation of IRIS-ZO on the 2IIWAs task with sphere and box collision geometries. Left: Comparison of the average computation times between the CPU and GPU implementation for the first inner iteration as we increase the number of particles N_p. Right: Average number of hyperplanes per region relative to the average hyperplane count at N_p = 50. The shaded region indicates the empirical standard deviation.

7.4. Growing polytopes along manifolds

In this section, we investigate how IRIS-ZO and IRIS-NP2 operate in parametrized configuration spaces. We begin with an illustrative example and then provide a more in-depth comparison of the performance to prior work on a constrained bimanual system.

Consider a planar double pendulum with link lengths l₁ and l₂, with a circular collision geometry on the end of the second link of radius r, and two walls at a distance w from the origin. The free space can be written in closed form as

\{(q_{1}, q_{2}) : r - w \leq l_{1} \sin (q_{1}) + l_{2} \sin (q_{1} + q_{2}) \leq w - r\} .

We now grow a collision-free polytope around the origin under the stereographic projection s_i≔ tan q_i/2. Note that this choice of coordinates allows us to rigorously certify that the polytope is entirely collision-free (Amice et al., 2024; Dai et al., 2024). The parametrization therefore reads ϕ(s)≔ arctan(2s), where arctan is applied component-wise. Figure 10 visualizes $C^{f r e e}$ , the resulting polytope, and its MVIE in the original coordinates.

Figure 10.

The polytope generated for the double pendulum by IRIS-NP2 under the rational parametrization. The blue curve is the boundary of $C^{f r e e}$ . The light green curve is the boundary of the collision-free region. Due to the nonlinearity of the transformation, the polytope appears curved when mapped back into the original coordinates. The dark green curve is the maximum-volume inscribed ellipsoid of the computed polytope (also distorted by the parametrization).

To verify our methods’ performance in a more complex scenario, we grow regions with IRIS-ZO and IRIS-NP2 (using both the greedy and ray sampling strategies) for the constrained bimanual setup used in Cohn et al. (2024). We use the same 19 seed points for the fixed grasp regions, and compare to the results from the specialized version of IRIS-NP. The domain is the Cartesian product of the joint limits for the control arm with [−π, π] for the self-motion angle. Besides searching for collisions inside the polytope, we enforce the domain and joint limit constraints presented in (Cohn et al., 2024, §III.E). The results are shown in Table 4. We also grow a polytope without the shelves and show three sample configurations that lie inside of the polytope in Figure 11, demonstrating the large swath of task space a single region can cover.

Table 4.

A comparison of the original IRIS-NP algorithm with the three new approaches for the constrained bimanual configuration space. As in Table 3, the volume is approximated by the maximum-volume inscribed ellipsoid. We estimate the fraction in collision with 10,000 uniform random samples. Across the board, the new algorithms are much faster than IRIS-NP, require fewer hyperplanes to describe the collision-free polytopes, and satisfy the target fraction in collision of $\leq 1 %$ . (The red font highlights polytopes produced by IRIS-NP that failed to achieve this threshold.) In addition, the IRIS-NP2 approaches yield larger volume regions.

Seed	Time [s]				Number of hyperplanes				Relative volume				Fraction in violation [%]
Point	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray	NP	ZO	Greedy	Ray
1	273.80	1.55	1.02	1.89	369	69	53	53	1	2.19 × 10⁻³	4.23 × 10²	6.81 × 10 ²	5.87	0.00	0.01	0.29
2	360.70	1.30	1.03	1.49	416	62	54	41	1	4.36 × 10⁻⁴	3.04 × 10²	5.03 × 10 ²	2.14	0.13	0.02	0.22
3	273.87	2.98	1.35	3.00	179	129	54	52	1	4.62	5.54 × 10³	5.81 × 10 ³	0.00	0.29	0.09	0.30
4	296.34	3.79	4.01	6.26	302	153	63	56	1	1.64 × 10⁻¹	1.83 × 10¹	3.59 × 10 ¹	0.00	0.11	0.09	0.42
5	280.35	3.14	1.47	2.62	370	132	70	60	1	2.29 × 10⁻¹	1.05 × 10¹	1.16 × 10 ¹	0.08	0.23	0.03	0.23
6	375.20	1.17	1.34	1.34	481	53	51	36	1	6.49 × 10⁻³	1.91 × 10¹	2.16 × 10 ²	6.31	0.01	0.16	0.55
7	233.16	1.50	2.17	1.75	185	90	57	46	1	1.60 × 10⁻¹	2.42 × 10 ²	2.42 × 10²	0.00	0.11	0.04	0.46
8	428.44	1.97	1.61	3.38	234	102	56	50	1	5.71	3.42 × 10³	3.93 × 10 ³	0.00	0.17	0.06	0.06
9	390.54	2.84	1.32	1.40	216	130	50	49	1	2.18	2.25 × 10 ³	1.73 × 10³	0.00	0.32	0.06	0.32
10	353.06	1.29	1.79	2.47	579	61	62	47	1	0.99 × 10⁻²	1.08 × 10¹	1.26 × 10 ¹	3.09	0.03	0.06	0.39
11	300.84	0.74	1.65	1.89	436	38	54	39	1	6.72 × 10⁻³	3.96 × 10²	4.61 × 10 ²	3.59	0.14	0.03	0.31
12	295.49	1.22	1.33	1.81	396	66	53	46	1	5.03 × 10⁻³	2.48 × 10 ²	1.89 × 10²	3.79	0.09	0.03	0.09
13	433.61	1.18	1.27	1.73	420	51	57	50	1	5.66 × 10⁻³	2.82 × 10 ¹	2.30 × 10¹	2.49	0.04	0.04	0.12
14	254.72	1.34	2.03	3.71	454	72	56	53	1	9.21 × 10⁻²	2.62 × 10¹	4.30 × 10 ¹	0.35	0.15	0.15	0.47
15	243.63	1.52	1.06	2.06	346	64	51	44	1	1.04 × 10⁻¹	2.97 × 10⁴	4.47 × 10 ⁴	0.45	0.04	0.02	0.24
16	290.49	1.41	1.71	1.44	226	72	58	42	1	9.21 × 10⁻¹	2.41 × 10³	4.66 × 10 ³	0.00	0.16	0.05	0.15
17	248.97	2.14	1.98	2.63	252	107	61	59	1	1.05	2.96 × 10²	3.69 × 10 ²	0.00	0.21	0.04	0.15
18	263.98	2.22	1.52	2.31	299	109	63	55	1	1.08	4.45 × 10²	2.92 × 10 ³	0.10	0.20	0.06	0.47
19	216.20	1.35	1.72	1.60	383	66	52	50	1	4.01 × 10⁻²	7.84 × 10¹	9.09 × 10 ¹	1.04	0.07	0.07	0.24
Min	216.20	0.74	1.02	1.34	179	38	50	36	1	3.11 × 10⁻³	3.54 × 10²	3.99 × 10 ²	0.00	0.00	0.01	0.06
Mean	305.97	1.82	1.65	2.36	344	85	56	48	1	2.30 × 10⁻¹	0.99 × 10²	1.22 × 10 ²	1.54	0.13	0.06	0.29
Max	433.61	3.79	4.01	6.26	579	153	70	60	1	1.64 × 10⁻¹	3.18 × 10¹	3.59 × 10 ¹	6.31	0.32	0.16	0.55

Figure 11.

When the relative transform between the end-effectors of two robot arms is fixed, the set of valid configurations is a measure-zero set, preventing the direct use of our polytope inflation algorithms. Instead, we can use the parametrization machinery to grow polytopes along this constraint manifold. Here, we visualize three collision-free bimanual configurations from a single convex set, demonstrating that a single region can cover a large portion of task space.

Compared to the specialized version of IRIS-NP presented in Cohn et al. (2024), our new algorithms are significantly more performant. IRIS-ZO, IRIS-NP2 (greedy), and IRIS-NP2 (ray) consistently achieve more than a 100x speedup over IRIS-NP. The fastest algorithm was either IRIS-ZO or IRIS-NP2 (greedy), with about a 50/50 split. The new algorithms produced polytopes that required around 2 to 10 times fewer hyperplanes than IRIS-NP, with IRIS-NP2 (ray) almost always requiring the fewest. IRIS-NP2 (ray) usually produces the largest regions, although IRIS-NP2 (greedy) sometimes gains a slight volume advantage. IRIS-ZO sometimes produces smaller regions than IRIS-NP, although this may be correlated with IRIS-NP regions that had large fractions in collision. IRIS-NP frequently produced regions with a large fraction in collision or in violation of constraints—8 out of 19 regions exceeded the target of 99% valid. In contrast, all the new algorithms cleared this threshold.

8. Discussion

In recent years, region generation has posed a major roadblock for the adoption of new decomposition-based motion planners such as GCSTrajOpt. Toward alleviating this roadblock, we have presented significant improvements to the IRIS-NP algorithm, reducing its runtime, making it more user-friendly, and increasing the quality of the resulting regions. We derive a probabilistic test for the proportion of a given region that is collision-free, allowing the user to more directly trade off algorithmic precision and speed. We have also presented new approaches to the SeparatingPlanes subroutine, which show significantly better performance than IRIS-NP, while consistently achieving the user-specified collision-free threshold. IRIS-NP2 is generally faster and requires fewer hyperplanes to define the polytopes. IRIS-ZO is easy to implement, is around 12.7 times faster, and often uses fewer hyperplanes than IRIS-NP, although the regions are smaller. Our preliminary investigations show that we can further speed up IRIS-ZO by around a factor of 10 if we run it on a GPU. Additionally, we have confirmed that IRIS-NP2 improves downstream motion planning by reducing planning time and path cost. When sets are constructed offline for real-time motion planning, these improvements can compound with the benefits of calculating simplified approximations of the regions downstream (Jiang et al., 2025) to enable fast motion planning. Furthermore, these new approaches scale more favorably with the complexity of the environment. As they are deployed in environments with increasingly many collision geometries, we anticipate the performance gap will grow even further. Besides the quantitative improvements, the hyperparameters are straightforward to tune and have clear effects on the behavior of the algorithm. We have also extended both algorithms to operate in parametrized configuration spaces. Our experiments on a constrained bimanual system show that our new algorithms provide dramatic speedups of around 100 times over prior work when computing polytopes on this constraint manifold.

There are myriad directions for future research. CPU-level multithreading, SIMD instructions (Tan et al., 2019) and GPU programming (Lauterbach et al., 2009) have shown great promise for speeding up collision-checking beyond our preliminary investigations. In addition, we would like to extend the support of our GPU code to additional nonlinear constraints and parametrized configuration spaces. Furthermore, the speed at which useful regions can now be constructed opens new avenues for research in combining DBMPs and SBMPs (Werner et al., 2025).

Finally, obstacles of small or zero measure may still pose a collision risk. But such obstacles would be hard for our algorithms to identify, due to our use of (relative) volume in collision as a target metric. This can be mitigated by initializing the IRIS-NP2 counterexample search program with samples that are nearly in-collision, or just inflating the obstacles. Alternatively, the certifiable IRIS algorithm (Amice et al., 2024; Dai et al., 2024) will avoid all collisions, but its use of polynomial optimization techniques requires significant runtime and compute resources. Thus, the fast construction of regions that can satisfy stronger safety notions, such as geometric clearance, remains an open question.

Footnotes

Acknowledgments

We thank Ravi Gondhalekar (The Charles Stark Draper Laboratory, Inc.) and Alexandre Amice for their feedback. This work was supported by the Toyota Research Institute, , PO No. 2D-06310236, the MIT Quest for Intelligence, and the National Science Foundation Graduate Research Fellowship Program under Grant No. 2141064, as well as the Office of Naval Research grant N00014-23-1-2354. Any opinions, findings, conclusions, or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the National Science Foundation (or other funding organizations).

ORCID iD

Peter Werner

Funding

The authors disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the Toyota Research Institute, , PO No. 2D-06310236, the MIT Quest for Intelligence, and the National Science Foundation Graduate Research Fellowship Program under Grant No. 2141064, as well as the Office of Naval Research grant N00014-23-1-2354.

Declaration of conflicting interests

The authors declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Notes

References

Alon

Spencer

(2016) The Probabilistic Method. John Wiley & Sons.

Amice

Werner

Tedrake

(2024) Certifying bimanual rrt motion plans in a second. In: International Conference on Robotics and Automation (ICRA). IEEE, pp. 9293–9299.

Bajaj

Kim

(1990) Generation of configuration space obstacles: moving algebraic surfaces. The International Journal of Robotics Research 9(1): 92–112. https://doi.org/10.1177/027836499000900104

Bonalli

Cauligi

Bylard

, et al. (2019) Trajectory optimization on manifolds: a theoretically-guaranteed embedded sequential convex programming approach. In: Robotics: Science and Systems. Germany: FreiburgimBreisgau. https://doi.org/10.15607/RSS.2019.XV.078

Bordalba

Schoels

Ros

, et al. (2022) Direct collocation methods for trajectory optimization in constrained robotic systems. IEEE Transactions on Robotics 39(1): 183–202. https://doi.org/10.1109/tro.2022.3193776

Boyd

Vandenberghe

(2004) Convex Optimization. Cambridge University Press.

Chen

Liu

Shen

(2016) Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments. In: Interntional Conference on Robotics and Automation (ICRA). IEEE, pp. 1476–1483.

Chew Chia

Jiang

Graesdal

, et al. (2024) Gcs*: forward heuristic search on implicit graphs of convex sets. In: To Appear in Proceedings of the 16th International Workshop on the Algorithmic Foundations of Robotics (WAFR). URL. https://arxiv.org/abs/2407.08848

Cohn

Shaw

Simchowitz

, et al. (2024) Constrained bimanual planning with analytic inverse kinematics. In: International Conference on Robotics and Automation (ICRA). IEEE, pp. 6935–6942.

10.

Dai

Amice

Werner

, et al. (2024) Certified polyhedral decompositions of collision-free configuration space. The International Journal of Robotics Research 43(9): 1322–1341. https://doi.org/10.1177/02783649231201437

11.

Deits

Tedrake

(2015) Computing large convex regions of obstacle-free space through semidefinite programming. In: Algorithmic Foundations of Robotics XI. Springer, pp. 109–124.

12.

Gill

Murray

Saunders

(2005) SNOPT: an SQP algorithm for large-scale constrained optimization. SIAM Review 47(1): 99–131. https://doi.org/10.1137/s0036144504446096

13.

Gurobi Optimization, LLC (2024) Gurobi optimizer reference manual. URL. https://www.gurobi.com

14.

Jaitly

Farzan

(2024) PAAMP: polytopic action-set and motion planning for long horizon dynamic motion planning via mixed integer linear programming. In: International Conference on Intelligent Robots and Systems. IEEE/RSJ, pp. 7617–7624.

15.

Jaulin

(2001) Path planning using intervals and graphs. Reliable Computing 7(1): 1–15. https://doi.org/10.1023/a:1011400431065

16.

Jiang

Gondhalekar

Tedrake

(2025) Polytope inner approximations with fewer faces subject to minimum-volume, point-inclusion, and set-intersection constraints. In: Conference on Decision and Control. IEEE.

17.

Kingston

Moll

Kavraki

(2018) Sampling-based methods for motion planning with constraints. Annual review of control, robotics, and autonomous systems 1(1): 159–185. https://doi.org/10.1146/annurev-control-060117-105226

18.

Latombe

(2012) Robot Motion Planning, Volume 124. Springer Science & Business Media.

19.

Lauterbach

Garland

Sengupta

, et al. (2009) Fast BVH construction on GPUs. In: Computer Graphics Forum, Volume 28. Wiley Online Library, pp. 375–384.

20.

LaValle

(2006) Planning Algorithms. Cambridge University Press.

21.

Liu

Watterson

Mohta

, et al. (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters 2(3): 1688–1695. https://doi.org/10.1109/lra.2017.2663526

22.

Lovász

(1999) Hit-and-run mixes fast. Mathematical Programming 86(3): 443–461. https://doi.org/10.1007/s101070050099

23.

Lovász

Vempala

(2004) Hit-and-run from a corner. Proceedings of the thirty-sixth Annual ACM Symposium on Theory of Computing. Association for Computing Machinery, 310–314.

24.

Lozano-Pérez

(1990) Spatial Planning: A Configuration Space Approach. Springer. DOI: 10.1007/978-1-4613-8997-2_20.

25.

Mamou

Ghorbel

(2009) A simple and efficient approach for 3D mesh approximate convex decomposition International Conference on Image Processing. IEEE, pp. 3501–3504.

26.

Marcucci

Petersen

von Wrangel

, et al. (2023) Motion planning around obstacles with convex optimization. Science Robotics 8(84): 7843. https://doi.org/10.1126/scirobotics.adf7843

27.

Marcucci

Nobel

Tedrake

, et al. (2024) Fast path planning through large collections of safe boxes. IEEE Transactions on Robotics.

28.

Marcucci

Halm

Yang

, et al. (2025) A biconvex method for minimum-time motion planning through sequences of convex sets. In: Accepted for Publication in Robotics: Science and Systems Foundation.

29.

Morozov

Marcucci

Amice

, et al. (2024) Multi-query shortest-path problem in graphs of convex sets. arXiv preprint arXiv:2409.19543.

30.

Morozov

Marcucci

Graesdal

, et al. (2025) Mixed Discrete and Continuous Planning Using Shortest Walks in Graphs of Convex Sets. arXiv preprint arXiv:2507.10878 .

31.

MOSEK

(2025) Introducing the MOSEK optimization suite 10.2.16. URL: https://docs.mosek.com/10.2/intro/index.html

32.

Natarajan

Liu

Choset

, et al. (2024) Implicit graph search for planning on graphs of convex sets. arXiv preprint arXiv:2410.08909 .

33.

Open Source Robotics Foundation (2024) Sdf specification. URL. https://sdformat.org/. Accessed: 2024-06-09.

34.

Pan

Manocha

(2015) Efficient configuration space construction and optimization for motion planning. Engineering 1(1): 046–057. https://doi.org/10.15302/j-eng-2015009

35.

Petersen

Tedrake

(2023) Growing convex collision-free regions in configuration space using nonlinear programming. arXiv preprint arXiv:2303.14737 .

36.

ROSorg (2024) URDF - unified robot description format. URL. https://wiki.ros.org/urdf. Accessed: 2024-06-09.

37.

Sarmientoy

Murrieta-Cidz

Hutchinsony

(2005) A sample-based convex cover for rapidly finding an object in a 3-d environment International Conference on Robotics and Automation (ICRA). IEEE, pp. 3486–3491.

38.

Tan

Weller

Zachmann

(2019) SIMDop: SIMD optimized bounding volume hierarchies for collision detection International Conference on Intelligent Robots and Systems. IEEE/RSJ, pp. 7256–7263.

39.

Tedrake

(2023) Robotic manipulation. URL. https://manipulation.mit.edu/

40.

Tedrake

the Drake Development Team (2019) Drake: Model-based design and verification for robotics. URL. https://drake.mit.edu

41.

Wächter

Biegler

(2006) On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical Programming 106(1): 25–57. https://doi.org/10.1007/s10107-004-0559-y

42.

Werner

Amice

Marcucci

, et al. (2024a) Approximating robot configuration spaces with few convex sets using clique covers of visibility graphs. In: International Conference on Robotics and Automation (ICRA). IEEE, pp. 10359–10365.

43.

Werner

Cohn

Jiang

, et al. (2024b) Faster algorithms for growing collision-free convex polytopes in robot configuration space. In: International Symposium of Robotics Research. IFRR.

44.

Werner

Cheng

Stewart

, et al. (2025) Superfast configuration-space convex set computation on gpus for online motion planning. In: Robotics: Science and Systems.

45.

Williams

Khatib

(1993) The virtual linkage: a model for internal forces in multi-grasp manipulation International Conference on Robotics and Automation (ICRA). IEEE, pp. 1025–1030.

46.

Spasojevic

Chaudhari

, et al. (2024) Towards optimizing a convex cover of collision-free space for trajectory generation. Robotics and Automation Letters 10(5): 4762–4769. https://doi.org/10.1109/LRA.2025.3553416

47.

Yang

LaValle

(2004) The sampling-based neighborhood graph: an approach to computing and executing feedback motion strategies. Transactions on Robotics and Automation 20(3): 419–432. https://doi.org/10.1109/TRA.2004.824640