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

TL;DR

This work tackles shortest-path problems on Graphs of Convex Sets (GCS), where each vertex encodes a discrete choice and a convex set of continuous decisions, making the problem NP-hard and lacking path-level optimal substructure. It introduces GCS*, a forward, best-first search that generalizes A* to the GCS setting by employing two domination checks, ReachesCheaper and ReachesNew, to prune paths while preserving cost-optimality and completeness. GCS* offers two implementation flavors: a complete, cost-optimal version using polyhedral-containment-based domination checks, and a probabilistically complete, asymptotically cost-optimal version using sampling-based checks. The approach is demonstrated on planar pushing tasks with large, implicitly defined graphs (e.g., STACK with about $1.3\times 10^9$ vertex-sets and up to $8.5\times 10^{17}$ edges), where GCS* achieves competitive or superior performance to state-of-the-art methods without full graph construction, highlighting its scalability and practicality for contact-rich planning problems.

Abstract

We consider large-scale, implicit-search-based solutions to Shortest Path Problems on Graphs of Convex Sets (GCS). We propose GCS*, a forward heuristic search algorithm that generalizes A* search to the GCS setting, where a continuous-valued decision is made at each graph vertex, and constraints across graph edges couple these decisions, influencing costs and feasibility. Such mixed discrete-continuous planning is needed in many domains, including motion planning around obstacles and planning through contact. This setting provides a unique challenge for best-first search algorithms: the cost and feasibility of a path depend on continuous-valued points chosen along the entire path. We show that by pruning paths that are cost-dominated over their entire terminal vertex, GCS* can search efficiently while still guaranteeing cost-optimality and completeness. To find satisficing solutions quickly, we also present a complete but suboptimal variation, pruning instead reachability-dominated paths. We implement these checks using polyhedral-containment or sampling-based methods. The former implementation is complete and cost-optimal, while the latter is probabilistically complete and asymptotically cost-optimal and performs effectively even with minimal samples in practice. We demonstrate GCS* on planar pushing tasks where the combinatorial explosion of contact modes renders prior methods intractable and show it performs favorably compared to the state-of-the-art. Project website: https://shaoyuan.cc/research/gcs-star/

Figures (5)

• Figure 1: An $\epsilon$-suboptimal solution found in 21.9s by GCS* using sampling-based $\textsc{ReachesCheaper}$ domination checks on the STACK planar pushing task. STACK is formulated as a GCS problem with approximately $1.3 \times 10^{9}$ vertices and up to $8.5 \times 10^{17}$ edges.
• 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 $s$ to $t$ 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 $s$ and $C$ (red) is a sub-path of the optimal path between $s$ and $t$ (green), which goes through $C$. In (b), the optimal path between $s$ and $C$ is not a sub-path of the optimal path between $s$ and $t$, which goes through $C$. However, each point on the green path's trajectory is reached optimally on the green path.
• Figure 3: A contrived example of a GCS where points spanning edges $(s,A)$, $(s,B)$, and $(C,t)$ are constrained to be vertically aligned, and points spanning $(A,C)$ and $(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]$ since it has higher cost-to-come than $[s,A,C]$. However, $[s,A,C,t]$ is infeasible (red), while $[s,B,C,t]$ is feasible (green).
• Figure 4: Scenarios showing whether a candidate path $\mathbf{v}$ (pink) ReachesCheaper (RC) and ReachesNew (RN) compared to alternate paths $\{\mathbf{v}^{(i)} | i \in \{1,2,3\}\}$ (blue), where the dimension of the set $\mathcal{X}_{\mathbf{v}_{\text{end}}}$ is 1. Green and orange lines denote points $x$ such that \ref{['eqn:reaches_cheaper']} and \ref{['eqn:reaches_new']} hold, respectively. Points $x$ for which the cost-to-come $\tilde{g}(\mathbf{v}, x)$ is not drawn (outside dashed lines) are unreachable via path $\mathbf{v}$ due to constraints, and $\tilde{g}(\mathbf{v}, x)=\infty$.
• 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 https://shaoyuan.cc/research/gcs-star/.

Theorems & Definitions (15)

• definition thmcounterdefinition
• theorem thmcountertheorem
• theorem thmcountertheorem
• lemma thmcounterlemma
• proof
• lemma thmcounterlemma
• proof
• theorem thmcountertheorem
• proof
• lemma thmcounterlemma