Literature
Display
← All papers

GCS*: Forward Heuristic Search on Implicit Graphs of Convex Sets

Shao Yuan Chew Chia · Rebecca H. Jiang · Bernhard Paus Graesdal · Leslie Pack Kaelbling · Russ Tedrake

· WAFR 2024 proceedings (2026 edition) ↗

Robotics12 min text · CondensedOriginal paper ↗
Contents

At a glance

GCS* extends heuristic graph search to graphs whose vertices contain continuous convex decision sets and whose edges couple those decisions. A cheapest path to a vertex need not reach the points needed to continue, so keeping only that path can lose both feasibility and optimality. GCS* instead keeps paths that reach some terminal point more cheaply, or that reach a previously unreachable point.

Conservative containment checks preserve completeness and, with a pointwise admissible heuristic and cost-based pruning, optimality. Faster sampled checks can wrongly prune useful paths; their guarantees are probabilistic or asymptotic. In three simplified planar-pushing tasks, even one sample per check works well. This is empirical evidence under a translation-only, frictionless model, not a finite-sample optimality guarantee or a physical-robot evaluation.

1 Introduction

A graph of convex sets separates discrete mode choices from continuous variables within each mode. Edge constraints enforce relationships such as trajectory continuity. Fixing a discrete path leaves a convex program, but choosing the path is generally NP-hard and enumerating every mode can be infeasible.

Unlike ordinary A*, a GCS solver cannot safely discard a path merely because another reaches the same vertex with lower scalar cost. Figures 2–3 show why: the paths may reach different continuous points, and only the more expensive one may permit continuation. Optimal substructure survives at the trajectory-point level. The formulation allows repeated vertices, because returning to a set at a different point may be useful.

GCS* uses an implicit successor operator and retains multiple paths per vertex. ReachesCheaper preserves any path that improves cost at some terminal point; ReachesNew retains only paths that expand terminal reachability. The latter trades optimality for more aggressive pruning. Figure 1 illustrates a 21.9-second sampled, inflated-heuristic solution on a nominal billion-vertex graph.

Figure 1: An ϵ\epsilon-suboptimal solution found in 21.9s by GCS* using sampling-based ReachesCheaper domination checks on the STACK planar pushing task. STACK is formulated as a GCS problem with approximately 1.3×1091.3\times 10^{9} vertices and up to 8.5×10178.5\times 10^{17} edges.
Original figure

Original figure

(a)
Original figure
(b)
Figure 2: An abstract discrete graph (a) and GCS (b), for example, mapping to the path planning problem for a hopping robot tasked to hop from ss to tt across polygonal stepping stones. Arrows are edges between vertices. Vertices are represented as black dots in (a) and via their (blue) convex sets in (b). White dots are trajectory points on the GCS. Edge costs are the Euclidean distances traversed between vertices. A discrete graph (a) has the optimal substructure property while a GCS (b) does not. In (a), the optimal path between ss and CC (red) is a sub-path of the optimal path between ss and tt (green), which goes through CC. In (b), the optimal path between ss and CC is not a sub-path of the optimal path between ss and tt, which goes through CC. However, each point on the green path’s trajectory is reached optimally on the green path.
Original figure
Figure 3: A contrived example of a GCS where points spanning edges (s,A)(s,A), (s,B)(s,B), and (C,t)(C,t) are constrained to be vertically aligned, and points spanning (A,C)(A,C) and (B,C)(B,C) horizontally aligned. For example, representing a path planning problem for a hopping robot that can only hop in axis-aligned directions. A* prunes [s,B,C][s,B,C] since it has higher cost-to-come than [s,A,C][s,A,C]. However, [s,A,C,t][s,A,C,t] is infeasible (red), while [s,B,C,t][s,B,C,t] is feasible (green).

2 Related Work

Sampling planners handle large spaces but face difficulties with coupled continuous constraints. Whole-problem mixed-integer convex optimization and GCS relaxations give strong continuous reasoning but require an explicit graph. Logic-geometric planning combines search and nonconvex optimization, whose local solves do not establish the same global guarantees.

IxG and IxG* also interleave graph search and optimization. The comparison here emphasizes jointly infeasible edge constraints and the cost of precomputing heuristics on an explicit graph. IxG can lose completeness in that setting; IxG* remains complete but may prune poorly when its preliminary search supplies no finite solution bound. GCS* replaces that dependence with path-domination checks.

3 Problem Formulation

Each vertex has a compact convex set, each edge has closed convex coupling constraints, and each vertex has finitely many successors. Edge costs are proper, closed, convex, positive, and bounded away from zero in the general analysis. The graph may be infinite and paths may revisit vertices.

Program 1 jointly chooses a vertex sequence and a continuous trajectory, minimizing summed edge costs under membership, successor, and edge constraints. Table 1 retains the notation. Once the vertex sequence is fixed, ConvexRestriction computes its feasible optimal trajectory. The later pushing implementation uses a zero-cost final edge into the terminal set as a special case; its ordinary mode transitions retain a positive unit penalty.

Table 1: Summary of mathematical notation
G:=(𝒱,ℰ)G:=(\mathcal{V},\mathcal{E}) Directed graph with vertices 𝒱\mathcal{V} and edges ℰ\mathcal{E}
𝒳v\mathcal{X}_{v} Convex set associated with vertex vv
𝒳e\mathcal{X}_{e} Convex constraint set for edge ee
s,ts,t Source and target vertices
𝐯\mathbf{v} Discrete path in the graph, defined as a sequence of vertices
𝐯i\mathbf{v}_{i} The ii-th vertex in the path 𝐯\mathbf{v}
𝐯end\mathbf{v}_{\text{end}} Last vertex in the path 𝐯\mathbf{v}
ℰ𝐯\mathcal{E}_{\mathbf{v}} Set of edges traversed by path 𝐯\mathbf{v}
𝐱\mathbf{x} Continuous trajectory
𝐱i\mathbf{x}_{i} The ii-th point on trajectory 𝐱\mathbf{x}
𝐱i∈𝒳v\mathbf{x}_{i}\in\mathcal{X}_{v} The point on trajectory 𝐱\mathbf{x} associated with vertex vv
c⁡(𝐱u,𝐱v)c(\mathbf{x}_{u},\mathbf{x}_{v}) Cost of the edge between vertices uu and vv, evaluated on trajectory 𝐱\mathbf{x}
f∗​(v)f^{*}(v) Optimal cost of a solution via vertex vv
g∗​(x)g^{*}(x) Optimal cost-to-come from ss to point xx
h∗​(x)h^{*}(x) Optimal cost-to-go from point xx to tt
f~​(𝐯)\tilde{f}(\mathbf{v}) Total cost estimate via path 𝐯\mathbf{v}
f~​(𝐯,x)\tilde{f}(\mathbf{v},x) Total cost estimate via path 𝐯\mathbf{v} and point x∈𝒳𝐯endx\in\mathcal{X}_{\mathbf{v}_{\text{end}}}
g~​(𝐯,x)\tilde{g}(\mathbf{v},x) Optimal cost-to-come from ss to point x∈𝒳𝐯endx\in\mathcal{X}_{\mathbf{v}_{\text{end}}} via path 𝐯\mathbf{v}
h~​(x)\tilde{h}(x) Heuristic estimate of cost-to-go from point xx to tt
𝐱∗​(𝐯)\mathbf{x}^{*}(\mathbf{v}) Optimal trajectory via path 𝐯\mathbf{v} subject to the heuristic h~\tilde{h}
Equations 1a–1e
minimize𝐯,𝐱\displaystyle\minimize_{\mathbf{v},\>\mathbf{x}}\quad ∑(u,v)∈ℰ𝐯c⁡(𝐱u,𝐱v)\displaystyle\sum_{(u,v)\in\mathcal{E}_{\mathbf{v}}}c(\mathbf{x}_{u},\mathbf{x}_{v}) (1a)
subject​to\displaystyle\subto\quad 𝐯0=s,𝐯end=t,\displaystyle\mathbf{v}_{0}=s,\>\mathbf{v}_{\text{end}}=t, (1b)
𝐯i+1∈Successors​(𝐯i),\displaystyle\mathbf{v}_{i+1}\in\textsc{Successors}(\mathbf{v}_{i}), ∀i∈[0,end−1],\displaystyle\forall i\in[0,\text{end}-1], (1c)
𝐱v∈𝒳v,\displaystyle\mathbf{x}_{v}\in\mathcal{X}_{v}, ∀v∈𝐯,\displaystyle\forall v\in\mathbf{v}, (1d)
(𝐱u,𝐱v)∈𝒳e,\displaystyle(\mathbf{x}_{u},\mathbf{x}_{v})\in\mathcal{X}_{e}, ∀e:=(u,v)∈ℰ𝐯.\displaystyle\forall e:=(u,v)\in\mathcal{E}_{\mathbf{v}}. (1e)

4 Approach

The search combines convex optimization of partial paths with either cost-based or reachability-based domination. Its queue priorities and pruning tests have different roles: a favorable priority alone does not justify discarding other ways to reach a continuous set.

4.1 Graph Search Formulation

Algorithms 1–2 contrast ordinary A* with GCS*. Both expand the lowest estimated total-cost path. Ordinary A* stores one best path per terminal vertex; GCS* stores a set of retained paths and adds a candidate only when NotDominated accepts it.

A partial path’s priority minimizes cost-to-come plus the heuristic over the points it can reach. Fixing a particular terminal point gives a more constrained value used in domination checks. A convex heuristic permits these quantities to be evaluated through ConvexRestriction. The heuristic is nonnegative; cost optimality additionally requires pointwise admissibility over each vertex set, as stated in Definition 1. The retained formulas distinguish optimal cost functions from computational estimates.

Equationf~​(𝐯):=g~​(𝐯,𝐱∗​(𝐯)end)+h~​(𝐱∗​(𝐯)end)\tilde{f}(\mathbf{v}):=\tilde{g}(\mathbf{v},\mathbf{x}^{*}(\mathbf{v})_{\text{end}})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{\text{end}})
Algorithm 1 Discrete A*
1: Input: ss, tt, f~\tilde{f}, g~\tilde{g}, Successors
2:
3: Output: Path from ss to tt or Fail
4: 𝐯=[s]\mathbf{v}=[s]
5: S←{s:𝐯}S\leftarrow\{s:\mathbf{v}\} ⊳\triangleright Map of paths
6: Q←priority queue ordered by ​f~​(𝐯)Q\leftarrow\text{priority queue ordered by }\tilde{f}(\mathbf{v})
7: Q.add​(𝐯)Q.\textsc{add}(\mathbf{v})
8: while Q≠∅Q\neq\emptyset do
9:    𝐯←\mathbf{v}\leftarrow Q.Pop​()Q.\textsc{Pop}()
10:    if 𝐯end\mathbf{v}_{\text{end}} = t then return 𝐯\mathbf{v}   
11:    for all v′∈Successors​(𝐯end)v^{\prime}\in\textsc{Successors}(\mathbf{v}_{\text{end}}) do
12:     𝐯′=[𝐯,v′]\mathbf{v}^{\prime}=[\mathbf{v},v^{\prime}]
13:     if v′∉Sv^{\prime}\notin S or g~​(𝐯′)<g~​(S⁡[v′])\tilde{g}(\mathbf{v}^{\prime})<\tilde{g}(S[v^{\prime}]) then
14:       S⁡[v′]=𝐯′S[v^{\prime}]=\mathbf{v}^{\prime}
15:       Q.Add​(𝐯′)Q.\textsc{Add}(\mathbf{v}^{\prime})      
16: return Fail
1: Input: ss, tt, f~\tilde{f}, NotDominated, Successors
2: Output: Path from ss to tt or Fail
3: 𝐯=[s]\mathbf{v}=[s]
4: S←{s:{𝐯}}S\leftarrow\{s:\{\mathbf{v}\}\} ⊳\triangleright Map of sets of paths
5: Q←priority queue ordered by ​f~​(𝐯)Q\leftarrow\text{priority queue ordered by }\tilde{f}(\mathbf{v})
6: Q.add​(𝐯)Q.\textsc{add}(\mathbf{v})
7: while Q≠∅Q\neq\emptyset do
8:    𝐯←\mathbf{v}\leftarrow Q.Pop​()Q.\textsc{Pop}()
9:    if 𝐯end\mathbf{v}_{\text{end}} = t then return 𝐯\mathbf{v}   
10:    for all v′∈Successors​(𝐯end)v^{\prime}\in\textsc{Successors}(\mathbf{v}_{\text{end}}) do
11:     𝐯′=[𝐯,v′]\mathbf{v}^{\prime}=[\mathbf{v},v^{\prime}]
12:     if NotDominated​(𝐯′,S⁡[v′])\textsc{NotDominated}(\mathbf{v}^{\prime},S[v^{\prime}]) then
13:       S⁡[v′].Add​(𝐯′)S[v^{\prime}].\textsc{Add}(\mathbf{v}^{\prime})
14:       Q.Add​(𝐯′)Q.\textsc{Add}(\mathbf{v}^{\prime})      
15: return Fail
Algorithm 2 GCS*
Equationf~​(𝐯,x):=g~​(𝐯,x)+h~​(x)\tilde{f}(\mathbf{v},x):=\tilde{g}(\mathbf{v},x)+\tilde{h}(x)

Definition 1

A heuristic function h~\tilde{h} is pointwise admissible if

Equation
h~​(x)≤h∗​(x),∀x∈𝒳v,∀v∈𝒱.\tilde{h}(x)\leq h^{*}(x),\quad\forall x\in\mathcal{X}_{v},\quad\forall v\in\mathcal{V}.

4.2 Domination Checks

ReachesCheaper is true when some reachable terminal point costs strictly less through the candidate than through every retained alternative. ReachesNew is true when the candidate reaches a point none of those alternatives can reach. New reachability implies cheaper reachability, but a cheaper path can cover only already reachable points.

A conservative check may retain unnecessary paths but never falsely declares a useful candidate dominated. Conservative ReachesNew preserves the existence of feasible continuations; conservative ReachesCheaper also preserves the paths needed for an optimum. Figure 4 distinguishes domination by one path from domination by several paths collectively.

Original figure
(a) RC=False,RN=False\textsc{RC}=\texttt{False},\>\textsc{RN}=\texttt{False}
Original figure
(b) RC=True,RN=True\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{True}
Original figure
(c) RC=True,RN=False\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{False}
Original figure
(d) RC=True,RN=False\textsc{RC}=\texttt{True},\>\textsc{RN}=\texttt{False}
Original figure
(e) RC=False,RN=False\textsc{RC}=\texttt{False},\>\textsc{RN}=\texttt{False}

Original figure

Figure 4: Scenarios showing whether a candidate path 𝐯\mathbf{v} (pink) ReachesCheaper (RC) and ReachesNew (RN) compared to alternate paths {𝐯(i)|i∈{1,2,3}}\{\mathbf{v}^{(i)}|i\in\{1,2,3\}\} (blue), where the dimension of the set 𝒳𝐯end\mathcal{X}_{\mathbf{v}_{\text{end}}} is 1. Green and orange lines denote points xx such that eq. 2 and eq. 3 hold, respectively. Points xx for which the cost-to-come g~​(𝐯,x)\tilde{g}(\mathbf{v},x) is not drawn (outside dashed lines) are unreachable via path 𝐯\mathbf{v} due to constraints, and g~​(𝐯,x)=∞\tilde{g}(\mathbf{v},x)=\infty.
Equations 2–3
ReachesCheaper​(\displaystyle\textsc{ReachesCheaper}( 𝐯,S⁡[𝐯end]):=∃x∈𝒳𝐯end\displaystyle\mathbf{v},S[\mathbf{v}_{\text{end}}])\>:=\>\exists\,x\in\mathcal{X}_{\mathbf{v}_{\text{end}}} (2)
s.t.g~(𝐯,x)<g~(𝐯′,x),∀𝐯′∈S[𝐯end].\displaystyle\text{s.t.}\>\tilde{g}(\mathbf{v},x)<\tilde{g}(\mathbf{v}^{\prime},x),\>\forall\,\mathbf{v}^{\prime}\in S[\mathbf{v}_{\text{end}}].
ReachesNew​(\displaystyle\textsc{ReachesNew}( 𝐯,S⁡[𝐯end]):=∃x∈𝒳𝐯end\displaystyle\mathbf{v},S[\mathbf{v}_{\text{end}}])\>:=\>\exists\,x\in\mathcal{X}_{\mathbf{v}_{\text{end}}} (3)
s.t.(g~(𝐯,x)<∞)∧(g~(𝐯′,x)=∞,∀𝐯′∈S[𝐯end]).\displaystyle\text{s.t.}\>\left(\tilde{g}(\mathbf{v},x)<\infty\right)\>\wedge\>\left(\tilde{g}(\mathbf{v}^{\prime},x)=\infty,\>\forall\,\mathbf{v}^{\prime}\in S[\mathbf{v}_{\text{end}}]\right).

Sampling-based Approximation.

Sampling tests the defining conditions at selected terminal points. Finding one witness proves that the candidate should be retained. Failing to find a witness does not prove domination: a small region where the candidate is useful may be missed. These checks can therefore discard a necessary or cheaper path at any finite sample budget.

Polyhedral-containment-based Approximation.

The reachable set is the projection of all feasible trajectories onto the terminal variables. Cost domination uses the corresponding epigraph of optimal cost-to-come. Exact pruning asks whether the candidate’s set lies inside the union of the alternatives’ sets, which is generally nonconvex and difficult.

The implementation instead tests containment by one alternative at a time and uses a sufficient polyhedral-containment certificate. Successful containment safely prunes a candidate; failure can retain a dominated candidate. This conservatism preserves the relevant guarantees but increases work. The tested polyhedral sets and linear costs fit this certificate directly; more general convex models require additional approximation machinery. Equations 6a–b, absent from the HTML, are restored from the matching PDF.

Equations 4a–5b
𝒫𝐯:={𝐱∣𝐱v∈𝒳v∀v∈𝐯,(𝐱u,𝐱v)∈𝒳(u,v)∀(u,v)∈ℰ𝐯}.\mathcal{P}_{\mathbf{v}}:=\{\mathbf{x}\mid\mathbf{x}_{v}\in\mathcal{X}_{v}\ \forall\,v\in\mathbf{v},\>(\mathbf{x}_{u},\mathbf{x}_{v})\in\mathcal{X}_{(u,v)}\ \forall\,(u,v)\in\mathcal{E}_{\mathbf{v}}\}. (4a)
𝒮𝐯:={x∈𝒳𝐯end∣∃𝐱∈𝒫𝐯​ s.t. ​x=𝐱end}⊆𝒳𝐯end.\mathcal{S}_{\mathbf{v}}:=\{x\in\mathcal{X}_{\mathbf{v}_{\text{end}}}\mid\exists\,\mathbf{x}\in\mathcal{P}_{\mathbf{v}}\text{ s.t. }x=\mathbf{x}_{\text{end}}\}\subseteq\mathcal{X}_{\mathbf{v}_{\text{end}}}. (4b)
𝒞𝐯:={(x,l)∈𝒳𝐯end×ℝ∣∃𝐱∈𝒫𝐯 s.t. x=𝐱end,l≥∑(u,v)∈ℰ𝐯c(𝐱u,𝐱v)}.\mathcal{C}_{\mathbf{v}}:=\{(x,\,l)\in\mathcal{X}_{\mathbf{v}_{\text{end}}}\times\mathbb{R}\mid\exists\,\mathbf{x}\in\mathcal{P}_{\mathbf{v}}\text{ s.t. }x=\mathbf{x}_{\text{end}},\>l\geq\sum_{(u,v)\in\mathcal{E}_{\mathbf{v}}}c(\mathbf{x}_{u},\mathbf{x}_{v})\}. (4c)
ReachesCheaper​(𝐯,S⁡[𝐯end])=(𝒞𝐯⊈⋃𝐯′∈S⁡[𝐯end]𝒞𝐯′),\textsc{ReachesCheaper}(\mathbf{v},S[\mathbf{v}_{\text{end}}])=\left(\mathcal{C}_{\mathbf{v}}\nsubseteq\bigcup_{\mathbf{v}^{\prime}\in S[\mathbf{v}_{\text{end}}]}\mathcal{C}_{\mathbf{v}^{\prime}}\right), (5a)
ReachesNew​(𝐯,S⁡[𝐯end])=(𝒮𝐯⊈⋃𝐯′∈S⁡[𝐯end]𝒮𝐯′).\textsc{ReachesNew}(\mathbf{v},S[\mathbf{v}_{\text{end}}])=\left(\mathcal{S}_{\mathbf{v}}\nsubseteq\bigcup_{\mathbf{v}^{\prime}\in S[\mathbf{v}_{\text{end}}]}\mathcal{S}_{\mathbf{v}^{\prime}}\right). (5b)
Equations 6a and 6b: no individual alternate path contains the candidate cost epigraph or reachable set.
Equations 6a–b: single-domination conditions.

4.3 Properties of the Algorithm

Under conservative domination checks, finite branching, the cost assumptions, and suitable priorities, a feasible solution is reached in finitely many iterations. With conservative ReachesCheaper and a pointwise admissible heuristic, the returned solution is cost optimal. The exact theorem statements are retained below.

An infeasible problem can still run forever when the graph is infinite or cycles are allowed. A maximum path length restores finite failure detection for that bounded problem. Inflating an admissible heuristic trades speed for bounded suboptimality under the corresponding conservative cost-based search; finite sampled pruning does not provide the same deterministic bound.

Theorem 4.1

GCS* returns a path from ss to tt in finite iterations if one exists.

Theorem 4.2

GCS* is cost optimal: GCS* returns an optimal trajectory from ss to tt in finite iterations if one exists.

Efficiency.

GCS* is not optimally efficient in the A* sense. Expanding a partial path reveals stronger information about future cost, but this implementation does not use that information to update its heuristic. Multiple paths ending in the same set can consequently be expanded even when a more sophisticated method might avoid them.

Properties when using sampling-based domination checks.

The sampling argument takes the number of samples per domination check to infinity. The authors reason that an uncovered region is relatively open within the reachable set and, when nonempty with positive sampling probability, is eventually encountered. Thus completeness and cost optimality are limiting properties under the relevant sampling and heuristic assumptions.

This statement concerns increasing coverage per check; simply allowing a run with a fixed one-sample pruning rule to continue does not repair a path already discarded. The experiments use projected samples, and their strong finite-sample performance is a property observed in these tasks.

5 Application to Planar Pushing

Planar pushing provides an intentionally combinatorial test: each pair of bodies can have different contact and separation modes. The authors aim to demonstrate search over a huge implicit graph, rather than establish the strongest possible manipulation planner. Hierarchy or more careful mode enumeration could reduce this particular search space.

5.1 Formulation

Polyhedral robots and objects translate without rotation among static obstacles. Contacts are frictionless, and quasi-static velocities are proportional to net forces. A vertex stores two position knots for each moving body, robot actuation forces, and contact-force magnitudes. Changing one pair’s contact state or separating face generates successors on demand.

All tested constraints are linear. Rotations, frictional contact, and more general shapes would need a richer formulation or relaxation. An approximate free-space decomposition may miss contact boundaries or be expensive to generate online, so the simple successor construction does not automatically transfer to arbitrary robot configuration spaces.

Non-penetration.

For each body pair, one polyhedral face acts as a separating hyperplane. Requiring the bodies to remain on opposite sides enforces non-penetration within that convex mode.

Contact.

A body pair may be separated, in face–face contact, or in face–vertex contact. Each active contact adds a normal force and position constraints. Sliding along the contacting features is allowed while contact remains feasible; tangential friction forces are omitted.

Quasi-static dynamics.

Between consecutive knots, each body’s translation is proportional to its summed forces, including actuation for robots. Edge constraints equate the preceding mode’s final positions with the next mode’s initial positions. Feasibility therefore depends on a common continuous configuration, not only a legal sequence of mode labels.

5.2 Implementation Details

Two knots per mode give an L1 displacement cost with unit weights for all moving bodies, plus a unit mode-switch penalty. The terminal edge costs zero. The shortcut heuristic ignores contact dynamics and downweights robot displacement to 0.2; an additional mode penalty accounts for at least one further switch when no direct target edge exists. Faster runs inflate this heuristic by ε = 10.

Each sampled domination check uses one point, drawn in the terminal set and projected onto the path’s reachable set. Containment checks first try this cheap witness and run the certificate only if it fails. The implementation exploits this model’s structure by comparing final position knots rather than every force variable, and eliminates equality constraints through a null-space parameterization. These optimizations are specific to the modeled dynamics.

Equationsc⁡(𝐱u,𝐱v):=1+∑iwi​‖pv,1i−pv,0i‖1c(\mathbf{x}_{u},\mathbf{x}_{v}):=1+\sum_{i}w_{i}||p^{i}_{v,1}-p^{i}_{v,0}||_{1}c⁡(𝐱u,t)=0c(\mathbf{x}_{u},t)=0h~​(x)←1+c⁡(x,xt)\tilde{h}(x)\leftarrow 1+c(x,x_{t})h~​(x)←ϵ⋅(1+c⁡(x,xt))\tilde{h}(x)\leftarrow\epsilon\cdot(1+c(x,x_{t}))

6 Results

AROUND has 194 vertices and 8,328 edges; SQUEEZE has 628 vertices and 56,806 edges. STACK has five bodies, approximately 1.3 billion vertices, and up to 8.5×10¹⁷ edges. Set dimensions range from 4–12 in the smaller tasks and 8–24 in STACK. Table 2 reports runtime, cost, and expanded paths on an AMD Ryzen 9 7950X; the paper does not report repeated-run uncertainty.

Sampled ReachesCheaper with the admissible heuristic solves AROUND in 26.2 seconds at cost 27.5 and SQUEEZE in four minutes at 47.80; STACK reaches the ten-hour cutoff. With ε = 10, the same check solves all three in 2.9, 38.3, and 21.9 seconds, with costs 27.5, 54.55, and 67.53. Thus the fastest SQUEEZE result sacrifices cost, and STACK’s fast finite-sample result is not a demonstrated exact optimum.

Sampled ReachesNew takes 2.5, 20, and 21.8 seconds, with SQUEEZE cost increasing to 57.42. Conservative containment preserves stronger guarantees but scales poorly: admissible ReachesCheaper takes 7.5 hours even on AROUND. Conservative ReachesNew is faster than the cost-based certificate, solving STACK in 3.5 minutes.

IxG requires 1.1 minutes of preprocessing for AROUND and 1.7 hours for SQUEEZE before the query times shown. It solves AROUND but fails on SQUEEZE; IxG* reaches the ten-hour cutoff in these tests. Neither baseline can construct STACK’s graph for its required heuristic preprocessing. These comparisons support the benefit of implicit, query-focused search on the chosen formulation.

The authors attribute effective one-sample pruning to the restrictive final object/robot configurations in these problems. That explanation and three task results do not establish comparable coverage in arbitrary high-dimensional graphs. Figures 1 and 5 retain all trajectory snapshots; videos are linked from the project page.

(a)
Original figure(a)
(b)
Original figure(b)
Figure 5: The ϵ\epsilon-suboptimal solution trajectories found by GCS* using the sampling-based ReachesCheaper domination check on the AROUND Task (a) and SQUEEZE Task (b). In SQUEEZE, in the second frame, the robot pushes the object past the obstacle to make room for itself to maneuver around in the third frame. In the fourth, the robot uses contact with the obstacle to slide the object vertically until the robot can fit underneath it. Videos of these trajectories are available on the project website.
Table 2: Here we show the solve time, solution cost, and number of expanded paths (Exp) for three planar pushing tasks. Our algorithm GCS* is compared to the baselines IxG* and IxG (grey). Sampling (Sampl.) and containment (Cont.) based implementations (Impl.) of the domination checkers (DC) ReachesCheaper (RC) and ReachesNew (RN) are shown. A dagger (†\dagger) indicates that the property holds probabilistically or asymptotically. Some experiments returned infeasible (Fail), were stopped after 10h (×\times), or could not be run as the graph could not be explicitly constructed (-). The baselines required preprocessing times of 1.1m and 1.7h for AROUND and SQUEEZE respectively. Our methods require no preprocessing time. All computation was performed on an AMD Ryzen 9 7950x 16-core 32-thread processor.
Alg. DC Impl. Heur. Com- plete Cost optimal AROUND SQUEEZE STACK
Time Cost Exp Time Cost Exp Time Cost Exp
GCS* RC Sampl. Admiss. Yes† Opt.† 26.2s 27.5 432 4m 47.80 2114 ×\times ×\times ×\times
GCS* RC Cont. Admiss. Yes Opt. 7.5h 27.5 734 ×\times ×\times ×\times ×\times ×\times ×\times
IxG* N.A. N.A. Admiss. Yes Opt. ×\times ×\times ×\times ×\times ×\times ×\times - - -
GCS* RC Sampl. Inadm. Yes† ϵ\epsilon-subopt.† 2.9s 27.5 51 38.3s 54.55 381 21.9s 67.53 57
GCS* RC Cont. Inadm. Yes ϵ\epsilon-subopt. 4.6m 27.5 73 ×\times ×\times ×\times ×\times ×\times ×\times
IxG* N.A. N.A. Inadm. Yes ϵ\epsilon-subopt. ×\times ×\times ×\times ×\times ×\times ×\times - - -
GCS* RN Sampl. Inadm. Yes† Subopt. 2.5s 27.5 44 20s 57.42 259 21.8s 67.53 57
GCS* RN Cont. Inadm. Yes Subopt. 35.4s 28.5 75 20.5m 57.42 458 3.5m 67.53 64
IxG N.A. N.A. Inadm. No Subopt. 4.7s 28 31 2.3m Fail 615 - - -

7 Conclusion

GCS* makes forward search compatible with continuous sets by testing terminal reachability and pointwise cost, rather than one scalar cost per vertex. It offers conservative guarantees or faster sampling approximations, with a substantial gap in practical runtime between them. Richer contact dynamics, rotations, stronger heuristics, hierarchy, and factorization remain future directions.

Acknowledgements

Support came from the Aker Scholarship, Amazon, The AI Institute, the Army Research Office, and the Charles Stark Draper Laboratory. Rebecca H. Jiang is a Draper Scholar.

Disclosure of Interests.

The authors declare no competing interests relevant to the article.

Appendix 0.A Proofs of Algorithmic properties

Appendix A proves feasible-case completeness for conservative reachability-preserving checks and optimality for conservative ReachesCheaper with a pointwise admissible heuristic. The exact statements are retained; the proofs below give their main argument. Infeasible-case termination requires the path-length bound described in the main text.

Completeness.

Lemma 1 establishes eventual queue expansion: positive transition-cost bounds and finite branching leave only finitely many paths below a fixed finite priority. A queued feasible path is therefore expanded eventually, unless a solution is returned first.

Lemma 2 follows a feasible trajectory to its furthest point reached by any queued path. Expanding a path reaching that point must either add a continuation or find that an existing path already reaches the next point. Conservative pruning cannot remove every continuation. Combining the two lemmas gives finite discovery and return of a feasible target path.

Lemma 1

For any path 𝐯\mathbf{v} in QQ, GCS* either expands 𝐯\mathbf{v} after a finite number of iterations or returns a feasible path to the target first.

Lemma 2

If target tt is reachable from source ss, then some path that reaches it is added to QQ in finite iterations.

Theorem 0.A.1

GCS* returns a path from ss to tt in finite iterations if one exists.

Cost-optimality.

Lemma 3 follows an optimal trajectory. Before reaching the target, the queue retains a path attaining optimal cost to some point on it: if an extension is pruned, an existing path must already reach that point at least as cheaply. Pointwise admissibility then bounds some queued priority by the global optimum, as shown in the retained inequality chain.

The final contradiction is the A* argument at the point level. At a target, the nonnegative admissible heuristic is zero. Returning a more expensive target would require popping it ahead of the queued path whose estimated cost does not exceed the optimum, contradicting the queue order. This proof depends on conservative cost pruning and does not directly certify the one-sample experimental variant.

Lemma 3

Suppose prog. (1) is feasible and 𝐯\mathbf{v} is an optimal path from ss to tt. If tt is unexpanded, there is a path 𝐯′\mathbf{v}^{\prime} in QQ with 𝐯end′=𝐯i\mathbf{v}^{\prime}_{\text{end}}=\mathbf{v}_{i} for some ii, such that 𝐯′\mathbf{v}^{\prime} attains the optimal cost-to-come to 𝐱∗​(𝐯)i\mathbf{x}^{*}(\mathbf{v})_{i}, g~​(𝐯′,𝐱∗​(𝐯)i)=g∗​(𝐱∗​(𝐯)i)\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})=g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i}).

Corollary 1

Suppose prog. (1) is feasible, and 𝐯\mathbf{v} is an optimal path from ss to tt. If algorithm 2 has not terminated, there is some 𝐯′\mathbf{v}^{\prime} in QQ such that 𝐯end′∈𝐯\mathbf{v}^{\prime}_{\text{end}}\in\mathbf{v} and the estimated total cost for 𝐯′\mathbf{v}^{\prime} does not exceed the true optimal cost, f~​(𝐯′)≤f∗​(s)\tilde{f}(\mathbf{v}^{\prime})\leq f^{*}(s).

Theorem 0.A.2

GCS* is cost optimal: GCS* returns an optimal trajectory from ss to tt in finite iterations if one exists.

Equation
f~​(𝐯′,𝐱∗​(𝐯)i)\displaystyle\tilde{f}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i}) =g~(𝐯′,𝐱∗(𝐯)i)+h~(𝐱∗(𝐯)i)(By definition of f~)\displaystyle=\tilde{g}(\mathbf{v}^{\prime},\mathbf{x}^{*}(\mathbf{v})_{i})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By definition of $\tilde{f}$)}
=g∗(𝐱∗(𝐯)i)+h~(𝐱∗(𝐯)i)(By lemma 3)\displaystyle=g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})+\tilde{h}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By lemma \ref{lemma:Q_has_node_with_optimal_cost_to_come})}
≤g∗(𝐱∗(𝐯)i)+h∗(𝐱∗(𝐯)i)(By h~ pointwise admissibility)\displaystyle\leq g^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})+{h}^{*}(\mathbf{x}^{*}(\mathbf{v})_{i})\quad\textnormal{(By $\tilde{h}$ pointwise admissibility)}
=f∗(s)(Since 𝐱∗​(𝐯)i is on an optimal trajectory).\displaystyle=f^{*}(s)\quad\textnormal{(Since $\mathbf{x}^{*}(\mathbf{v})_{i}$ is on an optimal trajectory).}
Concept demonstration

Open image ↗