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