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
  • Massachusetts Institute of Technology, Cambridge, MA, USA

Workshop on the Algorithmic Foundations of Robotics 2024

Paper Code

TL;DR:

A complete and optimal algorithm inspired by A* for finding shortest paths on Graphs of Convex Sets.

Abstract

We consider large-scale, implicit-search-based solutions to the 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 sampling-based implementation 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.

GCS* applied to simplified planar pushing

To focus on the combinatorial explosion of contact modes, we consider a simplified planar pushing formulation with:

  1. polyhedral bodies;
  2. no rotations;
  3. and only sliding contact (no friction).

Static obstacles are in grey, unactuated objects are in red, actuated robots are in green and the object target regions are dotted.
ϵ-suboptimal solutions found using sampling-based domination checks.

Comparison with other algorithms

(Applied to shortest path problems in graphs of convex sets.)

AlgorithmCompleteCost-optimalRequires Explicit Graph
Naive A* SearchNoNoNo
GCS1YesYes (If using Branch and Bound)Yes
IxG2NoNoYes
IxG*2YesYes (But no pruning if using admissible heuristic)Yes
GCS* (Ours)Yes (also has probabilistically complete variant)Yes (also has asymptotically optimal variant)No

BibTeX citation

@misc{chewchiaGCSForwardHeuristic2024,
  author = {Chew Chia, Shao Yuan and Jiang, Rebecca H. and Graesdal, Bernhard Paus and Kaelbling, Leslie Pack and Tedrake, Russ},
  title = {GCS*: Forward Heuristic Search on Implicit Graphs of Convex Sets},
  year = {2024},
  eprint={2407.08848},
  archivePrefix={arXiv},
  primaryClass={cs.RO},
  url={https://arxiv.org/abs/2407.08848}
}

Footnotes

  1. Marcucci, T., Umenberger, J., Parrilo, P., Tedrake, R.: Shortest paths in graphs of convex sets 34(1), 507–532. https://doi.org/10.1137/22M1523790, https://epubs.siam.org/doi/full/10.1137/22M1523790 ↩

  2. Natarajan, R., Liu, C., Choset, H., Likhachev, M.: Implicit graph search for planning on graphs of convex sets. In: Proceedings of Robotics: Science and Systems 2024. ↩ ↩2