Literature
Display
← All papers

Mixed Discrete and Continuous Planning using Shortest Walks in Graphs of Convex Sets

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

Preprint posted · arXiv (preprint) ↗

Robotics7 min text · CondensedOriginal paper ↗
Contents

At a glance

A graph of convex sets couples discrete mode choices with continuous optimization. Unlike a path, a walk may revisit a mode at a different continuous state, making repeated actions and variable-duration behavior natural to express. The paper builds quadratic lower bounds on remaining cost with semidefinite programming, then uses them to guide a fast approximate search.

Demonstrations cover acceleration-limited arm motion, rearrangement skills and simplified bipedal planning. Reported query speeds exclude substantial reusable preparation and use simulated parallel solver timing; the practical greedy search is neither complete nor globally optimal.

I Introduction

In an ordinary nonnegative-cost graph, deleting a cycle cannot worsen the route. In a graph of convex sets, revisiting the same vertex can select a new continuous point and make useful progress. This makes shortest walks materially different from shortest paths.

The method extends cost-to-go-guided GCS search: synthesize one convex quadratic lower bound per vertex, then choose successive transitions using those bounds. The formulation is general, while the numerical method imposes convex/quadratic structure and trades guarantees for speed.

Refer to caption
Fig. 1: Application of the SWP in GCS to a variety of robotics planning problems: collision-free motion planning for a robot arm with derivative constraints, item sorting for a top-down suction gripper, and footstep planning for a humanoid robot.

II Shortest Walks in Graphs of Convex Sets

A solution chooses the number of steps, the sequence of discrete vertices and a feasible continuous trajectory through them. Repetition is allowed and can improve cost.

II-A Preliminaries

Vertices carry local convex optimization problems; edges specify which consecutive continuous decisions can coexist.

Graph of Convex Sets (GCS)

Each vertex has a compact convex feasible set and a nonnegative convex cost. Each directed edge has a convex coupling set contained in the product of its endpoint sets, plus a nonnegative convex transition cost. Visiting a vertex selects a point in its set; crossing an edge must satisfy its coupling constraint.

Walk in a GCS

A K-step walk contains K+1 vertex visits and continuous points, with fixed source/target points and feasible consecutive edges. Repeated visits may select different continuous points. Total cost sums every visited vertex cost and every transition cost, including repeated visits.

Equations 1a–1d
v0=s,vK=t,\displaystyle v_{0}=s,\;v_{K}=t, (1a)
x0=x¯s,xK=x¯t,\displaystyle x_{0}=\bar{x}_{s},\;x_{K}=\bar{x}_{t}, (1b)
ek=(vk−1,vk)∈ℰ,\displaystyle e_{k}=(v_{k-1},v_{k})\in\mathcal{E},\quad ∀k=1,…,K,\displaystyle\forall k=1,\ldots,K, (1c)
(xk−1,xk)∈𝒳ek,\displaystyle(x_{k-1},x_{k})\in\mathcal{X}_{e_{k}}, ∀k=1,…,K.\displaystyle\forall k=1,\ldots,K. (1d)
l⁡(w,τ)=∑k=0Klvk​(xk)+∑k=1Klek​(xk−1,xk).l(w,\tau)=\sum_{k=0}^{K}l_{v_{k}}(x_{k})+\sum_{k=1}^{K}l_{e_{k}}(x_{k-1},x_{k}).

II-B Shortest-walk problem

The inner optimization finds the best feasible discrete/continuous walk of a fixed length; the outer infimum ranges over all lengths. This avoids choosing a fixed horizon in the problem definition, although the search implementation uses finite lookahead.

Equations 2a–2b
infKminw,τ\displaystyle\inf_{K}\;\min_{w,\tau}\quad l⁡(w,τ)\displaystyle l(w,\tau) (2a)
s.t.\displaystyle\mathrm{s.t.}\quad w∈𝒲s,tK,τ∈𝒯w​(x¯s,x¯t).\displaystyle w\in\mathcal{W}^{K}_{s,t},\quad\tau\in\mathcal{T}_{w}(\bar{x}_{s},\bar{x}_{t}). (2b)
Original figure2a diagram.
(a) A GCS embedded in ℝ2\mathbb{R}^{2}.
Original figure2b diagram.
(b) The shortest path between vertices ss and tt (orange) costs 35.
Original figure2c diagram.
(c) The shortest walk between vertices ss and tt (blue) costs 28. Vertices aa and bb are both revisited along the walk: aa is visited three times consecutively, and bb is visited twice non-consecutively (note that unlike vertex aa, there is no edge from vertex bb to itself).
Fig. 2: A GCS where a shortest walk is not a path.

II-C Shortest walks need not be paths

Figure 2 provides a counterexample to removing cycles: each edge costs one plus squared displacement. Revisiting vertices permits smaller, cheaper movements. The best illustrated walk costs 28, while the best path costs 35. Repetition changes the continuous state, so it is not wasted travel.

II-D Sufficient condition for finiteness of shortest walks

Without a strictly positive per-step charge, infinitely many smaller steps can approach an infimum that no finite walk attains. A positive minimum cost on every edge is a sufficient condition excluding an infinite-length finite-cost optimum. Adding epsilon to each edge enforces it but also changes the planning objective. The discussion assumes a feasible optimum exists.

Equation
min⁡{le​(xu,xv)∣(xu,xv)∈𝒳e}>0,\min\{l_{e}(x_{u},x_{v})\mid(x_{u},x_{v})\in\mathcal{X}_{e}\}>0,

III Applications in robotics and control

Walks are useful when repeated local trajectories, skills or hybrid modes are necessary. Paths remain appropriate when repetition is forbidden or unnecessary.

III-A Collision-free motion planning with acceleration limits

Standard GCS motion planning assigns a collision-free convex region to each vertex and a Bézier segment to each visit. Jointly optimizing segment shape and duration makes acceleration constraints nonconvex, motivating time rescaling or local nonlinear post-processing.

Fixing each segment’s duration makes derivative constraints convex. A walk can spend more time in a region by revisiting it, without duplicating vertices. This gives a compact formulation, but does not eliminate time discretization: very small fixed durations can create long walks. Region construction is a separate prerequisite.

Original figure3a diagram.
(a) The shortest-path formulation in [35] jointly searches for a Bézier curve and its duration per vertex, resulting in non-convex acceleration constraints.
Original figure3b diagram.
(b) Our shortest-walk formulation searches for a fixed-duration Bézier curve per vertex visit, resulting in convex acceleration constraints. Above, the same vertex is revisited 3 times.
Fig. 3: Contrasting the SPP and SWP formulations for the trajectory planning problem from Section III-A. Shown in blue are cubic Bézier curves, with their control points in orange.

III-B Skill chaining

Represent a skill by the set of feasible start/end configurations and a transition cost. A vertex visit executes one skill; edge constraints equate the preceding end configuration with the following start. Source and target vertices impose the task endpoints.

The shortest walk then jointly selects a skill sequence and continuous transition parameters. Exact equivalence requires convex skill transition sets and costs; otherwise convex approximations or decompositions must be supplied, potentially with considerable manual effort.

III-C Optimal control for hybrid systems

For a piecewise-affine system, create a vertex for each mode’s state/control set and impose its affine dynamics as edge constraints. Vertex costs are stage costs; revisiting a mode corresponds to spending additional time there or returning later. The soft-wall pendulum illustrates two contact modes.

The construction applies directly to the stated convex piecewise-affine model. Approximating nonlinear or contact-rich physics is an additional modeling step, not a guarantee that the resulting plan remains exact for the original system.

Original figure4a diagram.
(a)
Original figure4b diagram.
(b)
Original figure4c diagram.
(c)
Fig. 4: An actuated pendulum with a soft wall (a) can be approximated as a PWA system with two modes (no-contact and contact), which can be modeled as a GCS with two vertices and four edges (b). An optimal state-space trajectory for regulating the pendulum to the equilibrium position st=(0,0)s_{t}=(0,0) can be computed by solving a SWP in GCS, shown in (c).
Equation
sn+1=Ai​sn+Bi​an+ci, if ​sn∈𝒮i,an∈𝒜.\displaystyle s_{n+1}=A_{i}s_{n}+B_{i}a_{n}+c_{i},\text{ if }s_{n}\in\mathcal{S}_{i},\;a_{n}\in\mathcal{A}.

IV Solution method

Shortest walks remain NP-hard: on an acyclic graph they coincide with the already-hard shortest-path problem. Increasing-horizon layered copies provide a conceptual solution but enlarge the optimization repeatedly.

The proposed alternative works on the original graph. It synthesizes lower bounds through Bellman inequalities, then uses those bounds as a search heuristic. The bound representation scales with graph size rather than an explicitly unrolled horizon, although query complexity still depends on branching and walk length.

Refer to caption
(a)
Refer to caption
(b)
Fig. 5: A naive approach to solving the SWP in GCS in (a) involves constructing a layered GCS in (b) by duplicating vertices across K−1K-1 layers, for each K→∞K\to\infty. In contrast, our approach operates directly on the original, smaller GCS in (a), and avoids artificially increasing the size of the problem.

IV-A Bellman equation

An optimal walk has optimal subwalks. Its cost-to-go equals the current vertex cost plus the best feasible edge cost and successor cost-to-go, minimizing over both the next discrete vertex and continuous point. The successor may be the same vertex.

Equation 3
Ju∗​(xu)=minxv,v\displaystyle J_{u}^{*}(x_{u})\;\;=\;\;\min_{x_{v},\,v} lu​(xu)+le​(xu,xv)+Jv∗​(xv)\displaystyle l_{u}(x_{u})+l_{e}(x_{u},x_{v})+J_{v}^{*}(x_{v}) (3)
s.t.\displaystyle\text{s.t.} e=(u,v)∈ℰ,\displaystyle e=(u,v)\in\mathcal{E},
(xu,xv)∈𝒳e.\displaystyle(x_{u},x_{v})\in\mathcal{X}_{e}.

IV-B Synthesis of cost-to-go lower bounds

Replace Bellman equality with an inequality and fix the target value to its terminal vertex cost. Any feasible family of functions lower-bounds the optimal remaining cost. Maximizing the source value gives the exact source cost in the unrestricted infinite-dimensional formulation.

Objectives averaged over vertices/states, or extended to target states, support reusable bounds for multiple queries. The practical finite-dimensional approximation retains a lower-bound interpretation but generally loses tightness.

Equations 4a–4d
Ju​(xu)≤lu​(xu)+le​(xu,xv)+Jv​(xv),\displaystyle J_{u}(x_{u})\leq l_{u}(x_{u})+l_{e}(x_{u},x_{v})+J_{v}(x_{v}),
max\displaystyle\max\quad Js​(x¯s)\displaystyle J_{s}(\bar{x}_{s}) (4a)
s.t. Jv:𝒳v→ℝ+,∀v∈𝒱,\displaystyle J_{v}:\mathcal{X}_{v}\rightarrow\mathbb{R}_{+},\hskip 86.78099pt\forall v\in\mathcal{V}, (4b)
Ju​(xu)≤lu​(xu)+le​(xu,xv)+Jv​(xv),\displaystyle J_{u}(x_{u})\leq l_{u}(x_{u})+l_{e}(x_{u},x_{v})+J_{v}(x_{v}), (4c)
∀e=(u,v)∈ℰ,\displaystyle\hskip 150.79968pt\forall e=(u,v)\in\mathcal{E},
∀(xu,xv)∈𝒳e,\displaystyle\hskip 150.79968pt\forall(x_{u},x_{v})\in\mathcal{X}_{e},
Jt​(x¯t)=lt​(x¯t).\displaystyle J_{t}(\bar{x}_{t})=l_{t}(\bar{x}_{t}). (4d)

IV-C Tractable finite-dimensional cost-to-go lower bounds

The numerical method restricts per-vertex bounds to convex quadratics, feasible sets to polyhedra/ellipsoid intersections and costs to convex quadratics or suitable approximations. Semidefinite constraints provide tractable nonnegativity certificates. Higher-degree sums-of-squares relaxations could be more expressive but cost more.

Bound synthesis is the expensive preparation stage, often seconds to over a minute. Reuse in a fixed environment can amortize that cost; the query-time results alone do not describe initial planning latency.

IV-D Extracting walks from the approximate cost-to-go

At each step, enumerate finite-horizon candidate walks, solve their continuous convex programs in parallel and select the best accumulated cost plus terminal lower bound. The final vertex cost is subtracted once to avoid double counting. Commit only the first transition and repeat, allowing early arrival at the target.

An infeasible step triggers backtracking, and a fixed iteration budget can terminate the search. These choices give no completeness or optimality guarantee. Afterward, reoptimize all continuous points along the selected walk and remove cycles only when reoptimization preserves feasibility and lowers cost.

Equations 5a–5c
min(w,τ)\displaystyle\underset{(w,\,\tau)}{\min} l⁡(w,τ)+Jvk+n​(xk+n)−lvk+n​(xk+n)\displaystyle\quad l(w,\tau)+J_{v_{k+n}}(x_{k+n})-l_{v_{k+n}}(x_{k+n}) (5a)
s.t. w∈𝒲vkn,\displaystyle\quad w\in\mathcal{W}^{n}_{v_{k}}, (5b)
τ∈𝒯w​(xk,xk+n).\displaystyle\quad\tau\in\mathcal{T}_{w}(x_{k},x_{k+n}). (5c)
Refer to caption
(a) SPP in GCS generates a velocity-limited trajectory of duration 2.21s, which violates acceleration limits (red).
Refer to caption
(b) TOPP post-processes the trajectory to satisfy the acceleration limits, increasing its duration to 2.57s.
Refer to caption
(c) SWP in GCS enforces acceleration limits during search, yielding a faster 2.25s trajectory that operates at the limit longer.
Fig. 6: Visual comparison of SPP in GCS with TOPP (a,b) and SWP in GCS (c) formulations. Both planners are tasked with finding an acceleration-limited trajectory from the top-left shelf to the right bin. The end-effector trajectory is red when the acceleration limit is violated for some joint, green when the acceleration is within 5% of the limit, yellow when the velocity is within 5% of the limit, and blue otherwise.

V Results

Experiments use Mosek for convex programs and SNOPT for the nonlinear baseline on an Apple M1 Max laptop with 16 GB RAM. The paper reports simulated parallelized solver times, assuming up to ten solves in parallel and prebuilt program binaries; these are not straightforward measured end-to-end wall-clock times.

V-A Collision-free motion planning with acceleration limits

Warehouse arm planning uses 16 convex regions and 54 edges. Region decomposition costs 100 seconds and reusable lower-bound synthesis another 45 seconds. Each walk visit uses a 125 ms cubic Bézier segment, enforcing acceleration limits during search.

Across 100 queries, reported average query time is 0.46 seconds: 1.5× faster than shortest paths with time-optimal path parameterization and 2.3× faster than the nonlinear post-processing baseline. Average trajectory durations differ by only 1–2%. In the pictured example, the walk takes 2.25 seconds versus 2.57 seconds after rescaling the path; the original 2.21-second path violates acceleration limits.

Higher derivative constraints can also fit the fixed-duration formulation, but jerk-limited performance is not separately evaluated. The displayed studies use virtual items and do not establish real-hardware execution reliability.

Original figure7 diagram.
Fig. 7: For collision-free planning scenario (Section V-A), we compare solve time and trajectory duration across 100 queries for shortest-paths with TOPP, shortest-paths with non-convex post-processing, and shortest-walk formulations. SWP is 1.5 times faster than SPP with TOPP, 2.3 times faster than SPP with non-convex (left). Trajectory durations are similar (right).
Refer to caption
1: grasp object 1.
Refer to caption
2: flip object 1.
Refer to caption
3: regrasp object 1.
Refer to caption
4: move object 1 to target region.
Refer to caption
5: grasp object 2.
Refer to caption
6: move object 2.
Refer to caption
7: regrasp object 2.
Refer to caption
8: flip object 2.
Refer to caption
9: task is complete.
Fig. 8: An 8-step plan where the top-down suction-cup arm is tasked with sorting three objects into the green target region.

V-B Skill chaining

A simplified suction gripper moves horizontally and rearranges three rectangular objects using movement, centered pick/place and corner-flipping skills. Convex decomposition yields 15 subskills plus endpoint vertices, giving 22 vertices and 120 edges. Each skill costs one plus arm displacement.

Bound synthesis takes 20 seconds; reported query times are 0.5–1 second with prebuilt programs. The illustrated eight-action plan temporarily moves an already-placed object to fit another. The claim that sampling methods would struggle is qualitative; no timed sampling baseline is provided. The text calls this example optimal without establishing a general optimality guarantee for its greedy solver.

Refer to caption
(a) A footstep plan across a set of stepping stones.
Refer to caption
(b) With one stone removed, the robot must take a longer detour.
Fig. 9: Visualizations of the footstep plans across stepping stones for the Atlas bipedal robot. The SWP in GCS jointly optimizes for safe footstep placement, contact forces, and centroidal dynamics, while maintaining stability enforced by the ZMP condition. Both plans are produced in under 330ms.

V-C Hybrid dynamics: footstep planning for a ZMP walker

The bipedal example uses a zero-moment-point model with zero vertical acceleration, zero angular acceleration and fixed foot orientation. Friction, support-polygon and foot-collision constraints define piecewise-affine contact modes; this is a simplified model, not full humanoid dynamics.

Two layouts produce graphs with 24 vertices/58 edges and 21 vertices/50 edges. Lower-bound synthesis takes 2.3 and 3.7 seconds; reported searches produce 10- and 16-step plans in 110 and 330 ms. Receding-horizon disturbance rejection is suggested, but not demonstrated by a disturbance experiment.

VI Limitations

Building convex collision regions, decomposing skills and constructing piecewise-affine dynamics can be costly and require manual tuning. Large semidefinite programs limit online use unless their bounds can be reused. Fast search depends on prebuilding and parallelization, and the reported parallel timing is simulated.

The work’s main contribution is a compact modeling language for repeated modes plus a useful heuristic. It does not remove NP-hardness, convex-modeling assumptions, finite-duration discretization or the practical search’s lack of completeness and global optimality.

Concept demonstration

Open image ↗