Motion Planning with Precedence Specifications via Augmented Graphs of Convex Sets

TL;DR

This work develops an exact, scalable framework for motion planning with key-door precedence constraints by embedding the problem into an augmented graph of convex sets (GCS). It introduces an exact convex partition of obstacle-free space, keys, and doors via hyperplane arrangements and constructs an augmented GCS that encodes the key-door logic using layered reachable-key subsets; solving a shortest path on this augmented structure yields an optimal trajectory up to finite parameterization. The approach achieves orders-of-magnitude speedups over general temporal-logic tools and provides a principled way to certify infeasibility. The combination of exact partitioning, augmented GCS construction, and a dedicated maze generator offers a practical, open framework for fast, certifiable task-and-motion planning in environments with precedence constraints.

Abstract

We present an algorithm for planning trajectories that avoid obstacles and satisfy key-door precedence specifications expressed with a fragment of signal temporal logic. Our method includes a novel exact convex partitioning of the obstacle free space that encodes connectivity among convex free space sets, key sets, and door sets. We then construct an augmented graph of convex sets that exactly encodes the key-door precedence specifications. By solving a shortest path problem in this augmented graph of convex sets, our pipeline provides an exact solution up to a finite parameterization of the trajectory. To illustrate the effectiveness of our approach, we present a method to generate key-door mazes that provide challenging problem instances, and we perform numerical experiments to evaluate the proposed pipeline. Our pipeline is faster by several orders of magnitude than recent state-of-the art methods that use general purpose temporal logic tools.

Figures (7)

• Figure 1: Overview of the proposed approach. The environment is first partitioned and represented as a Graph of Convex Sets (GCS). An augmented GCS is then constructed to encode precedence specifications, and a shortest path problem is solved for the augmented GCS.
• Figure 2: A key-door environment with 2 keys and 2 doors. The blue line shows the optimal solution.
• Figure 3: The augmented GCS for a simple 2-key environment. The start point is located in free space node $\mathcal{C}_2$ and the target node is located in $\mathcal{C}_4$.
• Figure 4: Figure illustrating the size of the augmented GCS through graphs of subgraphs up to $n_K = 4$ keys.
• Figure 5: Example mazes: (a) narrow maze with key-door batch (1, 2, 1, 1, 2), (b) wide maze with key-door batch (7).