From a00c75001c81268950619dbe146ad44ba4172742 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Fri, 13 Dec 2024 14:14:11 -0500 Subject: [PATCH] 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 * fix for yapf format check * Added FCIT arXiv information --------- Co-authored-by: twill777 Co-authored-by: Wil Thomason --- README.md | 36 +- resources/problem_tar_to_pkl_json.py | 9 +- scripts/evaluate_mbm.py | 4 +- src/impl/vamp/bindings/common.hh | 52 +++ src/impl/vamp/bindings/settings.cc | 20 ++ src/impl/vamp/planning/fcit.hh | 357 ++++++++++++++++++++ src/impl/vamp/planning/prm.hh | 3 +- src/impl/vamp/planning/roadmap.hh | 32 +- src/impl/vamp/planning/simplify.hh | 17 +- src/impl/vamp/planning/simplify_settings.hh | 1 + src/impl/vamp/planning/utils.hh | 19 ++ src/vamp/__init__.py | 26 +- src/vamp/__init__.pyi | 2 + 13 files changed, 548 insertions(+), 30 deletions(-) create mode 100644 src/impl/vamp/planning/fcit.hh diff --git a/README.md b/README.md index 1a2b715f..6021f998 100644 --- a/README.md +++ b/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,7 +200,7 @@ 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()) @@ -191,9 +208,14 @@ 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 diff --git a/resources/problem_tar_to_pkl_json.py b/resources/problem_tar_to_pkl_json.py index d7d6d5ce..b1c7185b 100644 --- a/resources/problem_tar_to_pkl_json.py +++ b/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]: diff --git a/scripts/evaluate_mbm.py b/scripts/evaluate_mbm.py index b1888338..eb67f320 100644 --- a/scripts/evaluate_mbm.py +++ b/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 diff --git a/src/impl/vamp/bindings/common.hh b/src/impl/vamp/bindings/common.hh index 1dcbcca9..528b0790 100644 --- a/src/impl/vamp/bindings/common.hh +++ b/src/impl/vamp/bindings/common.hh @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -103,6 +104,7 @@ namespace vamp::binding using PRM = vamp::planning::PRM; using RRTC = vamp::planning::RRTC; + using FCIT = vamp::planning::FCIT; 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 &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 &goals, + const EnvironmentInput &environment, + const vamp::planning::RoadmapSettings &settings, + typename RNG::Ptr rng) -> PlanningResult + { + std::vector 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, diff --git a/src/impl/vamp/bindings/settings.cc b/src/impl/vamp/bindings/settings.cc index 84a7f65e..d1ad894a 100644 --- a/src/impl/vamp/bindings/settings.cc +++ b/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_(pymodule, "FCITNeighborParams") + .def(nb::init()) + .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; + nb::class_(pymodule, "FCITSettings") + .def(nb::init()) + .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_(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_(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) diff --git a/src/impl/vamp/planning/fcit.hh b/src/impl/vamp/planning/fcit.hh new file mode 100644 index 00000000..6de54bef --- /dev/null +++ b/src/impl/vamp/planning/fcit.hh @@ -0,0 +1,357 @@ +#pragma once +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace vamp::planning +{ + + struct QueueEdge + { + unsigned int index; + unsigned int parent; + float cost; + + inline constexpr auto operator==(const QueueEdge &o) const noexcept -> bool + { + return index == o.index; + } + }; + + struct FCITRoadmapNode + { + public: + FCITRoadmapNode( + unsigned int index, + float g = std::numeric_limits::infinity(), + unsigned int sampleIdx = 0) + : index(index), g(g), sampleIdx(sampleIdx) + { + } + + unsigned int index; + float g; + unsigned int sampleIdx; + + struct Neighbor + { + unsigned int index; + float distance; + }; + + std::vector neighbors; + std::vector::iterator neighbor_iterator; + std::unordered_set invalidList; + }; + + template < + typename Robot, + std::size_t rake, + std::size_t resolution, + typename NeighborParamsT = FCITStarNeighborParams> + struct FCIT + { + using Configuration = typename Robot::Configuration; + static constexpr auto dimension = Robot::dimension; + using RNG = typename vamp::rng::RNG; + + inline static auto solve( + const Configuration &start, + const Configuration &goal, + const collision::Environment> &environment, + const RoadmapSettings &settings, + typename RNG::Ptr &rng) noexcept -> PlanningResult + { + return solve(start, std::vector{goal}, environment, settings, rng); + } + + inline static auto solve( + const Configuration &start, + const std::vector &goals, + const collision::Environment> &environment, + const RoadmapSettings &settings, + typename RNG::Ptr &rng) noexcept -> PlanningResult + { + auto start_time = std::chrono::steady_clock::now(); + + PlanningResult result; + NN roadmap; + + std::size_t iter = 0; + typename Robot::template ConfigurationBlock temp_block; + auto states = std::unique_ptr( + vamp::utils::vector_alloc( + settings.max_samples * Configuration::num_scalars_rounded)); + // TODO: Is it better to just use arrays for these since we're reserving full capacity + // anyway? Test it! + std::vector nodes; + nodes.reserve(settings.max_samples); + std::vector parents; + parents.reserve(settings.max_samples); + + const auto state_index = [&states](unsigned int index) -> float * + { return states.get() + index * Configuration::num_scalars_rounded; }; + + // Add start and goal to structures + constexpr const unsigned int start_index = 0; + + float *start_state = state_index(start_index); + start.to_array(start_state); + parents.emplace_back(std::numeric_limits::max()); + nodes.emplace_back(start_index, 0.0); + roadmap.insert(NNNode{start_index, {start_state}}); + auto &start_node = nodes[start_index]; + start_node.neighbor_iterator = start_node.neighbors.begin(); + + for (const auto &goal : goals) + { + std::size_t index = nodes.size(); + auto *goal_state = state_index(index); + goal.to_array(goal_state); + parents.emplace_back(std::numeric_limits::max()); + nodes.emplace_back(index); + roadmap.insert(NNNode{index, {goal_state}}); + } + + Configuration temp_config; + Configuration temp_config_self; + std::vector open_set; + + // Search until Initial Solution + while (nodes.size() < settings.max_samples and iter++ < settings.max_iterations) + { + for (auto i = 0U; i < goals.size(); ++i) + { + const auto &goal = goals[i]; + const auto &goal_node = nodes[i + 1]; + + // Iterate through neighbors and add all outgoing neighbors + for (auto it = nodes.begin() + start_node.sampleIdx; it != nodes.end(); it++) + { + if (it->index == start_index) + { + start_node.sampleIdx++; + continue; + } + + const auto neighbor_index = it->index; + temp_config = state_index(neighbor_index); + const auto neighbor_distance = start.distance(temp_config); + start_node.sampleIdx = std::max(neighbor_index, start_node.sampleIdx); + + // g(p) + c^(p,c) < g(c) + if (neighbor_distance < it->g) + { + // f^(c) = g(p) + c^(p,c) + h^(c) + const auto cost = neighbor_distance + goal.distance(temp_config); + start_node.neighbors.emplace_back( + typename FCITRoadmapNode::Neighbor{neighbor_index, cost}); + } + start_node.sampleIdx++; + } + // =========================================== + + pdqsort_branchless( + start_node.neighbors.begin(), + start_node.neighbors.end(), + [](const auto &a, const auto &b) { return a.distance < b.distance; }); + start_node.neighbor_iterator = start_node.neighbors.begin(); + + open_set.emplace_back(QueueEdge{ + (*start_node.neighbor_iterator).index, + start_index, + (*start_node.neighbor_iterator).distance}); + start_node.neighbor_iterator++; + + while (not open_set.empty()) + { + pdqsort_branchless( + open_set.begin(), + open_set.end(), + [](const auto &a, const auto &b) { return a.cost > b.cost; }); + + const auto current = open_set.back(); + open_set.pop_back(); + + const auto current_index = current.index; + auto ¤t_node = nodes[current_index]; + auto current_g = current_node.g; + auto current_p = current.parent; + + auto &parent_node = nodes[current_p]; + // +++++++++++++++++++++++++++++++++++++++++++ + while (parent_node.neighbor_iterator != parent_node.neighbors.end()) + { + const auto &node = nodes[(*parent_node.neighbor_iterator).index]; + + if ((*parent_node.neighbor_iterator).distance < + node.g + goal.distance(state_index(node.index))) + { + open_set.emplace_back(QueueEdge{ + (*parent_node.neighbor_iterator).index, + current_p, + (*parent_node.neighbor_iterator).distance}); + parent_node.neighbor_iterator++; + break; + } + + parent_node.neighbor_iterator++; + } + // =========================================== + + if (parents[current_index] != current_p) + { + temp_config_self = state_index(current_index); + const auto dist_to_goal = goal.distance(temp_config_self); + + if (current.cost <= goal_node.g) + { + // If this edge could improve the path through this node + if (current.cost < current_g + dist_to_goal) + { + bool valid = not current_node.invalidList.count(current_p); + + // If this edge hasn't already been found as invalid + if (valid) + { + if (current_index != current_p) + { + temp_config = state_index(current_p); + valid = validate_motion( + temp_config, temp_config_self, environment); + } + + // If the edge is valid + if (valid) + { + // Update the node's parent and g value + parents[current_index] = current_p; + current_g = + parent_node.g + temp_config.distance(temp_config_self); + current_node.g = current_g; + } + else + { + // Found to be invalid, add to necessary sets + parent_node.invalidList.insert(current_index); + current_node.invalidList.insert(current_p); + + (*(parent_node.neighbor_iterator - 1)).distance = + std::numeric_limits::max(); + continue; + } + } + } + } + else + { + break; + } + } + + current_node.neighbors.reserve(nodes.size()); + + bool added_neighbors = false; + // Iterate through neighbors and add all outgoing neighbors + for (auto it = nodes.begin() + current_node.sampleIdx; it != nodes.end(); it++) + { + if (it->index == current_index) + { + current_node.sampleIdx++; + continue; + } + + const auto neighbor_index = it->index; + temp_config = state_index(neighbor_index); + const auto neighbor_distance = temp_config_self.distance(temp_config); + current_node.sampleIdx = std::max(neighbor_index, current_node.sampleIdx); + + // f^(c) = g(p) + c^(p,c) + h^(c) + auto cost = current_g + neighbor_distance + goal.distance(temp_config); + + current_node.neighbors.emplace_back( + typename FCITRoadmapNode::Neighbor{neighbor_index, cost}); + current_node.sampleIdx++; + + added_neighbors = true; + } + + // If I added any new neighbours + if (added_neighbors) + { + pdqsort_branchless( + current_node.neighbors.begin(), + current_node.neighbors.end(), + [](const auto &a, const auto &b) { return a.distance < b.distance; }); + current_node.neighbor_iterator = current_node.neighbors.begin(); + + open_set.emplace_back(QueueEdge{ + (*current_node.neighbor_iterator).index, + current_index, + (*current_node.neighbor_iterator).distance}); + current_node.neighbor_iterator++; + } + } + } + + // If we have a solution and just want an initial solution, break + if (not settings.optimize and parents[1] != std::numeric_limits::max()) + { + break; + } + + for (auto new_samples = 0U; + new_samples < settings.batch_size and nodes.size() < settings.max_samples;) + { + auto rng_temp = rng->next(); + Robot::scale_configuration(rng_temp); + + // Check sample validity + for (auto i = 0U; i < dimension; ++i) + { + temp_block[i] = rng_temp.broadcast(i); + } + + if (not Robot::template fkcc(environment, temp_block)) + { + continue; + } + + // Insert valid state into graph structures + auto *state = state_index(nodes.size()); + rng_temp.to_array(state); + + parents.emplace_back(std::numeric_limits::max()); + auto &node = nodes.emplace_back(nodes.size()); + + auto road_node = NNNode{node.index, {state}}; + roadmap.insert(road_node); + new_samples++; + } + } + + utils::recover_path(parents, state_index, result.path); + result.cost = nodes[1].g; + result.nanoseconds = vamp::utils::get_elapsed_nanoseconds(start_time); + result.iterations = iter; + result.size.emplace_back(roadmap.size()); + result.size.emplace_back(0); + + return result; + } + }; +} // namespace vamp::planning diff --git a/src/impl/vamp/planning/prm.hh b/src/impl/vamp/planning/prm.hh index 0b7a0be6..3c57277f 100644 --- a/src/impl/vamp/planning/prm.hh +++ b/src/impl/vamp/planning/prm.hh @@ -23,8 +23,7 @@ namespace vamp::planning typename Robot, std::size_t rake, std::size_t resolution, - typename NeighborParamsT = PRMStarNeighborParams, - std::size_t batch = 128> + typename NeighborParamsT = PRMStarNeighborParams> struct PRM { using Configuration = typename Robot::Configuration; diff --git a/src/impl/vamp/planning/roadmap.hh b/src/impl/vamp/planning/roadmap.hh index 7f65476b..2659b886 100644 --- a/src/impl/vamp/planning/roadmap.hh +++ b/src/impl/vamp/planning/roadmap.hh @@ -1,10 +1,10 @@ #pragma once -#include #include #include #include #include +#include #include #include @@ -76,6 +76,34 @@ namespace vamp::planning double gamma_scale = 2.0; }; + struct FCITStarNeighborParams + { + explicit FCITStarNeighborParams(std::size_t dim, double space_measure) noexcept + : dim(dim), space_measure(space_measure) + { + } + + [[nodiscard]] inline constexpr auto + max_neighbors(std::size_t num_states) const noexcept -> std::size_t + { + return std::numeric_limits::max(); + } + + [[nodiscard]] inline auto neighbor_radius(std::size_t num_states) const noexcept -> float + { + return std::numeric_limits::infinity(); + } + + [[nodiscard]] inline constexpr auto dimension() const noexcept -> double + { + return static_cast(dim); + } + + std::size_t dim; + double space_measure; + double gamma_scale = 2.0; + }; + struct FMTStarNeighborParams { explicit FMTStarNeighborParams(std::size_t dim, double space_measure) @@ -138,6 +166,8 @@ namespace vamp::planning std::size_t max_iterations = 100000; std::size_t max_samples = 100000; + std::size_t batch_size = 1000; + bool optimize = false; NeighborParams neighbor_params; }; diff --git a/src/impl/vamp/planning/simplify.hh b/src/impl/vamp/planning/simplify.hh index cca41e0c..613ed560 100644 --- a/src/impl/vamp/planning/simplify.hh +++ b/src/impl/vamp/planning/simplify.hh @@ -229,16 +229,25 @@ namespace vamp::planning } result.path = path; + + if (settings.interpolate) + { + result.path.interpolate(settings.interpolate); + } + if (path.size() > 2) { for (auto i = 0U; i < settings.max_iterations; ++i) { result.iterations++; - if (not std::all_of( - settings.operations.cbegin(), - settings.operations.cend(), - [&](const auto &op) { return operations.find(op)->second(); })) + bool any = false; + for (const auto &op : settings.operations) + { + any |= operations.find(op)->second(); + } + + if (not any) { break; } diff --git a/src/impl/vamp/planning/simplify_settings.hh b/src/impl/vamp/planning/simplify_settings.hh index 31262ad3..857a431a 100644 --- a/src/impl/vamp/planning/simplify_settings.hh +++ b/src/impl/vamp/planning/simplify_settings.hh @@ -41,6 +41,7 @@ namespace vamp::planning struct SimplifySettings { std::size_t max_iterations{5}; + std::size_t interpolate{0}; std::vector operations{{SHORTCUT, BSPLINE}}; ReduceSettings reduce; diff --git a/src/impl/vamp/planning/utils.hh b/src/impl/vamp/planning/utils.hh index b7893277..4cbb15fb 100644 --- a/src/impl/vamp/planning/utils.hh +++ b/src/impl/vamp/planning/utils.hh @@ -161,6 +161,25 @@ namespace vamp::planning::utils std::reverse(path.begin(), path.end()); } + template + inline void recover_path( + const std::vector &parents, + const StateLookupFn &state_index, + Path &path) noexcept + { + // NOTE: Assumes that the start is the first element of the graph and the goal is the second + constexpr const unsigned int start_index = 0; + constexpr const unsigned int goal_index = 1; + auto current_index = goal_index; + while (parents[current_index] != std::numeric_limits::max()) + { + path.emplace_back(state_index(current_index)); + current_index = parents[current_index]; + } + path.emplace_back(state_index(start_index)); + std::reverse(path.begin(), path.end()); + } + inline auto recover_path_indices(const std::unique_ptr parents) noexcept -> std::vector { diff --git a/src/vamp/__init__.py b/src/vamp/__init__.py index c36d43d1..a35c9e84 100644 --- a/src/vamp/__init__.py +++ b/src/vamp/__init__.py @@ -17,6 +17,8 @@ "RRTCSettings", "PRMSettings", "PRMNeighborParams", + "FCITSettings", + "FCITNeighborParams", "SimplifySettings", "SimplifyRoutine", "filter_pointcloud" @@ -37,6 +39,8 @@ from ._core import PRMNeighborParams as PRMNeighborParams from ._core import PRMSettings as PRMSettings from ._core import RRTCSettings as RRTCSettings +from ._core import FCITNeighborParams as FCITNeighborParams +from ._core import FCITSettings as FCITSettings from ._core import SimplifyRoutine as SimplifyRoutine from ._core import SimplifySettings as SimplifySettings from ._core import Sphere as Sphere @@ -83,6 +87,10 @@ def configure_robot_and_planner_with_kwargs(robot_name: str, planner_name: str, plan_settings.range = ROBOT_RRT_RANGES[robot_name] elif planner_name == "prm": plan_settings = PRMSettings(PRMNeighborParams(robot_module.dimension(), robot_module.space_measure())) + elif planner_name == "fcit": + plan_settings = FCITSettings( + FCITNeighborParams(robot_module.dimension(), robot_module.space_measure()) + ) else: raise NotImplementedError(f"Automatic setup for planner {planner_name} is not implemented yet!") @@ -97,16 +105,14 @@ def configure_robot_and_planner_with_kwargs(robot_name: str, planner_name: str, simp_settings = SimplifySettings() for k, v in kwargs.items(): - if "simplification_" not in k: - continue - - sk = k.replace("simplification_", "") - if hasattr(simp_settings, sk): - print(f"Setting simplification - {sk}: {v}") - if sk == "operations": - v = [getattr(SimplifyRoutine, r) for r in v] - - setattr(simp_settings, sk, v) + if "simplification_" in k: + sk = k.replace("simplification_", "") + if hasattr(simp_settings, sk): + print(f"Setting simplification - {sk}: {v}") + if sk == "operations": + v = [getattr(SimplifyRoutine, r) for r in v] + + setattr(simp_settings, sk, v) subs = ["reduce", "shortcut", "bspline", "perturb"] for sub in subs: diff --git a/src/vamp/__init__.pyi b/src/vamp/__init__.pyi index e360dd10..47d29cf2 100644 --- a/src/vamp/__init__.pyi +++ b/src/vamp/__init__.pyi @@ -15,6 +15,8 @@ from vamp._core import Environment as Environment from vamp._core import PRMNeighborParams as PRMNeighborParams from vamp._core import PRMSettings as PRMSettings from vamp._core import RRTCSettings as RRTCSettings +from vamp._core import FCITNeighborParams as FCITNeighborParams +from vamp._core import FCITSettings as FCITSettings from vamp._core import SimplifySettings as SimplifySettings from vamp._core import Sphere as Sphere from vamp._core import baxter as baxter