Literature
Display
← All papers

Multi-Query Shortest-Path Problem in Graphs of Convex Sets

Savva Morozov · Tobia Marcucci · Alexandre Amice · Bernhard Paus Graesdal · Rohan Bosworth · Pablo A. Parrilo · Russ Tedrake

· WAFR 2024 proceedings (2026 print edition) ↗

Robotics9 min text · CondensedOriginal paper ↗
Contents

At a glance

The paper accelerates repeated motion-planning queries in a fixed graph of convex sets. Offline semidefinite programs construct quadratic lower bounds on remaining path cost; online short-horizon convex lookahead uses those bounds to choose successive regions, with backtracking when necessary. Numerical examples include a simulated seven-joint arm and a 190-vertex graph. The planner trades some path quality for much lower reported solver time, but is incomplete and does not guarantee globally optimal motion in the original free space.

1 Introduction

A graph of convex sets [24] represents discrete region choices together with continuous configurations or trajectory parameters. Existing shortest-path formulations [26] optimize both simultaneously, but repeated large mixed discrete–continuous problems can be too slow for an online planning loop.

The proposed approach amortizes work over many queries in an unchanged environment. Convex quadratic value bounds are cheap to store and permit parallel convex lookahead. These bounds guide an approximate policy; they are not a complete precomputed oracle for every optimal path.

Refer to caption
Figure 1: Robotic arm in a simulated environment, tasked with moving items between shelves and bins. Shown are four queries for collision-free motion planning.
Original figure
(a) Offline: synthesize cost-to-go over the GCS. Contour plots are shown.
Original figure
Original figure
Original figure
(b) Online: at each iteration, we evaluate all nn-step paths from the current vertex (n=1n\!=\!1 shown) and greedily select the decision that minimizes the nn-step lookahead cost-to-go. The first three iterations are shown, as the path is built incrementally.
Figure 2: Illustration of our approach. The GCS instance is embedded in ℝ2\mathbb{R}^{2}, with the source vertex at the top and the target vertex at the bottom. The edges are shown as red arrows, and the edge length is the squared Euclidean distance.

1.1 Literature review

Roadmaps and sampling trees search over sampled configurations; GCS searches through continuous collision-free regions, potentially improving path quality while requiring expensive region construction. Classical all-pairs shortest paths and explicit model predictive control motivate precomputing cost-to-go information. Exact value functions can be expensive to compute and represent, so this work chooses inexpensive lower bounds and online lookahead rather than exact dynamic programming.

2 All-Pairs Shortest Paths in a Graph of Convex Sets

The formulation starts with ordinary graph paths, then adds continuous vertex states and convex transition constraints. Its central difficulty is that forbidding repeated vertices makes the optimal continuation depend on the history of visited regions.

2.1 All-Pairs Shortest Paths

The classical graph problem provides the comparison case: fixed nonnegative edge costs allow an optimal successor policy to reconstruct a shortest path from precomputed distances.

Graphs and paths.

A path is a sequence of distinct vertices connected by directed edges. The no-revisit convention matters later: a walk that returns to a convex region at a different continuous state is excluded even if it would reduce total cost.

Shortest Path Problem (SPP).

The ordinary shortest-path problem minimizes summed fixed edge costs. With nonnegative costs, an optimal subpath is itself optimal, supporting a history-independent cost-to-go and efficient dynamic programming.

Equation
min𝑝∑e∈ℰpces.t.p∈𝒫s,t.\underset{p}{\min}\quad\sum_{e\in\mathcal{E}_{p}}c_{e}\quad\text{s.t.}\quad p\in\mathcal{P}_{s,t}.

All-Pairs Shortest Paths (APSP).

All-pairs shortest paths computes the remaining cost between every source and target vertex. Selecting the outgoing edge with minimum immediate cost plus remaining cost yields the next optimal vertex. A successor table or distance matrix can therefore encode many complete paths compactly.

Equations 1a–1b
π⁡(v,t)=arg⁡min𝑤\displaystyle\pi(v,t)\quad=\quad\underset{w}{\arg\min}\quad ce+Jw,t∗\displaystyle c_{e}+J^{*}_{w,t} (1a)
s.t. e=(v,w)∈ℰ.\displaystyle e=(v,w)\in\mathcal{E}. (1b)

2.2 Shortest-Path Problem in a Graph of Convex Sets

GCS replaces each isolated graph node with a convex domain for a continuous optimization variable, and couples neighboring domains through convex costs and constraints.

Graph of Convex Sets.

Each vertex carries a bounded convex set and a variable inside it. Each directed edge restricts the pair of adjacent variables to a convex transition set and assigns a nonnegative convex cost. Selecting a discrete path leaves a continuous convex optimization problem along that path.

The Shortest Path Problem in a Graph of Convex Sets.

The optimization jointly selects a simple vertex path and continuous variables satisfying the prescribed source, target, and transition conditions. Although a fixed path is tractable, selecting the best path is generally NP-hard. Strong mixed-integer convex relaxations and rounding often work well in practice.

The four-vertex example explains why ordinary Bellman optimality does not transfer directly. A locally optimal continuation may pass through a vertex already visited earlier, invalidating the full simple path. Allowing revisits restores the memoryless principle in this example but changes the requested problem. Thus a value function indexed only by current and target states cannot encode every history-dependent optimal decision.

Equations 2a–2e
minp,{xv}v∈p\displaystyle\qquad\qquad\underset{p,\;\{x_{v}\}_{v\in p}}{\min}\quad ∑e=(v,w)∈ℰple​(xv,xw)\displaystyle\sum_{e=(v,w)\in\mathcal{E}_{p}}l_{e}(x_{v},\,x_{w}) (2a)
s.t. p∈𝒫s,t,\displaystyle p\in\mathcal{P}_{s,t}, (2b)
xs=x¯s,xt=x¯t,\displaystyle x_{s}=\bar{x}_{s},\quad x_{t}=\bar{x}_{t}, (2c)
xv∈𝒳v,\displaystyle x_{v}\in\mathcal{X}_{v}, ∀v∈p,\displaystyle\forall v\in p, (2d)
(xv,xw)∈𝒳e,\displaystyle(x_{v},x_{w})\in\mathcal{X}_{e}, ∀e=(v,w)∈ℰp.\displaystyle\forall e=(v,w)\in\mathcal{E}_{p}. (2e)
Original figure
(a) The optimal policy at vertex vv depends on previous decisions. If ww has been visited already, the optimal decision is to go to tt (orange), otherwise it is to go to ww (blue).
Original figure
(b) If we allow vertex revisits, the optimal policy is independent of past decisions. Shown are the optimal s​-​ts\textsf{-}t (green) and v​-​tv\textsf{-}t (blue) solutions to the relaxed problem.
Figure 3: The two-dimensional GCS from Example 1. The convex sets paired with s,v,ts,v,t are points and the one paired with ww is a segment. The GCS is fully connected, and the edge lengths are the squared Euclidean distance.

2.3 All-Pairs Shortest Paths in a Graph of Convex Sets

The multi-query problem asks for paths across specified sets of source and target vertices and every relevant pair of continuous endpoint states. It contains the single-query problem and remains computationally hard.

2.4 Method outline

Offline vertex penalties discourage repeated regions while preserving valid cost lower bounds. Online rollout explicitly excludes previously visited vertices and evaluates several future steps at once to reduce the effect of coarse quadratic bounds. Both the history approximation and the restricted value-function family can introduce suboptimality.

3 Offline phase: synthesis of cost-to-go lower bounds

The derivation first assumes one source region and a fixed target point. It introduces an infinite-dimensional optimization over value functions, then restricts those functions and their certificates to obtain a finite semidefinite program. The appendix extends the construction to multiple sources and continuous or multiple targets.

3.1 Cost-to-go lower bounds via infinite-dimensional LP

The objective raises the average source value under a specified distribution of anticipated starting states. For every feasible transition, a Bellman-like inequality bounds the current value by edge cost, a nonnegative entry penalty for the next vertex, and its remaining value. The terminal value subtracts the sum of all vertex penalties, waiving each penalty once.

The lower-bound proof sums these inequalities along a simple optimal path. Intermediate values telescope; subtracting all penalties cancels every penalty actually accrued and additionally subtracts the nonnegative penalties for vertices never visited. The resulting value is therefore no greater than the true path cost. Those unused penalties also explain a possible relaxation gap: the construction can remain conservative even before choosing quadratic functions.

Equations 3a–5
max{Jv,hv}v∈𝒱\displaystyle\max_{\{J_{v},h_{v}\}_{v\in\mathcal{V}}}\quad ∫𝒳sJs​(x)​d​ϕs​(x)\displaystyle\int_{\mathcal{X}_{s}}J_{s}(x)d\phi_{s}(x) (3a)
s.t. Jv:𝒳v→ℝ,\displaystyle J_{v}:\mathcal{X}_{v}\rightarrow\mathbb{R}, ∀v∈𝒱,\displaystyle\forall v\in\mathcal{V}, (3b)
hw≥0\displaystyle h_{w}\geq 0 ∀w∈𝒱,\displaystyle\forall w\in\mathcal{V}, (3c)
Jv​(xv)≤le​(xv,xw)+hw+Jw​(xw),\displaystyle J_{v}(x_{v})\leq l_{e}(x_{v},x_{w})+h_{w}+J_{w}(x_{w}), ∀e=(v,w)∈ℰ,\displaystyle\forall e=(v,w)\in\mathcal{E},\! (3d)
∀(xv,xw)∈𝒳e,\displaystyle\forall(x_{v},x_{w})\in\mathcal{X}_{e},\!
Jt(xt)=−∑w∈𝒱hw.\displaystyle J_{t}(x_{t})=-\sum_{w\in\mathcal{V}}h_{w}. (3e)
Jv​(xv)≤Jv∗​(xv)​ for all ​v∈𝒱.J_{v}(x_{v})\leq J_{v}^{*}(x_{v})\text{ for all }v\in\mathcal{V}.
Jv​(xv)≤∑e=(u,w)∈ℰple​(xu,xw)​∑w∈phw+Jt​(xt),\displaystyle J_{v}(x_{v})\leq\sum_{e=(u,w)\in\mathcal{E}_{p}}l_{e}(x_{u},x_{w})\sum_{w\in p}h_{w}+J_{t}(x_{t}), (4)
Jv​(xv)+∑w∉phw≤Jv∗​(xv).\displaystyle J_{v}(x_{v})+\sum_{w\notin p}h_{w}\;\leq\;J_{v}^{*}(x_{v}). (5)

Example 1, continued.

In the four-vertex example, zero penalties give value 14, corresponding to a cheaper walk that revisits a vertex. Optimizing its penalty to 2 recovers the simple-path optimum 16. This example demonstrates possible tightness, not a general equality theorem. When every region is a singleton, the construction reduces to a standard finite shortest-path linear program; general regions require optimization over functions.

Equation 6
max{Jv}v∈𝒱\displaystyle\max_{\{J_{v}\}_{v\in\mathcal{V}}} Js\displaystyle J_{s} (6)
s.t.\displaystyle\text{s.t.} Jv≤le+Jw,\displaystyle J_{v}\leq l_{e}+J_{w},\qquad ∀e=(v,w)∈ℰ,\displaystyle\forall e=(v,w)\in\mathcal{E},
Jt=0.\displaystyle J_{t}=0.

3.2 Numerical approximation via semidefinite programming

Quadratic nonnegativity can be certified through positive-semidefinite matrices. A multiplier certificate enforces nonnegativity on a constrained domain by subtracting a nonnegative combination of its defining inequalities. This S-procedure-style condition is sufficient and can be conservative; restricting value functions and certificates yields a tractable approximation rather than an exact solution of the functional program.

Equation
f⁡(x)=[1x]⊤​Q​[1x]​ for some ​Q⪰0.\displaystyle f(x)=\begin{bmatrix}1\\ x\end{bmatrix}^{\top}\!Q\begin{bmatrix}1\\ x\end{bmatrix}\;\text{ for some }\;Q\succeq 0.

Defining cost-to-go lower bounds in ( 3b ).

Each lower bound is a convex quadratic with optimized coefficients and a positive-semidefinite curvature constraint. Penalty nonnegativity and the terminal constraint remain linear in the decision variables. The low-order representation keeps evaluation cheap but cannot express a general piecewise value function.

Enforcing the lower-bound constraint ( 3d ).

The numerical construction uses polyhedral or ellipsoidal region and transition descriptions and quadratic edge costs. These assumptions make the transition lower-bound certificate a linear matrix inequality. Nonquadratic costs, including Euclidean distance, are replaced by quadratic approximations, so the certificate must be interpreted for the chosen cost model.

The objective function ( 3a ).

Averaging a quadratic under a fixed source distribution is linear in its coefficients. Higher-degree sum-of-squares bounds could be more expressive but are reported too expensive here. Affine bounds are closely connected to the dual of the standard GCS convex relaxation and provide a cheaper, weaker comparison in the experiments.

4 Online phase: greedy multi-step lookahead policy

At each rollout step, enumerate feasible short vertex sequences that avoid previously visited vertices. Solve their continuous convex lookahead problems in parallel, using actual edge costs and the final quadratic value bound, then execute only the first planned transition. Entry penalties are omitted online because revisits are explicitly prohibited.

Once the target is reached, reoptimize all continuous variables along the selected vertex path. If lookahead becomes infeasible, backtrack to an earlier vertex with another outgoing option. This procedure is sound but incomplete: returned paths satisfy the modeled constraints, but it may fail to find a path that exists. Short-horizon feasibility does not guarantee recursive feasibility, and final continuous reoptimization does not make the discrete path globally optimal.

Equations 7a–7c
π⁡(vk,xk,pk)=arg⁡min(w,xw)\displaystyle\pi(v_{k},x_{k},p_{k})\quad=\quad\underset{(w,x_{w})}{\arg\min} le​(xk,xw)+Jw​(xw)\displaystyle\quad l_{e}(x_{k},x_{w})+J_{w}(x_{w}) (7a)
s.t. e=(vk,w)∈ℰ,w∉pk,\displaystyle\quad e=(v_{k},w)\in\mathcal{E},\quad w\notin p_{k}, (7b)
(xk,xw)∈𝒳e.\displaystyle\quad(x_{k},x_{w})\in\mathcal{X}_{e}. (7c)

5 Experimental evaluation

All reported experiments are numerical, using a 4.5 GHz sixteen-core Ryzen 9 desktop, 64 GB memory, and Mosek 10.2.1. The evaluation separates a visual two-dimensional example, a simulated arm workload, and a larger graph ablation. Reported online timings use parallelized solver time.

5.1 Two-dimensional example

A nine-vertex, twenty-five-edge example uses squared Euclidean edge costs and a point target. Quadratic bounds visibly underestimate the more complex optimal value function. One- and two-step lookahead create more expressive piecewise bounds; two-step rollout is nearly always optimal in the plotted example even though its bound is not everywhere tight. This illustrates the mechanism rather than establishing a general approximation ratio.

Original figure
Figure 4: Comparison of lower and upper bounds on the cost-to-go over a horizontal slice of the source set 𝒳s\mathcal{X}_{s} from Fig. 2(a). The cost-to-go function Js∗J_{s}^{*} (green) is piecewise-quadratic. Convex quadratic lower bound JsJ_{s} (purple) is naturally a poor lower-bound. Multi-step lookaheads (solid orange, blue) produce tighter piecewise-quadratic lower bounds. Upper bounds on the cost-to-go are obtained by rolling out the multi-step lookahead policy (dashed orange, blue), which produces near-optimal solutions.

5.2 Collision-free motion planning for a robot arm

A simulated seven-joint KUKA iiwa moves virtual items between six shelves and three bins. IRIS-NP [29] and clique seeding construct a collision-free convex cover with 23 vertices and 68 edges. Vertices parameterize line segments with position-continuous joins. Twelve source regions and three target regions support 120 queries; bin-to-shelf paths reverse the computed shelf-to-bin solutions.

Generating the cover costs about 100 seconds, and the new value bounds add six seconds: Figure 5 reports 106 seconds total offline work for this method, compared with 100 seconds for GCS and 0.9 seconds for the 10,000-vertex shortcut roadmap. The six-second figure alone excludes the shared cover construction.

Online median solver time is 5 ms and maximum 11 ms. The method is reported 40 times faster on average than fresh GCS queries with paths 7% longer, and 110 times faster than shortcut PRM with paths 5% shorter. These are simulated planning results and parallel solver timings, not measured physical-robot control latency. Path quality also depends on the supplied convex cover; no global free-space optimality claim is made.

Original figure
Figure 5: For the robot arm scenario in Section 5.2, we compare path length and solve time performance between the APSP in GCS, single-query SPP in GCS, and shortcut PRM over 120 queries. The offline phases take 106s, 100s, and 0.9s respectively. The APSP in GCS is on average 40 times faster than the SPP in GCS, with minimal reduction in solution quality. Compared to sPRM, the APSP in GCS is on average 110 times faster.

5.3 Scalability and ablation on lower bounds and lookahead horizon

The larger environment has 190 vertices and 540 edges. Vertex variables specify cubic Bézier control points, adjacent curves join with continuous first derivatives, and the objective sums squared control-point distances. Exact mixed-integer solutions supply reference costs for 120 endpoint queries.

Quadratic bounds with two- and three-step lookahead yield median gaps of 9.4% and 8.8% in 4 and 5 ms, with no failures in the tested queries. Affine bounds with three steps yield an 80.2% median gap and 9.9% failures; nine steps improve the gap to 7.0% but take 388 ms. Rollouts stop after 10,000 iterations, so failure rates reflect that finite budget.

Table 1 reports a 6.9% median gap and 716 ms median time for the standard GCS relaxation-based planner; its 75th-percentile time is 1,051 ms. This distinguishes the table statistics from prose describing time above 1,000 ms. Observing no failures for selected quadratic settings does not make the incomplete algorithm complete.

Original figure
(a) Optimal solution.
Original figure
(b) Quadratic lower bounds.
Original figure
(c) Affine lower bounds.
Figure 6: A 3-step lookahead policy with quadratic JvJ_{v} (blue) yields diverse vertex paths resembling the optimal solutions (green). A 3-step lookahead with affine JvJ_{v} (orange) follows a single vertex sequence regardless of the target point, accruing much higher cost.
Solution method Optimality gap, % Solve time, ms Failure rate, %
Quadratic JvJ_{v}, 1-step 20.0 (62.1) 3 (3) 0.0
Quadratic JvJ_{v}, 2-step 9.4 (22.3) 4 (4) 0.0
Quadratic JvJ_{v}, 3-step 8.8 (15.7) 5 (6) 0.0
Affine JvJ_{v}, 1-step 157.1 (N/A) 2 (657) 27.2
Affine JvJ_{v}, 2-step 142.4 (418.8) 3 (914) 14.0
Affine JvJ_{v}, 3-step 80.2 (348.3) 5 (808) 9.9
Affine JvJ_{v}, 8-step 11.9 (37.4) 169 (1996) 3.3
Affine JvJ_{v}, 9-step 7.0 (26.2) 388 (2454) 0.0
SPP in GCS 6.9 (12.0) 716 (1051) 0.0
Table 1: Impact of the degree of JvJ_{v} and lookahead horizon on performance, over 120 queries for the GCS in Fig. 6. We report optimality gaps (ratio between solution cost and optimal cost), solve times, and failure rates (rollout policy is terminated after 10,000 iterations). We report median values, with the 75th percentile in the parenthesis. Low-horizon lookahead policies with quadratic lower bounds yield near optimal solutions, perform much better than the affine bounds.

6 Conclusion and future work

The results support amortizing static-environment planning through quadratic cost bounds and short convex lookahead. Remaining limits include expensive convex-cover construction, approximate costs and value functions, history dependence, and incomplete online search. Moving obstacles, grasp-dependent geometry, and alternative search policies are future work. Adding nonconvex online constraints could accommodate changes but would alter the convex-solver advantage tested here.

Appendix 0.A Extensions and variations

Multiple source regions contribute additional average-value terms to the objective. A continuous target region is handled by value functions of both current and target states, with corresponding transition and terminal constraints. Target-dependent nonnegative penalties can tighten the bounds, and separate programs for multiple target vertices can run in parallel.

The online policy likewise receives the target vertex and point. The appendix also proposes penalties for traversing both directions of a two-cycle, waived once at the target, and sketches extension to longer cycles. These variations retain the lower-bound construction’s intent; they do not supply a general completeness or exact-optimality guarantee.

Equations
Jv,t​(xv,xt)≤le​(xv,xw)+hw+Jw,t​(xw,xt),J_{v,t}(x_{v},x_{t})\leq l_{e}(x_{v},x_{w})+h_{w}+J_{w,t}(x_{w},x_{t}),
Jt,t(xt,xt)=−∑w∈𝒱hw(xt),J_{t,t}(x_{t},x_{t})=-\sum_{w\in\mathcal{V}}h_{w}(x_{t}),
π⁡(vk,xk,pk,t,xt)=arg⁡min(w,xw)\displaystyle\pi(v_{k},x_{k},p_{k},t,x_{t})\quad=\quad\underset{(w,x_{w})}{\arg\min} le​(xk,xw)+Jw,t​(xw,xt)\displaystyle\quad l_{e}(x_{k},x_{w})+J_{w,t}(x_{w},x_{t})
s.t. e=(vk,w)∈ℰvkout,w∉pk,\displaystyle\quad e=(v_{k},w)\in\mathcal{E}_{v_{k}}^{\text{out}},\quad w\notin p_{k},
(xk,xw)∈𝒳e.\displaystyle\quad(x_{k},x_{w})\in\mathcal{X}_{e}.
Concept demonstration

Open image ↗