# GlobMP: continuous-time constrained motion planning

Code for *Continuous-Time Constrained Motion Planning with Global Convergence Guarantees*.

GlobMP plans piecewise polynomial trajectories under constraints that must hold at **every** time
t, not only at sample points. Each constraint block (an obstacle, a robot-robot separation, a speed
limit, a half-space, ...) is a polynomial in t on every segment, and it holds for all t exactly
when its Bernstein coefficients lie in the cone of polynomials nonnegative on [0, 1]. Two first-order
algorithms work on that representation:

- **gradient projection** (GlobMP's main loop) takes gradient steps on the cost and repairs after each one;
- **the repair of Eq. (5)**: the next curve is the nearest
  point of the set where every block, linearized at the current feasible curve, is nonnegative at
  every t. That set is convex and lies inside the feasible set, so every iterate is a complete
  feasible trajectory, and a step is accepted only if it does not raise the cost. The repair is
  solved through a short list of contact instants (`repair.py`).

A seed that is not feasible is first made feasible by a restoration (`restore.py`): gradient steps
on the distances of the blocks to their cones, from any start.

In the paper: INIT is Algorithm 1, the repair of Eq. (5) Algorithm 2, gradient projection Algorithm 3, and the
cone projection (`cone.py`) Algorithm 4. The code and its documentation name the algorithms and do not number them.

When every block is quadratic in the curve, a single eigenvalue test on the Lagrangian Hessian at a
KKT point certifies global optimality (a failed test is inconclusive).

```
globmp/                      the method (pip install -e .)
  spline.py                  Bernstein basis, derivatives, Gram matrices, degree elevation
  cone.py                    projection onto the cone of nonnegative polynomials (exact atom search)
  problem.py                 the program (P): chart of the boundary/continuity conditions, costs
  blocks/                    one module per constraint family:
                             sphere, separation, moving, ellipsoid, speed, halfspace, box, superellipse
  restore.py                 restoration of an infeasible seed
  repair.py                  the repair of Eq. (5)
  gpa.py                     gradient projection
  certificate.py             contacts, multipliers, global-optimality test
  check.py                   exact continuous-time feasibility check
  seed.py                    initial curves from waypoints
  init.py                    INIT: shortest path over the joints for given per-joint state sets
  synthetic/                 Section 4.1: robots exchanging places among spheres (2 to 200 robots)
  gridstart/                 Section 3: INIT on the grid of Theorem 1 (the grid start), against the plain grid
  tests/                     fast unit tests
applications/
  multi_robot/               Section 4.2 and appendix: multi-robot planning
    robotarium/              Section 4.2: the Robotarium arena (GlobMP, run/, data/, robotarium_baselines/)
    multi_robot_extension/   appendix: the simulated suite (GlobMP, run/, data/, suite_baselines/)
  maze2d/                    appendix: Maze2D navigation
    maze2d_baselines/        DiRecT's maze2d benchmark with the ten diffusion baselines and their checkpoint
  steering/                  Section 4.3: steering tactile-reactive dexterous policies
```

## Install

```bash
pip install -e .                 # numpy, scipy, torch, cvxpy, clarabel
pip install -e .[baselines]      # ompl, drake, matplotlib: baselines and figures
python -m pytest globmp/tests    # about 10 s on one CPU core
```

## Minimal example

```python
import numpy as np
from globmp import Problem, init_waypoints, is_feasible

# one robot in the plane, minimum-jerk cost (k=3), 2 segments of degree 7, a disc to pass
p = Problem(n=2, d=7, k=3, ell=3, M=2, start=[0., 0.], goal=[6., 0.], delta=0.005, whiten=True)
p.add_sphere([3.0, -0.5], 1.0)

Z = init_waypoints(p, np.array([[0., 0.], [3., 1.2], [6., 0.]]))   # the seed only picks the route class
tv = p.tolvec()
Z, _, ok = p.solve(Z, tv)          # restoration of the seed (a no-op if it is feasible), then gradient projection

print(ok, is_feasible(p, Z), float(p.cost(Z)))
```

Problem dimensions: `n` coordinates (all robots stacked), `M` segments of degree `d`, cost
`int ||gamma^(k)||^2` (or a tracking cost, `set_tracking`), boundary derivatives up to `ell` fixed,
continuity `C^ell_j` at junctions.

## What reproduces what

| paper | code | check against the paper's runs |
|---|---|---|
| Sec. 4.1, Figs. 2-3 (pairs of robots among spheres, 2 to 200 robots; RRT-Connect, RRT*, GCS, SDP-MP) | `globmp/synthetic/` | default: INIT returns one feasible curve (its last step compares the curves of at most three route classes after 5 iterations of gradient projection), then one run of gradient projection as written in the paper (step 1/beta, repair of Eq. (5), no restoration; median 0.45 / 0.69 / 0.79 / 0.92 s per instance for 2 / 4 / 6 / 8 robots); 80 of 80 instances reach the cost of the all-class run within 1e-5 (77 of 80 without the last step of INIT). All-class run (`--routes all`): 2472 route classes, all feasible, median 0.9 / 1.1 / 1.2 / 1.4 s per class; per-instance costs within 7e-5 of the runs behind Fig. 2, and at most 3e-7 above the paper's (relative). Baselines bit-identical. Fig. 3 (scaling) is from the paper's runs, not rerun; 200 robots on CPU: 65 restoration iterations in 70 s (paper 71 s) |
| Sec. 3, INIT (the grid start of Theorem 1) and its appendix table | `globmp/gridstart/` | start of the same cost as the plain grid in 27 of 27 cases (same curve except one tie); 16 free coordinates, 5.9e18 grid points: 208,929 exact piece tests |
| Sec. 4.2 and appendix (Robotarium arena; simulated suite) | `applications/multi_robot/robotarium/`, `applications/multi_robot/multi_robot_extension/` | suite cells: same success, path length to 3e-5; Robotarium: saved plans rescored to the paper's numbers on all 20 cells |
| appendix (Maze2D) | `applications/maze2d/` | score, success and violations identical; plans match the figure data to 1e-11 |
| Sec. 4.3 (steering dexterous policies) | `applications/steering/` | robot-side code not included yet |

The core in `globmp/` was checked against the solver behind every result: on instances covering every
block family, both step rules, both charts, the tracking costs and the certificate, it
returns bit-identical iterates from the same start. Each folder's README gives the exact commands,
data, environments and expected numbers, and lists what differs from the paper and why.

The five demonstration archives of the Robotarium baselines (`applications/multi_robot/robotarium/robotarium_baselines/data_prep/demonstrations/*.tar.zst`, 779 MB) are on Hugging Face only: https://huggingface.co/GlobalMotionPlanning/GlobMP

Project page: https://globalmotionplanning.github.io/