Add the FCIT* asymptotically optimal motion planner (#35)

* doc: note about incremental rebuilds

* feat: initial implementation

* feat: Python RNG functions

* fix: Halton sequence blows up after around 1.5M iters - restart and rotate bases after 1M

* wip: refactor names, remove old RNG code

* feat: added xorshift

* fix/feat: xorshift mapping fixed + sampling feature on other scripts

* feat: final script fix + documentation

* format: applied clang-format

* Update LLVM version

* goof, update format version

* yapf format

* Added fcit.hh

* Got rid of unecessary left over function

* Brought in support for fcit*

* format

* RNG refactor for FCIT

* use pdqsort_branchless for a modest perf improvement

* Add batch size and ASAO mode to settings for FCIT

* Minor readability updates

* changed simplification semantics so will work with FCIT paths

* reducing uncessary scopes

* Fixes after merge

* format

* Added paper link to README

* Added other paper link to README

* Addressing suggestions for PR:
 - Fixed "simplificiation" typo in README
   + Also changed "simplification_interpolation" in README to
     "simplification_interpolate" to reflect correct argument
   + Arguments not beginning in "simplification_" were being skipped in
     configure_robot_and_planner_with_kwargs -- fixed by changing a
"continue" into an if statement
 - Changed "fcit_single"/"fcit" to "fcit"/"fcit_multi_goal"
 - Moved "QueueEdge" out of utils.hh and into fcit.hh
   + Should we also move "FCITRoadmapNode" out of roadmap.hh as well?
     It's only used by FCIT*
 - Changed "(*it)." to "it->"
 - Changed invalidList to an unordered_set (best results)

* Moved FCITRoadmapNode out of roadmap.hh to fcit.hh

* Update README.md

Co-authored-by: Wil Thomason <[email protected]>

* fix for yapf format check

* Added FCIT arXiv information

---------

Co-authored-by: twill777 <[email protected]>
Co-authored-by: Wil Thomason <[email protected]>

Author: 3 people

README.md

@@ -2,10 +2,14 @@
 
 [![arXiv VAMP](https://img.shields.io/badge/arXiv-2309.14545-b31b1b.svg)](https://arxiv.org/abs/2309.14545)
 [![arXiv CAPT](https://img.shields.io/badge/arXiv-2406.02807-b31b1b.svg)](https://arxiv.org/abs/2406.02807)
+[![arXiv FCIT](https://img.shields.io/badge/arXiv-2411.17902-b31b1b.svg)](https://arxiv.org/abs/2411.17902)
 [![Build Check](https://github.com/KavrakiLab/vamp/actions/workflows/build.yml/badge.svg)](https://github.com/KavrakiLab/vamp/actions/workflows/build.yml)
 [![Format Check](https://github.com/KavrakiLab/vamp/actions/workflows/format.yml/badge.svg)](https://github.com/KavrakiLab/vamp/actions/workflows/format.yml)
 
-This repository hosts the code for the ICRA 2024 paper [“Motions in Microseconds via Vectorized Sampling-Based Planning”](https://arxiv.org/abs/2309.14545) as well as an implementation of the Collision-Affording Point Tree (CAPT) from the forthcoming RSS 2024 paper [“Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking”](http://arxiv.org/abs/2406.02807).
+This repository hosts the code for:
+- the ICRA 2024 paper [“Motions in Microseconds via Vectorized Sampling-Based Planning”](https://arxiv.org/abs/2309.14545),
+- an implementation of the Collision-Affording Point Tree (CAPT) from the RSS 2024 paper [“Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking”](http://arxiv.org/abs/2406.02807),
+- an implementation of the Fully Connected Informed Trees (FCIT*) algorithm from the ICRA 2025 submission [“Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees (FCIT*)”](https://robotic-esp.com/papers/wilson_arxiv24).
 
 **TL;DR**: By exploiting ubiquitous [CPU SIMD instructions](https://en.wikipedia.org/wiki/Single_instruction,_multiple_data) to accelerate collision checking and forward kinematics (FK), `vamp`'s RRT-Connect [[1]](#1) solves problems for the Franka Emika Panda from the MotionBenchMaker dataset [[3]](#3) at a median speed of 35 microseconds (on one core of a consumer desktop PC).
 This approach to hardware-accelerated parallel sampling-based motion planning extends to other planning algorithms without modification (e.g., PRM [[2]](#2)) and also works on low-power systems (e.g., an ARM-based [OrangePi](http://www.orangepi.org/)).
@@ -17,8 +21,10 @@ If you found this research useful for your own work, please use the following ci
   title = {Motions in Microseconds via Vectorized Sampling-Based Planning},
   author = {Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.},
   booktitle = {IEEE International Conference on Robotics and Automation},
-  date = {2024},
+  pages = {8749--8756},
   url = {http://arxiv.org/abs/2309.14545},
+  doi = {10.1109/ICRA57147.2024.10611190},
+  date = {2024},
 }
 ```
 
@@ -28,9 +34,19 @@ If you use CAPTs or the pointcloud collision checking components of this reposit
   title = {Collision-Affording Point Trees: {SIMD}-Amenable Nearest Neighbors for Fast Collision Checking},
   author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},
   booktitle = {Robotics: Science and Systems},
-  date = {2024},
   url = {http://arxiv.org/abs/2406.02807},
-  note = {To Appear.}
+  doi = {10.15607/RSS.2024.XX.038},
+  date = {2024},
+}
+```
+
+If you use FCIT*, please use the following citation:
+```bibtex
+@misc{fcit_2024,
+  title = {Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees (FCIT*)},
+  author = {Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary  and Kavraki, Lydia E. and Gammell, Jonathan D.},
+  url = {https://arxiv.org/abs/2411.17902},
+  date = {2024}
 }
 ```
 
@@ -151,6 +167,7 @@ We ship implementations of the following pseudorandom number generators (PRNGs):
 We currently ship two planners:
 - `rrtc`, which is an implementation of a dynamic-domain [[6]](#6) balanced [[7]](#7) RRT-Connect [[1]](#1).
 - `prm`, which is an implementation of basic PRM [[2]](#2) (i.e., PRM without the bounce heuristic, etc.).
+- `fcit`, which is an asymptotically optimal planner, described in the [linked paper](https://robotic-esp.com/papers/wilson_arxiv24).
 
 Note that these planners support planning to a set of goals, not just a single goal.
 
@@ -183,17 +200,22 @@ For `rrtc`:
 - `--start_tree_first`: `True` or `False`, grow from start tree or goal tree first.
 See `rrtc_settings.hh` for more information.
 
-For `prm`, the settings must be configured with a neighbor parameter structure, e.g.:
+For `prm` and `fcit`, the settings must be configured with a neighbor parameter structure, e.g.:
 ```py
 robot_module = vamp.panda # or other robot submodule
 prmstar_params = vamp.PRMNeighborParams(robot_module.dimension(), robot_module.space_measure())
 prm_settings = vamp.PRMSettings(prmstar_params)
 ```
 This is handled by default in the configuration function.
 
+For `fcit`, there are also the settings:
+- `--batch_size`: The number of samples to evaluate in a batch per iteration. Default is 1000.
+- `--optimize`: If true, will use all iterations and samples available to find the best possible solution. Default is False. If true, set `--max_samples` to the desired value of refinement.
+
 For simplification:
 - `--simplification_operations`: sequence of shortcutting heuristics to apply each iteration. By default, `[SHORTCUT,BSPLINE]`. Can specify any sequence of the above keys.
-- `--max_iterations`: maximum iterations of simplification. If no heuristics do any work, then early terminates from simplification.
+- `--simplification_max_iterations`: maximum iterations of simplification. If no heuristics do any work, then early terminates from simplification.
+- `--simplification_interpolate`: if non-zero, will interpolate the path before simplification heuristics are applied to the desired resolution.
 - `--bspline_max_steps`: maximum iterations of B-spline smoothing.
 - `--bspline_min_change`: minimum change before smoothing is done.
 - `--bspline_midpoint_interpolation`: point along each axis B-spline interpolation is done from.
@@ -250,6 +272,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
   Planning and simplification routines.
   `rrtc.hh` and `rrtc_settings.hh` are for our RRT-Connect implementation.
   `prm.hh` and `roadmap.hh` are for our PRM implementation.
+  `fcit.hh` is for the FCIT* implementation.
   `simplify.hh` and `simplify_settings.hh` are for simplification heuristics.
   `validate.hh` contains the raked motion validator.
 
@@ -268,6 +291,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
 - [X] Pointcloud collision checking
 - [ ] Manifold-constrained planning
 - [ ] Time-optimal trajectory parameterization
+- [X] Asymptotically-optimal planning
 - and more...
 
 ## References

resources/problem_tar_to_pkl_json.py

@@ -121,11 +121,10 @@ def main(robot: str = "panda"):
         data['problems'][k] = [
             {
                 **s, **r
-                } for s,
-            r in zip(
-                sorted(scenes[k], key = lambda e: e['index']),
-                sorted(requests[k], key = lambda e: e['index'])
-                )
+                } for (s, r) in zip(
+                    sorted(scenes[k], key = lambda e: e['index']),
+                    sorted(requests[k], key = lambda e: e['index'])
+                    )
             ]
 
         for problem in data['problems'][k]:

scripts/evaluate_mbm.py

@@ -17,7 +17,7 @@ def main(
     dataset: str = "problems.pkl",         # Pickled dataset to use
     problem: Union[str, List[str]] = [],   # Problem name or list of problems to evaluate
     trials: int = 1,                       # Number of trials to evaluate each instance
-    sampler_name: str = "halton",          # Sampler to use.
+    sampler: str = "halton",               # Sampler to use.
     skip_rng_iterations: int = 0,          # Skip a number of RNG iterations
     print_failures: bool = False,          # Print out failures and invalid problems
     pointcloud: bool = False,              # Use pointcloud rather than primitive geometry
@@ -50,7 +50,7 @@ def main(
     (vamp_module, planner_func, plan_settings,
      simp_settings) = vamp.configure_robot_and_planner_with_kwargs(robot, planner, **kwargs)
 
-    sampler = getattr(vamp_module, sampler_name)()
+    sampler = getattr(vamp_module, sampler)()
 
     total_problems = 0
     valid_problems = 0

src/impl/vamp/bindings/common.hh

@@ -15,6 +15,7 @@
 #include <vamp/planning/simplify.hh>
 #include <vamp/planning/plan.hh>
 #include <vamp/planning/prm.hh>
+#include <vamp/planning/fcit.hh>
 #include <vamp/planning/rrtc.hh>
 #include <vamp/vector.hh>
 
@@ -103,6 +104,7 @@ namespace vamp::binding
 
         using PRM = vamp::planning::PRM<Robot, rake, Robot::resolution>;
         using RRTC = vamp::planning::RRTC<Robot, rake, Robot::resolution>;
+        using FCIT = vamp::planning::FCIT<Robot, rake, Robot::resolution>;
 
         inline static auto halton() -> typename RNG::Ptr
         {
@@ -233,6 +235,36 @@ namespace vamp::binding
             return PRM::solve(start_v, goals_v, EnvironmentVector(environment), settings, rng);
         }
 
+        inline static auto fcit(
+            const ConfigurationArray &start,
+            const ConfigurationArray &goal,
+            const EnvironmentInput &environment,
+            const vamp::planning::RoadmapSettings<vamp::planning::FCITStarNeighborParams> &settings,
+            typename RNG::Ptr rng) -> PlanningResult
+        {
+            return FCIT::solve(
+                Configuration(start), Configuration(goal), EnvironmentVector(environment), settings, rng);
+        }
+
+        inline static auto fcit_multi_goal(
+            const ConfigurationArray &start,
+            const std::vector<ConfigurationArray> &goals,
+            const EnvironmentInput &environment,
+            const vamp::planning::RoadmapSettings<vamp::planning::FCITStarNeighborParams> &settings,
+            typename RNG::Ptr rng) -> PlanningResult
+        {
+            std::vector<Configuration> goals_v;
+            goals_v.reserve(goals.size());
+
+            for (const auto &goal : goals)
+            {
+                goals_v.emplace_back(goal);
+            }
+
+            const Configuration start_v(start);
+            return FCIT::solve(start_v, goals_v, EnvironmentVector(environment), settings, rng);
+        }
+
         inline static auto roadmap(
             const ConfigurationArray &start,
             const ConfigurationArray &goal,
@@ -527,6 +559,26 @@ namespace vamp::binding
             "rng"_a,
             "Solve the motion planning problem with PRM.");
 
+        submodule.def(
+            "fcit",
+            RH::fcit,
+            "start"_a,
+            "goal"_a,
+            "environment"_a,
+            "settings"_a,
+            "rng"_a,
+            "Solve the motion planning problem with FCIT*.");
+
+        submodule.def(
+            "fcit",
+            RH::fcit_multi_goal,
+            "start"_a,
+            "goal"_a,
+            "environment"_a,
+            "settings"_a,
+            "rng"_a,
+            "Solve the motion planning problem with FCIT*.");
+
         submodule.def(
             "roadmap",
             RH::roadmap,

src/impl/vamp/bindings/settings.cc

@@ -42,6 +42,25 @@ void vamp::binding::init_settings(nanobind::module_ &pymodule)
         .def("max_neighbors", &PRMStarSettings::max_neighbors)
         .def("neighbor_radius", &PRMStarSettings::neighbor_radius);
 
+    nb::class_<vp::FCITStarNeighborParams>(pymodule, "FCITNeighborParams")
+        .def(nb::init<std::size_t, double>())
+        .def_rw("dim", &vp::FCITStarNeighborParams::dim)
+        .def_rw("space_measure", &vp::FCITStarNeighborParams::space_measure)
+        .def_rw("gamma_scale", &vp::FCITStarNeighborParams::gamma_scale)
+        .def("max_neighbors", &vp::FCITStarNeighborParams::max_neighbors)
+        .def("neighbor_radius", &vp::FCITStarNeighborParams::neighbor_radius);
+
+    using FCITStarSettings = vp::RoadmapSettings<vp::FCITStarNeighborParams>;
+    nb::class_<FCITStarSettings>(pymodule, "FCITSettings")
+        .def(nb::init<vp::FCITStarNeighborParams>())
+        .def_rw("max_iterations", &FCITStarSettings::max_iterations)
+        .def_rw("max_samples", &FCITStarSettings::max_samples)
+        .def_rw("batch_size", &FCITStarSettings::batch_size)
+        .def_rw("optimize", &FCITStarSettings::optimize)
+        .def_rw("neighbor_params", &FCITStarSettings::neighbor_params)
+        .def("max_neighbors", &FCITStarSettings::max_neighbors)
+        .def("neighbor_radius", &FCITStarSettings::neighbor_radius);
+
     nb::enum_<vp::SimplifyRoutine>(pymodule, "SimplifyRoutine")
         .value("BSPLINE", vp::SimplifyRoutine::BSPLINE)
         .value("REDUCE", vp::SimplifyRoutine::REDUCE)
@@ -73,6 +92,7 @@ void vamp::binding::init_settings(nanobind::module_ &pymodule)
     nb::class_<vp::SimplifySettings>(pymodule, "SimplifySettings")
         .def(nb::init<>())
         .def_rw("max_iterations", &vp::SimplifySettings::max_iterations)
+        .def_rw("interpolate", &vp::SimplifySettings::interpolate)
         .def_rw("operations", &vp::SimplifySettings::operations)
         .def_rw("reduce", &vp::SimplifySettings::reduce)
         .def_rw("shortcut", &vp::SimplifySettings::shortcut)