Literature
Display
← All papers

Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning

Peter Werner · Richard Cheng · Tom Stewart · Russ Tedrake · Daniela Rus

Preprint posted · arXiv (preprint) ↗

Robotics12 min text · CondensedOriginal paper ↗
Contents

Overview

The complete setup figure retains the physical arm and depth cameras, the perceived obstacle model, and a two-dimensional slice through the seven-dimensional configuration space with the constructed regions.

Original PDF illustration S0.F1
Fig. 1: On the left, our hardware setup with a KUKA LBR iiwa 7 R800 robotic manipulator and three Intel RealSense D415 depth cameras for perceiving obstacles is shown. In the center, the simulated system with the perceived obstacles is shown. The target end effector pose is indicated by the yellow gripper and the collision geometries of the system are shown in red. The perceived obstacles are approximated by a union of spheres shown in the center of the table. The figure on the right shows a two-dimensional slice of the seven-dimensional configuration space, along with a slice of the safe sets (blue and green), and a slice of the configuration obstacles (self-collisions in black, collisions with perceived obstacles in red). The slice lies tangent to the trajectory such that the u-direction is co-linear with the velocity and the v-direction is a random orthogonal vector. The current configuration is indicated by the black dot.

At a glance

EI-ZO inflates collision-free path segments into convex configuration-space regions using GPU-parallel sampling, collision checks and boundary searches. A dynamic roadmap first supplies a path; convex-set trajectory optimization then improves it, with collision checks and region repairs when needed. In two- and seven-dimensional simulation benchmarks, the sequence-based approaches improve conditional collision-free planning reliability and average speed relative to nonlinear trajectory optimization, at a trajectory-cost penalty. Fifteen hardware plans average 823 ms end to end. The regions are only probabilistically collision-free, the reported simulation results are conditioned on roadmap success, and obstacles are assumed stationary during each plan and execution.

I Introduction

Nonlinear trajectory optimization accommodates rich objectives and constraints but can fail from poor initialization. Sampling-based methods find routes but may produce inefficient motion. Decomposition-based motion planners move collision avoidance into a collection of convex free-space regions, leaving simpler trajectory constraints; constructing and positioning those regions has been the bottleneck in changing scenes.

This paper builds on IRIS-ZO [21] and inflates line segments rather than isolated points. Each region contains its seed segment, so regions along a valid polygonal path form a connected corridor. Constructing only this corridor avoids approximating the entire free space. Dynamic roadmaps supply the initial paths, GPU acceleration makes region construction fast, and a final repair loop handles residual collisions. The speed and reliability evidence concerns the tested pipeline, with precomputation and a working collision model.

II Assumptions

The robot model must provide kinematics and collision geometry, such as a URDF [23] or SDF [24]. Perceived obstacle geometry must support conservative checks: the collision checker is assumed never to label an actually colliding configuration safe. Added obstacles must remain stationary during the brief planning and execution interval. “Changing environments” therefore means updates between plans; the experiments do not validate avoidance of obstacles moving during execution.

III Background

The method combines probabilistic convex-region construction with path-guided placement.

III-A Constructing Safe Sets in Robot Configuration Space

IRIS variants iteratively separate sampled or optimized collision points from a candidate polytope. EI-ZO extends the zero-order variant because collision queries and simple updates parallelize well. Its guarantee bounds the probability that more than an ε fraction of the region’s volume is in collision by δ. This is a volume statement about the returned region, not a bound on collision probability of an arbitrary optimized trajectory. Exact polynomial certificates [19] solve a stronger problem at greater cost.

Equation 1
𝐏𝐫[λ⁡(𝒫∖𝒞free)λ⁡(𝒫)>ε]≤δ,\displaystyle\mathop{\bf Pr}\left[\frac{\lambda(\mathcal{P}\setminus\mathcal{C}^{\mathrm{free}})}{\lambda(\mathcal{P})}>\varepsilon\right]\leq\delta, (1)

III-B Positioning Safe Sets for Effective Motion Planning

A global convex cover may need too many sets for online recomputation. A path corridor constructs only regions relevant to one route, as in earlier safe-flight corridors [12][13]. EI-ZO replaces an ellipsoidal distance metric with distance to a segment, allowing direct segment containment without ellipsoid optimization. This placement is efficient but inherits the route quality and connectivity available from the seed path.

IV Rapidly Inflating Line Segments

EI-ZO constructs full-dimensional polytopes around seed segments using only collision checks, projections and separating hyperplanes.

Original PDF illustration S4.F2
Fig. 2: A single iteration of adding hyperplanes to the polytope in the EI-ZO algorithm. The configuration obstacles are shaded in grey and the line segment seeding EI-ZO is shown in black. Starting from the left, first, a batch of configurations is sampled in the current polytope given by the black rectangle. These samples are checked for collisions. The colliding configurations, shown in red, are then used to seed a bisection search to find collisions that minimize the distance to the line segment. The resulting candidates, indicated by the red stars (center frame), are ranked by their distance to the line segment in ascending order, and used to position the tangent planes (final frame) that separate the collisions from the line segment.

IV-A Preliminaries

Distance to a convex set is convex, so its sublevel sets are convex. At a colliding point outside a safe segment, the gradient of distance to that segment defines a supporting plane that separates the point while retaining the segment. Projection and gradient have closed-form expressions, including the degenerate case of coincident endpoints.

Starting from a large domain, the algorithm repeatedly removes collision points with such planes. Searching for collisions close to the segment tends to eliminate more distant collisions with fewer faces. A small step back from each collision permits finite approximations of concave obstacle boundaries; the practical step is reduced when necessary to avoid removing either seed endpoint. Infinite exact separation is replaced by a finite statistical stopping test.

Equations 2–8b
𝐝𝐢𝐬𝐭𝒜​(x)=minz∈𝒜⁡‖x−z‖2\displaystyle{\bf dist}_{\mathcal{A}}(x)=\min_{z\in\mathcal{A}}||x-z||_{2} (2)
ℬℒ​(t)={x|𝐝𝐢𝐬𝐭ℒ​(x)≤t}\displaystyle\mathcal{B}_{\mathcal{L}}(t)=\{x|{\bf dist}_{\mathcal{L}}(x)\leq t\} (3)
a=∇(distℒ)|c,b=aT​c,\displaystyle a=\nabla(\text{dist}_{\mathcal{L}})|_{c},\penalty\ b=a^{T}c, (4a)
𝒯={x|aT​x−b=0},\displaystyle\mathcal{T}=\{x|a^{T}x-b=0\}, (4b)
∇(distℒ)|c=c−cproj‖c−cproj‖2,\displaystyle\nabla(\text{dist}_{\mathcal{L}})|_{c}=\frac{c-c_{\text{proj}}}{||c-c_{\text{proj}}||_{2}}, (5)
cproj={v1if ​v1=v2,(1−α)​v1+α​v2,otherwise.\displaystyle c_{\text{proj}}=\begin{cases}v_{1}&\text{if }\begin{aligned} v_{1}&=v_{2},\end{aligned}\\ \left(1-\alpha\right)v_{1}+\alpha v_{2},&\text{otherwise.}\end{cases} (6)
α=min​(max​((c−v1)T​(v2−v1)‖(v2−v1)‖22,0),1).\alpha=\text{min}\left(\text{max}\left(\tfrac{(c-v_{1})^{T}(v_{2}-v_{1})}{||(v_{2}-v_{1})||_{2}^{2}},0\right),1\right). (7)
minimizex\displaystyle\mathop{\textbf{minimize}}_{x}  distℒ​(x),\displaystyle\;\;\text{ dist}_{\mathcal{L}}(x), (8a)
subject to\displaystyle\mathop{\textbf{subject to}} x∉𝒞free,x∈𝒫,\displaystyle\;\;x\notin\mathcal{C}^{\mathrm{free}},\;\;x\in\mathcal{P}, (8b)

IV-B The EI-ZO Algorithm

The initial region is the domain, typically the joint-limit box. Each iteration draws a hit-and-run batch of size max(Np,M), where M is determined by ε, threshold τ and iteration confidence δk. The schedule δk = 6δ/(π²k²) allocates total uncertainty across iterations. If at most M(1−τ)ε of the first M samples collide, the unadaptive test accepts the region.

Otherwise, colliding samples are projected toward the segment and refined by a fixed number of bisection steps, retaining points on the colliding side. Candidates are sorted by distance to the segment; up to Nf separating planes are added, skipping candidates already removed by earlier planes. Sampling and refinement repeat until acceptance. The exact original pseudocode is retained; its sample call shows M, while the prose specifies max(Np,M). The guarantee presumes the required sampling/test assumptions and a correct collision checker.

Algorithm 1 EI-ZO
Input: Domain 𝒟⊆𝒞\mathcal{D}\subseteq\mathcal{C}, collision-free line segment ℒ\mathcal{L} with ℒ⊂𝒟\mathcal{L}\subset\mathcal{D}, test parameters (δ,ε,τ)(\delta,\varepsilon,\tau), maximum step back Δmax\Delta_{\text{max}}, and optimizer parameters (Np,Nf,Nb)(N_{p},N_{f},N_{b}).
Output: Polytope 𝒫⊆𝒟\mathcal{P}\subseteq\mathcal{D} satisfying (1) for (ε,δ)(\varepsilon,\delta) with ℒ⊆𝒫\mathcal{L}\subseteq\mathcal{P}.
Algorithm: k←1k\leftarrow 1, 𝒫←𝒟\mathcal{P}\leftarrow\mathcal{D}
while True do
   𝒮∼UniformSample​(𝒫,M)\mathcal{S}\sim\textsc{UniformSample}(\mathcal{P},M)
   𝒮col←PointsInCollision​(𝒮)\mathcal{S}_{\text{col}}\leftarrow\textsc{PointsInCollision}(\mathcal{S})
   If UnadaptiveTest​(|𝒮colM|,δk,ε,τ)\textsc{UnadaptiveTest}(|\mathcal{S}_{\text{col}}^{M}|,\delta_{k},\varepsilon,\tau) returns accept then b​r​e​a​kbreak.
   𝒮colproj←ProjectPoints​(𝒮col,ℒ)\mathcal{S}_{\text{col}}^{\text{proj}}\leftarrow\textsc{ProjectPoints}(\mathcal{S}_{\text{col}},\mathcal{L})
   𝒮col⋆←UpdatePointsViaBisection​(𝒮col,𝒮colproj)\mathcal{S}_{\text{col}}^{\star}\leftarrow\textsc{UpdatePointsViaBisection}(\mathcal{S}_{\text{col}},\mathcal{S}_{\text{col}}^{\text{proj}})
   𝒫←OrderAndPlaceHyperplanes​(𝒫,𝒮colproj,𝒮col⋆,Δ)\mathcal{P}\leftarrow\textsc{OrderAndPlaceHyperplanes}(\mathcal{P},\mathcal{S}_{\text{col}}^{\text{proj}},\mathcal{S}_{\text{col}}^{\star},\Delta)
   k←k+1k\leftarrow k+1
end while
return 𝒫\mathcal{P}
Equations 9–10b
δk=6​δπ2​k2.\displaystyle\delta_{k}=\frac{6\delta}{\pi^{2}k^{2}}. (9)
ai=∇(distℒ)|ci,bi=aiT​ci,\displaystyle a_{i}=\nabla(\text{dist}_{\mathcal{L}})|_{c_{i}},\penalty\ b_{i}=a_{i}^{T}c_{i}, (10a)
ℋi={x|aiT​x≤bi−Δ},\displaystyle\mathcal{H}_{i}=\{x|a_{i}^{T}x\leq b_{i}-\Delta\}, (10b)

IV-C Practical Algorithmic Considerations

An optional iteration cap bounds construction time but explicitly voids the statistical guarantee if the test has not passed. Since the implementation checks seed paths by discretization, they can contain missed collisions. A candidate within collision tolerance of the segment triggers failure; checking its projection early can fail faster.

The step back is reduced whenever the maximum step would exclude a segment endpoint, preserving seed containment at the cost of potentially more faces. A rule of thumb selects the fixed bisection count as log₂(L/Δmax), where L estimates the largest boundary-search distance. Fewer steps can increase runtime by requiring more rounds of cuts; the method does not replace sampled path validation with a continuous collision certificate.

Equations 11–13
r=𝐦𝐚𝐱⁡{aiT​v1,aiT​v2}−bi+Δmax,\displaystyle r={\bf max}\left\{a_{i}^{T}v_{1},a_{i}^{T}v_{2}\right\}-b_{i}+\Delta_{\text{max}}, (11)
Δ={Δmax−r,if​r>0,Δmax,otherwise.\displaystyle\Delta=\begin{cases}\Delta_{\text{max}}-r,\penalty\ \text{if}\penalty\ r>0,\\ \Delta_{\text{max}},\penalty\ \text{otherwise}.\end{cases} (12)
Nb≈log2⁡(LΔmax),\displaystyle N_{b}\approx\log_{2}\left(\frac{L}{\Delta_{\text{max}}}\right), (13)

IV-D CUDA Implementation of EI-ZO

CSDecomp implements the method in C++/CUDA with Python bindings. GPU threads parallelize hit-and-run walks, forward kinematics, pairwise collision checks, projections and bisection. Candidate sorting and nonredundant-plane placement use one thread. Samples remain in GPU memory; only the seed segment and final polytope cross the CPU/GPU boundary. Current collision geometry supports boxes and spheres, and finite mixing steps approximate uniform sampling.

V Generating Collision-Free Polygonal Paths using Dynamic Roadmaps

Dynamic roadmaps [35] combine a precomputed probabilistic roadmap [36] with workspace-voxel lookup tables for quickly invalidating configurations after perception updates. Following GPU-accelerated roadmap work [38], this implementation stores node collisions and checks edges lazily online. Precomputation shifts substantial work out of online timing.

Original PDF illustration S5.F3
Fig. 3: A DRM is a PRM with additional lookup tables that allow the PRM to rapidly be updated provided task space observations of obstacles. To this end, the task space is discretized into voxels. We associate a circumscribing sphere with each voxel as collision geometry for collision checking. A portion of these spheres are shown in purple in the right figure. Next, a lookup table is constructed, ℳc\mathcal{M}_{c}, that maps each of these spheres to nodes in the road map that would collide with the sphere. Above, a single sphere is shown in yellow on the right, along with three configurations, shown in red, green, and blue, that collide with it and are stored in ℳc\mathcal{M}_{c}. On the left, a cartoon of the configuration space is shown with the underlying PRM and configuration obstacles. The three colliding configurations are highlighted again in red, blue, and green. The configuration obstacle corresponding to the yellow sphere is highlighted in yellow.

V-A Offline Roadmap Construction

Four maps store node configurations, adjacency, voxel-to-colliding-node associations and end-effector poses. Occupancy changes prune nodes using the lookup map. The offline construction is tied to the robot, geometry and workspace discretization; it is not rebuilt from scratch for every plan.

V-B Online Planning given Perception

Online planning activates voxels from the point cloud and collects their invalidated nodes. Nearby collision-free roadmap poses warm-start inverse kinematics for the goal. Start and goal configurations connect to the remaining graph; A* [41], lazy edge checks and greedy shortcutting produce a polygonal path. Circumscribed voxel spheres provide the obstacle geometry. Occlusions or modeling errors still matter because conservative perception is an assumption.

VI The Motion Planning Pipeline

The pipeline solves goal IK, searches the roadmap, inflates the path and optimizes inside the resulting regions. Candidate trajectories are discretely collision-checked; detected collisions trigger region cuts and reoptimization. It can also inflate multiple alternative routes for graph-based convex planners.

Original PDF illustration S6.F4
Fig. 4: This figure depicts our motion planning pipeline. Our motion planning pipeline is split into an offline phase in which a DRM is constructed, and an online phase where we use the DRM to generate a collision-free PWL paath, inflate the path, and use a DBMP to optimize a trajectory. Starting from the left, in the first figure the DRM is constructed. In the second, we observe a new obstacle (red blob), and build the collision set (red dot). Using the updated DRM, we then solve the IK problem for the goal (yellow star), and find a collision-free path connecting the start (blue dot) to the goal. We inflate this path to an SCS using EI-ZO in the third figure. In the fourth figure, we solve the motion planning problem using DBMPs and check the trajectory for collisions, finding one inside of the green set, indicated by the red arrow. In the fifth figure, we use the recovery mechanism from §VI-B to modify the green set and produce a collision-free path. In general, these last two steps need to be repeated multiple times.

VI-A Inflating a Single Path

Segments are considered in order. A segment is inflated only if no existing region already contains it. Consecutive seed segments share endpoints, and containment therefore preserves a connected route from start to goal, provided the original path is truly collision-free.

VI-B Rapidly Recovering from Detected Collisions

Residual collision points on an optimized trajectory warm-start the same projection/bisection/cutting procedure. After modifying regions, the planner rechecks coverage of every original path segment: a cut preserves its own seed but can remove other segments previously covered incidentally. Uncovered segments receive new regions. Optimization and checking repeat until no sampled collisions remain. The loop’s empirical success does not furnish a continuous-time safety guarantee or a fixed worst-case runtime.

Original PDF illustration S6.F5
Fig. 5: Flowchart for recovering from collisions found in the optimized trajectory.

VI-C Inflating Multiple Paths

To obtain alternative routes, the heuristic selects an interior region containing neither endpoint, temporarily invalidates roadmap nodes inside it, reruns A*, and inflates the new path. It stops if this fails. Additional intersecting regions may reveal better paths to graph-based planners, but neither route diversity nor better optimized cost is guaranteed.

VII Experimental Evaluation

The evaluation uses synthetic planning problems and a perception-driven robot demonstration; it compares both path length and traversal-time objectives.

VII-A Simulation Benchmark

Simulation covers a two-dimensional obstacle field and a seven-joint arm. Four roadmap sizes, ten scene seeds and ten roadmap seeds produce 400 instances per optimization approach and problem setting.

VII-A 1 Environments

Forest places 15 radius-0.35 circles inside the central 7 × 7 area of a 10 × 10 square, with opposite-corner endpoints. Franka places 20 spheres in front of a seven-DOF FR3 arm; 33 robot spheres and a box table generate 1,187 collision pairs, including 527 self-collision pairs. Random starts lie on one side of the table and goal poses on the other.

Original PDF illustration S7.F6
Fig. 6: Our simulation benchmark consists of the Forest environment, shown on the left, and the Franka environment, shown on the right. A collision-free motion plan starting at the red shaded robot and ending at the blue is shown in the Franka environment.

VII-A 2 Problem Types

Minimum-distance planning minimizes polygonal path length. Minimum-time planning also enforces joint velocity/acceleration limits and zero velocity at both endpoints. The original objectives and constraints are preserved; these are kinematic planning problems rather than full dynamics or torque-constrained control.

Equations 14a–15f
minimizep\displaystyle\mathop{\textbf{minimize}}_{p} L⁡(p),\displaystyle\;\;L(p), (14a)
subject to\displaystyle\mathop{\textbf{subject to}} p⊆𝒞free,\displaystyle\;\;p\subseteq\mathcal{C}^{\mathrm{free}}, (14b)
pinit=s,pterm=g,\displaystyle\;\;p_{\text{init}}=s,\penalty\ p_{\text{term}}=g, (14c)
minimizep,T\displaystyle\mathop{\textbf{minimize}}_{p,T} T,\displaystyle\;\;T, (15a)
subject to\displaystyle\mathop{\textbf{subject to}} p⊆𝒞free,\displaystyle\;\;p\subseteq\mathcal{C}^{\mathrm{free}}, (15b)
p⁡(0)=s,p⁡(T)=g,\displaystyle\;\;p(0)=s,\penalty\ p(T)=g, (15c)
p˙​(0)=0,p˙​(T)=0,\displaystyle\;\;\dot{p}(0)=0,\penalty\ \dot{p}(T)=0, (15d)
vmin≤p˙​(t)≤vmax​∀t∈[0,T],\displaystyle\;\;v_{\text{min}}\leq\dot{p}(t)\leq v_{\text{max}}\penalty\ \forall t\in[0,T], (15e)
amin≤p¨​(t)≤amax​∀t∈[0,T],\displaystyle\;\;a_{\text{min}}\leq\ddot{p}(t)\leq a_{\text{max}}\penalty\ \forall t\in[0,T], (15f)

VII-A 3 Baseline Approach

The nonlinear baseline uses Drake [42] and SNOPT or IPOPT, initialized from the same roadmap path. Collision constraints are imposed at finitely many points. Minimum-time initialization sets fourth-order B-spline control points from the roadmap path. Solver choices and collision-discretization counts are tuned separately by scene and task, so this is a particular implemented baseline rather than all nonlinear planning methods.

VII-A 4 Employed DBMPs

LSCS solves a shortest-path second-order cone program through one region sequence using MOSEK. LGCS inflates two routes and solves a mixed-integer graph-of-convex-sets formulation [46] with Gurobi. SCSTO uses SCSTrajopt [15] and Clarabel for minimum-time trajectories through a sequence, alternating convex subproblems. Global optimality of the graph problem remains restricted to its constructed regions.

VII-A 5 Results

On an i9-10850K and RTX 3090, Forest roadmaps contain 200–1,600 nodes and Franka roadmaps 3,000–12,000. Table I conditions optimization statistics on roadmap success: that prerequisite succeeds in all Forest cases and 96.1% of Franka cases. The single-sequence LSCS/SCSTO methods attain 100% conditional collision-free rate. The two-route LGCS Franka result is 99.3%, so the blanket “never failed” claim does not apply to every convex-set variant.

The reported headline averages are 17.1× speedup and a rise from about 72.1% to 100% conditional reliability: roughly 27.9 percentage points, not a 27.9% relative gain. Table I’s total optimization-stage time includes region construction and trajectory optimization, but not the preceding roadmap/IK/perception stages. For Franka, LSCS averages 152.5 ms versus NTO’s 3,084.6 ms; SCSTO averages 307.9 ms versus 4,147.8 ms. Full costs and empirical standard deviations remain in the table.

Among successful solutions, NTO finds lower-cost trajectories. Franka length is 5.519 versus 7.382 for LSCS (about 34% higher for LSCS relative to NTO), and traversal time is 5.057 versus 6.473 (about 28% higher). This is a material quality tradeoff, not uniformly a slight cost increase. Two-route LGCS offers only small length improvements while sharply increasing runtime. Region repair triggers in 5.6%/0.46% of Forest distance/time cases and 40.22%/21.36% of Franka cases.

Problem Minimum Distance Minimum Time
Environment Forest Franka Forest Franka
DRM SR 1.00 0.961 1.00 0.961
DRM num. LS 3.45±\pm0.86 4.49±\pm1.24 3.45±\pm0.86 4.49±\pm1.24
DRM path len. 10.90±\pm0.40 [m] 8.55±\pm2.28 [rad] 10.90±\pm0.40 [m] 8.55±\pm2.28 [rad]
TO Approach NTO LSCS LGCS NTO LSCS LGCS NTO SCSTO NTO SCSTO
cost (S only) 10.727±\pm0.3 10.770±\pm0.3 10.732±\pm0.3 5.519±\pm1.1 7.382±\pm1.7 7.110±\pm1.6 9.363±\pm0.2 10.096±\pm0.7 5.057±\pm1.2 6.473±\pm1.5
time total [ms] 357.8±\pm2520.3 18.2±\pm9.8 96.8±\pm142.1 3084.6±\pm2938.0 152.5±\pm89.1 4441.6±\pm10194 335.1±\pm167.2 19.3±\pm7.8 4147.8±\pm2674.6 307.9±\pm226.3
time TO [ms] 357.8±\pm2520.3 13.9±\pm5.7 91.3±\pm138.6 3084.6±\pm2938.0 17.4±\pm14.4 4236.5±\pm10135 335.1±\pm167.2 15.9±\pm6.1 4147.8±\pm2674.6 189.1±\pm170.7
time CS [ms] - 4.2±\pm4.7 5.5±\pm5.5 - 135.1±\pm76.0 205.2±\pm107.8 - 3.4±\pm2.6 - 118.8±\pm61.0
num. CS - 2.45±\pm0.9 3.67±\pm2.3 - 3.49±\pm1.2 6.18±\pm2.8 - 2.45±\pm0.9 - 3.49±\pm1.2
TO SR 0.993 1.000 1.000 0.863 1.000 0.993 0.459 1.000 0.735 1.000
CFR 0.970 1.000 1.000 0.725 1.000 0.993 0.459 1.000 0.732 1.000
CFR given TO S 0.977 1.000 1.000 0.840 1.000 1.000 1.000 1.000 0.995 1.000
abbreviation S SR CFR TO LS CS
expanded success success rate collision-free rate trajectory optimization line segments convex sets
TABLE I: Simulation results of solving the minimum distance and minimum time problems on both the Forest and Franka environments. The table indicates the averages and empirical standard deviations of the statistics across the different road map sizes, and random seeds for the environment and road map construction provided the DRM found a solution.

VII-B Validation on Hardware

Hardware uses a KUKA LBR iiwa 7 R800, three RealSense D415 cameras, an i9-7900X and RTX 2080 Ti. Robot geometry has 11 boxes and 105 self-collision pairs. Joint velocity and acceleration bounds are ±1 rad/s and ±1 rad/s². The robot alternates between poses on opposite sides of a table, while obstacles are placed or moved between plans.

Fifteen trajectories have no observed planning failure. Total latency is 823.4 ± 122.6 ms: perception 369.7 ms, IK 130.1 ms, roadmap 116.8 ms, regions 122.5 ms and SCSTO 82.8 ms, plus overhead. Thus the 0.12-second region construction is only one part of online latency. The prose describes 8 cm perception voxels, while the offline roadmap parameter table gives 6 cm voxels; these different stages should not be silently conflated. This small demonstration does not establish a general failure rate or responsiveness to moving obstacles.

Original PDF illustration S7.F7
Fig. 7: In our hardware experiments, we alternate between planning to the right and the left side of the table. Between plans we place, move and remove obstacles. Starting from the left, first we place the shoe rack and box, highlighted in green, then we run our planner and move the robot to the right side of the table. Next, we slide the shoe rack closer to the robot and run our planner again. The shoe rack forces the arm to move in a higher arc to reach the left side of the table.
Component Time [ms]
perception 369.7 ±\pm 16.7
IK 130.1 ±\pm 79.5
DRM 116.8 ±\pm 34.8
convex sets 122.5 ±\pm 43.1
SCSTO 82.8 ±\pm 67.9
overhead 1.5 ±\pm 0.6
total 823.4 ±\pm 122.6
Auxiliary Stats. Value
num. line seg. 2.9 ±\pm 0.6
num. safe sets 1.93 ±\pm 0.57
col. checks / set 121018 ±\pm 24262
hyperplanes / set 60.7 ±\pm 14.4
voxels 359.6 ±\pm 76.4
col. pairs 4431.2 ±\pm 916.7
self-col. pairs 105
TABLE II: Statistics (the mean and standard deviation) for the hardware experiments. For the 15 collected trajectories we aggregate the timing information on the left, and auxiliary information on the right. Between each of the trajectories, the obstacle setup was modified similarly to Fig. 7.

VIII Limitations

The principal limits are region-constrained solution quality, memory-intensive geometry checking, untested higher-dimensional scaling and weak goal IK.

VIII-A Local and Global Improvements with Nonlinear Solvers

Convex planners cannot leave the constructed region union, so poor seed paths or overly small regions can exclude good trajectories. A nonlinear optimizer can sometimes escape its initialization and reach a lower-cost solution, explaining part of the observed tradeoff. Multiple inflated paths only modestly help in these tests. Conversely, failed plans can carry operational costs absent from successful-only trajectory averages.

VIII-B GPU Memory

The collision checker evaluates configurations against voxel obstacles in parallel; finer three-dimensional grids increase memory use cubically with resolution. Signed-distance-field representations [49][50] are proposed future work, not part of the evaluated implementation.

VIII-C Scaling to Higher Dimensions

Only two- and seven-dimensional systems are tested. Mobile manipulation, bimanual systems and larger configuration spaces may challenge both roadmap coverage and region sampling; their scalability remains unverified.

VIII-D Inverse Kinematics

Hardware goal poses are deliberately not overlapped by obstacles, producing relatively simple IK problems. The authors identify IK as a weak point but do not stress-test it. Robot-specific analytic IK [52] is a possible improvement.

IX Conclusion

EI-ZO makes path-guided convex-region generation fast enough for repeated planning in scenes that change between motions. In these benchmarks, single-sequence planning trades higher trajectory cost for better conditional reliability and lower optimization latency. Its useful complementarity with faster sampling planners and richer convex-set optimizers does not remove assumptions about perception, static execution scenes, valid seed paths or sampled collision checks.

Acknowledgments

The authors acknowledge discussions, SCSTrajopt code, software/hardware assistance and funding from Toyota Research Institute.

Appendix A Appendix

The appendix retains the convexity proof, roadmap construction, complete shortest-path formulation and all implementation parameters.

A-A Distance Function Convexity

The proof combines triangle inequality with convexity of the target set: a convex combination of nearest points remains in the set, so distance at a convex combination of query points is no greater than the corresponding weighted distances. The original three displayed steps are retained. As written, the argument assumes the distance minimizers exist, which holds for the closed line-segment seed used here.

Equations 16–18
λ​𝐝𝐢𝐬𝐭𝒞​(x1)+(1−λ)​𝐝𝐢𝐬𝐭𝒞​(x2)=λ​‖x1−z1‖+(1−λ)​‖x2−z2‖\displaystyle\begin{split}\lambda{\bf dist}_{\mathcal{C}}(x_{1})+(1-\lambda){\bf dist}_{\mathcal{C}}(x_{2})\\ =\lambda||x_{1}-z_{1}||+(1-\lambda)||x_{2}-z_{2}||\end{split} (16)
λ​‖x1−z1‖+(1−λ)​‖x2−z2‖≥‖λ​x1+(1−λ)​x2−(λ​z1+(1−λ)​z2)‖,\displaystyle\begin{split}\lambda||x_{1}-z_{1}||+(1-\lambda)||x_{2}-z_{2}||\\ \geq||\lambda x_{1}+(1-\lambda)x_{2}-(\lambda z_{1}+(1-\lambda)z_{2})||,\end{split} (17)
‖λ​x1+(1−λ)​x2−(λ​z1+(1−λ)​z2)‖≥𝐝𝐢𝐬𝐭𝒞​(λ​x1+(1−λ)​x2),\displaystyle\begin{split}||\lambda x_{1}+(1-\lambda)x_{2}-(\lambda z_{1}+(1-\lambda)z_{2})||\\ \geq{\bf dist}_{\mathcal{C}}(\lambda x_{1}+(1-\lambda)x_{2}),\end{split} (18)

A-B DRM Construction

Offline construction samples collision-free configurations, connects nearby nodes subject to configuration-space and task-space distance thresholds, limits neighbors, and symmetrizes adjacency. Each workspace voxel is represented by its circumscribing sphere of radius √3 s/2. Checking every voxel against roadmap nodes builds the collision lookup map; online edge checks remain lazy.

A-C Polygonal Shortest Paths through Sequences of Convex Sets

The intended LSCS objective minimizes total Euclidean distance between adjacent knot points, with start/goal endpoints and successive-region intersection constraints. Equation 19 is preserved exactly but contains an apparent transcription error: (19d) imposes vi = vi+1 for every segment, forcing all knots equal and making distinct endpoints infeasible. The variable range also starts at v0 while the endpoint constraint uses v1. These printed equations should be reconciled with the implementation before treating them as runnable optimization specifications.

Equations 19a–19f
minimizev0≤i≤M+1\displaystyle\mathop{\textbf{minimize}}_{v_{0\leq i\leq M+1}} ∑i=1M‖vi−vi+1‖2,\displaystyle\;\;\sum_{i=1}^{M}||v_{i}-v_{i+1}||_{2}, (19a)
subject to\displaystyle\mathop{\textbf{subject to}} v1=s,\displaystyle\;\;v_{1}=s, (19b)
vM+1=g,\displaystyle\;\;v_{M+1}=g, (19c)
vi=vi+1​ for ​i=1,…,M,\displaystyle\;\;v_{i}=v_{i+1}\text{ for }i=1,\dots,M, (19d)
vi∈𝒫i, for i=1,…,M,\displaystyle\;\;v_{i}\in\mathcal{P}_{i},\text{ for }i=1,\dots,M, (19e)
vi∈𝒫i−1, for i=2,…,M,\displaystyle\;\;v_{i}\in\mathcal{P}_{i-1},\text{ for }i=2,\dots,M, (19f)

A-D Supplementary Information on Experimental Evaluation

Simulation uses Ubuntu 22.04, CUDA 12.6 and an RTX 3090. At batches of two million configurations, collision-check throughput is about 19.9 million/s for Forest and 8.6 million/s for Franka. SNOPT is used for both Forest tasks with 20/100 collision points; Franka uses SNOPT for distance and IPOPT for time, with 150 points. Lazy path checks use steps of 0.1 m or 0.1 rad. These batch throughputs do not imply equivalent end-to-end planning rates.

Forest EI-ZO uses δ = 0.05, ε = 0.01, 1,000 update samples, ten faces per iteration, 30 mixing steps and step back 0.01. Franka uses δ = ε = 0.005, 10,000 samples, ten faces, 60 mixing steps and the same step back. Acceptance typically occurs within ten iterations; this observation is not an iteration guarantee. Tables III–VI retain every parameter.

Hardware uses CUDA 12.5, a 35,000-node roadmap and the Franka EI-ZO settings with 20 faces per iteration. Collision checking reaches about 6.9 million configurations/s at a two-million batch. No region repair was triggered in the 15 hardware examples. The offline roadmap uses a 0.45 m task-space neighbor limit, 4.5 rad configuration-space limit, ten neighbors and 0.06 m voxel side length.

Parameter Symbol Value
admissible uncertainty δ\delta 0.05
admissible fraction in collisions ε\varepsilon 0.01
number of samples to update NpN_{p} 1000
max number of faces per iteration NfN_{f} 10
number of mixing steps for sampling NmsN_{\text{ms}} 30
step back Δmax\Delta_{\text{max}} 0.01
TABLE III: Employed parameters in the Forest environment for EI-ZO.
Parameter Symbol Value
admissible uncertainty δ\delta 0.005
admissible fraction in collision ε\varepsilon 0.005
number of samples to update NpN_{p} 10000
max number of faces per iteration NfN_{f} 10
number of mixing steps for sampling NmsN_{\text{ms}} 60
step back Δmax\Delta_{\text{max}} 0.01
TABLE IV: Employed parameters in the Franka environment for EI-ZO.
Parameter Symbol Value
max. task space distance dTSd_{\text{TS}} 10 [m]
max. configuration space distance dCSd_{\text{CS}} 10 [m]
max. neighbors kk 10
offline voxel length ss 0.06 [m]
TABLE V: Employed parameters in the Franka and Forest environments for constructing the DRMs.
Parameter Symbol Value
max. task space distance dTSd_{\text{TS}} 0.45 [m]
max. configuration space distance dCSd_{\text{CS}} 4.5 [rad]
max. neighbors kk 10
offline voxel length ss 0.06 [m]
TABLE VI: Employed DRM construction parameters for the hardware expermients.
Concept demonstration

Open image ↗