Greedy Heuristics for Sampling‑Based Motion Planning in High‑Dimensional State Spaces

Informed sampling techniques focus sampling on the set of states whose heuristic cost is below the cost of the current solution. Initial solutions are often tortuous, which keeps this region large. The greedy informed set, on the other hand, depends only on the state along the path with the maximum heuristic cost, so any detours in other parts of the path do not make the set larger. Our planner G-RRT* finds an initial solution with two trees that grow toward each other, and then samples mostly from this greedy informed set while remaining asymptotically optimal.

Every animation and planner run on this page uses VAMP, except the OMPL experiments from the paper. Simplification means shortcutting followed by B-spline smoothing.

Abstract

Greedy informed sampling

The heuristic cost $\hat f(\mathbf{x})$ of a state is a lower bound on the cost of any path from the start through that state to the goal. No path through the state can have a lower cost. Under the Euclidean metric, it is the sum of the straight-line distances from the start to the state and from the state to the goal, and the states whose heuristic cost is below a given value form a prolate hyperspheroid. Informed sampling samples from the set of states whose heuristic cost is at most the cost of the current solution [1]. The greedy informed set lowers this bound to the maximum heuristic cost along the current solution path [2].

With $\hat f(\mathbf{x}) = \lVert \mathbf{x} - \mathbf{x}_I \rVert_2 + \lVert \mathbf{x} - \mathbf{x}_G \rVert_2$, both sets are the collision-free parts of hyperellipsoids with foci at the start $\mathbf{x}_I$ and the goal $\mathbf{x}_G$.

When the current solution is in the same homotopy class as the optimal path, the greedy informed set contains the optimal path (Theorem 1 of the paper). Two paths with the same endpoints are in the same homotopy class when one can be continuously deformed into the other without crossing an obstacle. When the two paths are in different homotopy classes, the greedy informed set might not include the optimal path, as in the Slalom scene above (Remark 1 of the paper). For this reason, G-RRT* also samples from the informed set to preserve asymptotic optimality.

Greedy sampling in high dimensions

For fixed path costs, the volume of the greedy informed set relative to the informed set decreases exponentially with the dimension of the state space. When the greedy informed set contains every state that can improve the current solution, G-RRT* needs fewer samples than informed sampling to find a better solution. The expected number of samples decreases by a factor of about 0.9 divided by this volume ratio (Theorem 3 of the paper). Greedy sampling therefore helps most in high dimensions.

G-RRT*

G-RRT* grows one tree from the start and one from the goal. As in RRT-Connect [3], it connects the two trees greedily, so it finds an initial solution quickly. After the trees meet, G-RRT* rewires both trees as RRT* [4] does. It samples each new state from the greedy informed set with probability $\epsilon$, the greedy biasing ratio, and from the informed set otherwise. Any $\epsilon < 1$ keeps asymptotic optimality, and we use $\epsilon = 0.9$.

Plan in your browser

G-RRT* is also part of VAMP [5], which uses SIMD instructions to speed up collision checking and forward kinematics. Here, VAMP runs in your browser through its WebAssembly bindings.

MotionBenchMaker on four robots

MotionBenchMaker [6] generates realistic manipulation problems for common robots, in scenes such as bookshelves, tables, boxes and cages. We solve them with G-RRT* in VAMP for the Panda, UR5, Fetch and the dual-arm Baxter, which have 6 to 14 degrees of freedom (DoF). For a typical problem in each scene, the viewer replays the initial solution and the best solution found within the time budget, each before and after simplification.

Results

VAMP on MotionBenchMaker

We compare G-RRT* with RRT-Connect [3], AORRTC [7] and FCIT* [8] on the MotionBenchMaker problems of all four robots. AORRTC simplifies each solution before it reports it, so we compare final solution costs after simplification. After simplification, the median final solution costs of G-RRT* and AORRTC are within 0.3% of each other on the Panda and UR5 and within 1% on the Baxter, and G-RRT* has a 4% lower median final solution cost on the Fetch.

We also run G-RRT* with $\epsilon = 0$, so that it samples only from the informed set. Both versions find the same initial solution, but the $\epsilon = 0$ version converges more slowly on all four robots. The difference is largest on the Fetch and Baxter, where the greedy informed set is smallest relative to the informed set.

OMPL experiments from the paper

The paper's experiments in OMPL [9] compare G-RRT* with eight other planners, on abstract problems in up to 16 dimensions, on MotionBenchMaker problems for the Panda and Fetch, and on a dual-arm Barrett WAM problem with 14 DoF. G-RRT* finds initial solutions at least as fast as RRT-Connect and converges faster than the other asymptotically optimal planners. Without path simplification, its final solution costs are comparable to those of AORRTC, whose reported costs already include simplification. After simplification, G-RRT* has the lowest median final solution cost on 13 of the 15 problems.

Citation

@article{kyaw2026greedy,
  title   = {Greedy Heuristics for Sampling-Based Motion Planning
             in High-Dimensional State Spaces},
  author  = {Kyaw, Phone Thiha and Le, Anh Vu and
             Mohan, Rajesh Elara and Kelly, Jonathan},
  journal = {Autonomous Robots},
  volume  = {50},
  number  = {4},
  pages   = {44},
  year    = {2026},
  doi     = {10.1007/s10514-026-10269-0}
}

G-RRT* is available in our fork of OMPL at utiasSTARS/ompl and in VAMP at KavrakiLab/vamp.

References

1. J. D. Gammell, T. D. Barfoot, and S. S. Srinivasa. Informed sampling for asymptotically optimal path planning. IEEE Transactions on Robotics, 34(4), 2018. doi:10.1109/TRO.2018.2830331
2. P. T. Kyaw, A. V. Le, P. Veerajagadheswar, M. R. Elara, T. T. Thu, N. H. K. Nhan, P. Van Duc, and M. B. Vu. Energy-efficient path planning of reconfigurable robots in complex environments. IEEE Transactions on Robotics, 38(4), 2022. doi:10.1109/TRO.2022.3147408
3. J. J. Kuffner and S. M. LaValle. RRT-Connect: An efficient approach to single-query path planning. In IEEE International Conference on Robotics and Automation (ICRA), 2000. doi:10.1109/ROBOT.2000.844730
4. S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 2011. doi:10.1177/0278364911406761
5. W. Thomason, Z. Kingston, and L. E. Kavraki. Motions in microseconds via vectorized sampling-based planning. In IEEE International Conference on Robotics and Automation (ICRA), 2024. doi:10.1109/ICRA57147.2024.10611190
6. C. Chamzas, C. Quintero-Peña, Z. Kingston, A. Orthey, D. Rakita, M. Gleicher, M. Toussaint, and L. E. Kavraki. MotionBenchMaker: A tool to generate and benchmark motion planning datasets. IEEE Robotics and Automation Letters, 7(2), 2022. doi:10.1109/LRA.2021.3133603
7. T. S. Wilson, W. Thomason, Z. Kingston, and J. D. Gammell. AORRTC: Almost-surely asymptotically optimal planning with RRT-Connect. IEEE Robotics and Automation Letters, 10(12), 2025. doi:10.1109/LRA.2025.3615522
8. T. S. Wilson, W. Thomason, Z. Kingston, L. E. Kavraki, and J. D. Gammell. Nearest-neighbourless asymptotically optimal motion planning with Fully Connected Informed Trees (FCIT*). In IEEE International Conference on Robotics and Automation (ICRA), 2025. doi:10.1109/ICRA55743.2025.11127785
9. I. A. Șucan, M. Moll, and L. E. Kavraki. The Open Motion Planning Library. IEEE Robotics & Automation Magazine, 19(4), 2012. doi:10.1109/MRA.2012.2205651