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
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
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/