From 0d13c50eb683a1e9d4127ea7fe52387c794e1156 Mon Sep 17 00:00:00 2001 From: Zachary Kingston Date: Fri, 18 Oct 2024 14:32:51 -0400 Subject: [PATCH] feat: RNG is now a planner argument, expose XORShift generation (#29) * 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 * Update scripts/README.md Co-authored-by: Wil Thomason * initial nits * Initial nits * documentation * format --------- Co-authored-by: Wil Thomason --- .github/workflows/format.yml | 6 +- README.md | 13 +- scripts/README.md | 2 + scripts/attachments.py | 5 +- scripts/cpp/ompl_integration.cc | 4 +- scripts/evaluate_mbm.py | 10 +- scripts/flying_sphere.py | 16 +- scripts/sphere_cage_example.py | 13 +- scripts/visualize_mbm.py | 10 +- src/impl/vamp/bindings/common.hh | 146 +++++++++++++----- src/impl/vamp/bindings/environment.cc | 4 +- src/impl/vamp/bindings/settings.cc | 4 +- src/impl/vamp/collision/capt.hh | 3 - src/impl/vamp/collision/factory.hh | 25 +-- src/impl/vamp/collision/sphere_capsule.hh | 4 +- src/impl/vamp/collision/sphere_cuboid.hh | 4 +- src/impl/vamp/collision/validity.hh | 4 +- src/impl/vamp/planning/nn.hh | 4 +- src/impl/vamp/planning/prm.hh | 22 +-- src/impl/vamp/planning/roadmap.hh | 13 +- src/impl/vamp/planning/rrtc.hh | 16 +- src/impl/vamp/planning/rrtc_settings.hh | 1 - src/impl/vamp/planning/simplify.hh | 35 ++--- src/impl/vamp/planning/utils.hh | 8 +- .../random/{uniform.hh => distribution.hh} | 5 +- src/impl/vamp/random/fastrand.hh | 39 ----- src/impl/vamp/random/halton.hh | 62 +++++--- src/impl/vamp/random/rng.hh | 18 +++ src/impl/vamp/random/xorshift.hh | 31 ++-- src/impl/vamp/robots/ur5/fk.hh | 48 +++--- src/impl/vamp/utils.hh | 8 +- src/impl/vamp/vector/interface.hh | 12 +- src/impl/vamp/vector/utils.hh | 9 +- 33 files changed, 350 insertions(+), 254 deletions(-) rename src/impl/vamp/random/{uniform.hh => distribution.hh} (93%) delete mode 100644 src/impl/vamp/random/fastrand.hh create mode 100644 src/impl/vamp/random/rng.hh diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index f7b9ac3..74a3993 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -30,13 +30,13 @@ jobs: run: | wget https://apt.llvm.org/llvm.sh chmod +x llvm.sh - yes | sudo ./llvm.sh 16 || true - sudo apt-get install -y clang-format-16 python3-pip + yes | sudo ./llvm.sh 18 || true + sudo apt-get install -y clang-format-18 python3-pip pip install yapf - name: Run clang-format style check. run: > - find . -iname *.hh -o -iname *.cc | xargs -I{} clang-format-16 --dry-run -Werror {} + find . -iname *.hh -o -iname *.cc | xargs -I{} clang-format-18 --dry-run -Werror {} - name: Run yapf style check. run: > diff --git a/README.md b/README.md index f8e9871..1a2b715 100644 --- a/README.md +++ b/README.md @@ -75,6 +75,13 @@ python scripts/sphere_cage_example.py --visualize Which will benchmark a simple scenario of the Franka Emika Panda in a cage of spheres and visualize one of the results. See the [README in the scripts directory](scripts/README.md) for more details. +#### Incremental Rebuilds +Rather than building the entire library from scratch each time, `nanobind` supports [incremental rebuilds](https://nanobind.readthedocs.io/en/latest/packaging.html#step-5-incremental-rebuilds): +```bash +cd vamp +pip install --no-build-isolation -Ceditable.rebuild=true -ve . +``` + ### C++ If you wish to extend `vamp` via C++, please build directly with CMake, e.g.: ``` @@ -135,13 +142,17 @@ For the flying sphere in $\mathbb{R}^3$, additional operations are available to - `vamp.sphere.set_lows()` and `vamp.sphere.set_highs()` to set bounding box of space - `vamp.sphere.set_radius()` to set the sphere's radius +## Supported RNG +We ship implementations of the following pseudorandom number generators (PRNGs): +- `halton`: An implementation of a [multi-dimensional Halton sequence](https://en.wikipedia.org/wiki/Halton_sequence) [[12-13]](#12). +- `xorshift`: A SIMD-accelerated implementation of an [XOR shift](https://en.wikipedia.org/wiki/Xorshift) generator, only available on x86 machines. Uses the [`SIMDxorshift`](https://github.com/lemire/SIMDxorshift) library. + ## Supported Planners 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.). Note that these planners support planning to a set of goals, not just a single goal. -Also, all planners use a multi-dimensional Halton sequence for deterministic planning [[12-13]](#12). We also ship a number of heuristic simplification routines: - randomized and deterministic shortcutting [[8, 9]](#8) (`REDUCE` and `SHORTCUT`) diff --git a/scripts/README.md b/scripts/README.md index 6d8993a..2d5dbef 100644 --- a/scripts/README.md +++ b/scripts/README.md @@ -39,6 +39,8 @@ These scripts support standard planner and simplifier configuration arguments (s In addition, they both support the following arguments: - `problem`: which takes in either a single problem name (e.g., `table_pick`) or a list (e.g., `table_pick,table_under_pick`) to evaluate against a specific set of problems. - `dataset`: which describes the specific dataset of problems that should be loaded and inspected. See [Datasets](../resources/README.md#supported-planners) for more information. +- `sampler_name`: specify which PRNG sampler to use for generating random configurations (see [supported RNGs](../README.md#supported-rng)). +- `skip_rng_iterations`: skip _n_ PRNG iterations. Can be used to "seed" trials differently. - `pointcloud`: construct a pointcloud approximation of the problem's scene geometry and plan against this representation using the CAPT datastructure. - `samples_per_object`: number of samples per each object for pointcloud generation. - `filter_radius`: pointcloud filtering radius. Will remove all redundant points that are within the specified radius. diff --git a/scripts/attachments.py b/scripts/attachments.py index ea0aec8..45731cc 100644 --- a/scripts/attachments.py +++ b/scripts/attachments.py @@ -70,8 +70,9 @@ def callback(configuration): sim.update_object_position(attachment_sphere, sphere.position) # Plan and display - result = planner_func(a, b, e, plan_settings) - simple = vamp_module.simplify(result.path, e, simp_settings) + sampler = vamp_module.halton() + result = planner_func(a, b, e, plan_settings, sampler) + simple = vamp_module.simplify(result.path, e, simp_settings, sampler) simple.path.interpolate(vamp.panda.resolution()) sim.animate(simple.path, callback) diff --git a/scripts/cpp/ompl_integration.cc b/scripts/cpp/ompl_integration.cc index 29dad93..a9e00fe 100644 --- a/scripts/cpp/ompl_integration.cc +++ b/scripts/cpp/ompl_integration.cc @@ -130,8 +130,8 @@ struct VAMPMotionValidator : public ob::MotionValidator ompl_to_vamp(s1), ompl_to_vamp(s2), env_v); } - auto checkMotion(const ob::State *, const ob::State *, std::pair &) const - -> bool override + auto + checkMotion(const ob::State *, const ob::State *, std::pair &) const -> bool override { throw ompl::Exception("Not implemented!"); } diff --git a/scripts/evaluate_mbm.py b/scripts/evaluate_mbm.py index ab08a43..b188833 100644 --- a/scripts/evaluate_mbm.py +++ b/scripts/evaluate_mbm.py @@ -17,6 +17,8 @@ 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. + 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 samples_per_object: int = 10000, # If pointcloud, samples per object to use @@ -48,6 +50,8 @@ 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)() + total_problems = 0 valid_problems = 0 failed_problems = 0 @@ -88,13 +92,15 @@ def main( else: env = vamp.problem_dict_to_vamp(data) + sampler.reset() + sampler.skip(skip_rng_iterations) for _ in range(trials): - result = planner_func(data['start'], data['goals'], env, plan_settings) + result = planner_func(data['start'], data['goals'], env, plan_settings, sampler) if not result.solved: failures.append(i) break - simple = vamp_module.simplify(result.path, env, simp_settings) + simple = vamp_module.simplify(result.path, env, simp_settings, sampler) trial_result = vamp.results_to_dict(result, simple) if pointcloud: diff --git a/scripts/flying_sphere.py b/scripts/flying_sphere.py index 5478549..ddd507e 100644 --- a/scripts/flying_sphere.py +++ b/scripts/flying_sphere.py @@ -8,12 +8,13 @@ def main( - visualize: bool = False, - x: float = 20, - y: float = 20, - z: float = 1, - radius: float = 0.1, - iterations: int = 10000 + visualize: bool = False, + x: float = 20, + y: float = 20, + z: float = 1, + radius: float = 0.1, + iterations: int = 10000, + sampler_name: str = "halton", ): env = vamp.Environment() @@ -33,7 +34,8 @@ def main( settings = vamp.PRMSettings(vamp.PRMNeighborParams(vamp.sphere.dimension(), vamp.sphere.space_measure())) settings.max_iterations = iterations - roadmap = vamp.sphere.roadmap(robot_initial_pos, goal_pos, env, settings) + sampler = getattr(vamp.sphere, sampler_name)() + roadmap = vamp.sphere.roadmap(robot_initial_pos, goal_pos, env, settings, sampler) print(f"Created roadmap with {len(roadmap)} nodes in {roadmap.nanoseconds / 1e9} seconds!") diff --git a/scripts/sphere_cage_example.py b/scripts/sphere_cage_example.py index c314f0c..a40fa76 100644 --- a/scripts/sphere_cage_example.py +++ b/scripts/sphere_cage_example.py @@ -38,12 +38,17 @@ def main( radius: float = 0.2, visualize: bool = False, planner: str = "rrtc", + sampler_name: str = "halton", # Sampler to use. + skip_rng_iterations: int = 0, # Skip a number of RNG iterations **kwargs, ): (vamp_module, planner_func, plan_settings, simp_settings) = vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs) + sampler = getattr(vamp_module, sampler_name)() + sampler.skip(skip_rng_iterations) + if benchmark: random.seed(0) np.random.seed(0) @@ -60,8 +65,8 @@ def main( e.add_sphere(vamp.Sphere(sphere, radius)) if vamp.panda.validate(a, e) and vamp.panda.validate(b, e): - result = planner_func(a, b, e, plan_settings) - simple = vamp_module.simplify(result.path, e, simp_settings) + result = planner_func(a, b, e, plan_settings, sampler) + simple = vamp_module.simplify(result.path, e, simp_settings, sampler) results.append(vamp.results_to_dict(result, simple)) df = pd.DataFrame.from_dict(results) @@ -94,8 +99,8 @@ def main( e.add_sphere(vamp.Sphere(sphere, radius)) sim.add_sphere(radius, sphere) - result = planner_func(a, b, e, plan_settings) - simple = vamp_module.simplify(result.path, e, simp_settings) + result = planner_func(a, b, e, plan_settings, sampler) + simple = vamp_module.simplify(result.path, e, simp_settings, sampler) simple.path.interpolate(vamp.panda.resolution()) diff --git a/scripts/visualize_mbm.py b/scripts/visualize_mbm.py index 39dcce9..0038e28 100644 --- a/scripts/visualize_mbm.py +++ b/scripts/visualize_mbm.py @@ -15,6 +15,8 @@ def main( dataset: str = "problems.pkl", # Pickled dataset to use problem: str = "", # Problem name index: int = 1, # Problem index + sampler_name: str = "halton", # Sampler to use. + skip_rng_iterations: int = 0, # Skip a number of RNG iterations display_object_names: bool = False, # Display object names over geometry pointcloud: bool = False, # Use pointcloud rather than primitive geometry samples_per_object: int = 10000, # If pointcloud, samples per object to use @@ -77,8 +79,11 @@ def main( goals = problem_data['goals'] valid = problem_data['valid'] + sampler = getattr(vamp_module, sampler_name)() + sampler.skip(skip_rng_iterations) + if valid: - result = planner_func(start, goals, env, plan_settings) + result = planner_func(start, goals, env, plan_settings, sampler) solved = result.solved else: print("Problem is invalid!") @@ -86,7 +91,7 @@ def main( if valid and solved: print("Solved problem!") - simplify = vamp_module.simplify(result.path, env, simp_settings) + simplify = vamp_module.simplify(result.path, env, simp_settings, sampler) stats = vamp.results_to_dict(result, simplify) print( @@ -152,4 +157,5 @@ def main( if __name__ == "__main__": + Fire(main) diff --git a/src/impl/vamp/bindings/common.hh b/src/impl/vamp/bindings/common.hh index 853e001..1dcbcca 100644 --- a/src/impl/vamp/bindings/common.hh +++ b/src/impl/vamp/bindings/common.hh @@ -1,5 +1,14 @@ #pragma once +#include +#include + +#if defined(__x86_64__) +#include +#else +#include +#endif + #include #include #include @@ -86,12 +95,29 @@ namespace vamp::binding using EnvironmentInput = vamp::collision::Environment; using EnvironmentVector = vamp::collision::Environment>; + using RNG = vamp::rng::RNG; using Halton = vamp::rng::Halton; - using PRM = vamp::planning::PRM; - using RRTC = vamp::planning::RRTC; +#if defined(__x86_64__) + using XORShift = vamp::rng::XORShift; +#endif + + using PRM = vamp::planning::PRM; + using RRTC = vamp::planning::RRTC; + + inline static auto halton() -> typename RNG::Ptr + { + return std::make_shared(); + } - inline static auto fk(const ConfigurationArray &configuration) - -> std::vector> +#if defined(__x86_64__) + inline static auto xorshift() -> typename RNG::Ptr + { + return std::make_shared(); + } +#endif + + inline static auto + fk(const ConfigurationArray &configuration) -> std::vector> { typename Robot::template Spheres<1> out; typename Robot::template ConfigurationBlock<1> block; @@ -112,9 +138,9 @@ namespace vamp::binding return result; } - inline static auto - sphere_validate(const ConfigurationArray &configuration, const EnvironmentInput &environment) - -> std::vector> + inline static auto sphere_validate( + const ConfigurationArray &configuration, + const EnvironmentInput &environment) -> std::vector> { auto spheres = fk(configuration); std::vector> result; @@ -130,9 +156,9 @@ namespace vamp::binding return result; } - inline static auto - validate_configuration(const Configuration &configuration, const EnvironmentInput &environment) - -> bool + inline static auto validate_configuration( + const Configuration &configuration, + const EnvironmentInput &environment) -> bool { return vamp::planning::validate_motion( configuration, configuration, EnvironmentVector(environment)); @@ -150,17 +176,19 @@ namespace vamp::binding const ConfigurationArray &start, const ConfigurationArray &goal, const EnvironmentInput &environment, - const vamp::planning::RRTCSettings &settings) -> PlanningResult + const vamp::planning::RRTCSettings &settings, + typename RNG::Ptr rng) -> PlanningResult { return RRTC::solve( - Configuration(start), Configuration(goal), EnvironmentVector(environment), settings); + Configuration(start), Configuration(goal), EnvironmentVector(environment), settings, rng); } inline static auto rrtc( const ConfigurationArray &start, const std::vector &goals, const EnvironmentInput &environment, - const vamp::planning::RRTCSettings &settings) -> PlanningResult + const vamp::planning::RRTCSettings &settings, + typename RNG::Ptr rng) -> PlanningResult { std::vector goals_v; goals_v.reserve(goals.size()); @@ -171,27 +199,27 @@ namespace vamp::binding } const Configuration start_v(start); - return RRTC::solve(start_v, goals_v, EnvironmentVector(environment), settings); + return RRTC::solve(start_v, goals_v, EnvironmentVector(environment), settings, rng); } inline static auto prm_single( const ConfigurationArray &start, const ConfigurationArray &goal, const EnvironmentInput &environment, - const vamp::planning::RoadmapSettings &settings) - -> PlanningResult + const vamp::planning::RoadmapSettings &settings, + typename RNG::Ptr rng) -> PlanningResult { ; return PRM::solve( - Configuration(start), Configuration(goal), EnvironmentVector(environment), settings); + Configuration(start), Configuration(goal), EnvironmentVector(environment), settings, rng); } inline static auto prm(const ConfigurationArray &start, const std::vector &goals, const EnvironmentInput &environment, - const vamp::planning::RoadmapSettings &settings) - -> PlanningResult + const vamp::planning::RoadmapSettings &settings, + typename RNG::Ptr rng) -> PlanningResult { std::vector goals_v; goals_v.reserve(goals.size()); @@ -202,26 +230,28 @@ namespace vamp::binding } const Configuration start_v(start); - return PRM::solve(start_v, goals_v, EnvironmentVector(environment), settings); + return PRM::solve(start_v, goals_v, EnvironmentVector(environment), settings, rng); } inline static auto roadmap( const ConfigurationArray &start, const ConfigurationArray &goal, const EnvironmentInput &environment, - const vamp::planning::RoadmapSettings &settings) -> Roadmap + const vamp::planning::RoadmapSettings &settings, + typename RNG::Ptr rng) -> Roadmap { return PRM::build_roadmap( - Configuration(start), Configuration(goal), EnvironmentVector(environment), settings); + Configuration(start), Configuration(goal), EnvironmentVector(environment), settings, rng); } inline static auto simplify( const Path &path, const EnvironmentInput &environment, - const vamp::planning::SimplifySettings &settings) -> PlanningResult + const vamp::planning::SimplifySettings &settings, + typename RNG::Ptr rng) -> PlanningResult { return vamp::planning::simplify( - path, EnvironmentVector(environment), settings); + path, EnvironmentVector(environment), settings, rng); } inline static auto filter_self_from_pointcloud( @@ -233,8 +263,8 @@ namespace vamp::binding return filter_robot_from_pointcloud(pc, start, environment, point_radius); } - inline static auto eefk(const ConfigurationArray &start) - -> std::pair, std::array> + inline static auto + eefk(const ConfigurationArray &start) -> std::pair, std::array> { const auto &result = Robot::eefk(start); @@ -255,6 +285,31 @@ namespace vamp::binding using RH = Helper; auto submodule = pymodule.def_submodule(Robot::name, "Robot-specific submodule"); + nb::class_(submodule, "RNG", "RNG for robot configurations.") + .def( + "reset", + [](typename RH::RNG::Ptr rng) { rng->reset(); }, + "Reset the RNG to initial state and seed.") + .def( + "next", + [](typename RH::RNG::Ptr rng) + { + auto x = rng->next(); + Robot::scale_configuration(x); + return x; + }, + "Sample the next configuration. Modifies internal RNG state.") + .def( + "skip", + [](typename RH::RNG::Ptr rng, std::size_t n) + { + for (auto i = 0U; i < n; ++i) + { + auto x = rng->next(); + } + }, + "Skip the next n iterations."); + submodule.def( "dimension", []() { return Robot::dimension; }, @@ -265,8 +320,7 @@ namespace vamp::binding "Collision checking resolution for this robot."); submodule.def( "n_spheres", []() { return Robot::n_spheres; }, "Number of spheres in robot collision model."); - submodule.def( - "space_measure", []() { return Robot::space_measure(); }, "Measure "); + submodule.def("space_measure", []() { return Robot::space_measure(); }, "Measure "); nb::class_(submodule, "Configuration", "Robot configuration.") .def(nb::init<>(), "Empty constructor. Zero initialized.") @@ -424,13 +478,23 @@ namespace vamp::binding .def_ro( "iterations", &RH::Roadmap::iterations, "Number of iterations taken to construct roadmap."); + submodule.def("halton", RH::halton, "Creates a new Halton sampler."); + +#if defined(__x86_64__) + submodule.def("xorshift", RH::xorshift, "Creates a new XORShift sampler."); +#else + submodule.def( + "xorshift", []() { throw std::runtime_error("XORShift is not supported on non-x86 systems!"); }); +#endif + submodule.def( "rrtc", RH::rrtc_single, "start"_a, "goal"_a, "environment"_a, - "settings"_a = vamp::planning::RRTCSettings(), + "settings"_a, + "rng"_a, "Solve the motion planning problem with RRTConnect."); submodule.def( @@ -439,7 +503,8 @@ namespace vamp::binding "start"_a, "goal"_a, "environment"_a, - "settings"_a = vamp::planning::RRTCSettings(), + "settings"_a, + "rng"_a, "Solve the motion planning problem with RRTConnect."); submodule.def( @@ -448,8 +513,8 @@ namespace vamp::binding "start"_a, "goal"_a, "environment"_a, - "settings"_a = vamp::planning::RoadmapSettings( - vamp::planning::PRMStarNeighborParams(Robot::dimension, Robot::space_measure())), + "settings"_a, + "rng"_a, "Solve the motion planning problem with PRM."); submodule.def( @@ -458,18 +523,18 @@ namespace vamp::binding "start"_a, "goal"_a, "environment"_a, - "settings"_a = vamp::planning::RoadmapSettings( - vamp::planning::PRMStarNeighborParams(Robot::dimension, Robot::space_measure())), + "settings"_a, + "rng"_a, "Solve the motion planning problem with PRM."); submodule.def( "roadmap", RH::roadmap, - nb::arg("start"), - nb::arg("goal"), - nb::arg("environment"), - nb::arg("settings") = vamp::planning::RoadmapSettings( - vamp::planning::PRMStarNeighborParams(Robot::dimension, Robot::space_measure())), + "start"_a, + "goal"_a, + "environment"_a, + "settings"_a, + "rng"_a, "PRM roadmap construction."); submodule.def( @@ -477,7 +542,8 @@ namespace vamp::binding RH::simplify, "path"_a, "environment"_a, - "settings"_a = vamp::planning::SimplifySettings(), + "settings"_a, + "rng"_a, "Simplification heuristics to post-process a path."); submodule.def( diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc index c942ada..97f334c 100644 --- a/src/impl/vamp/bindings/environment.cc +++ b/src/impl/vamp/bindings/environment.cc @@ -31,9 +31,7 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule) .def_ro("r", &vc::Sphere::r) .def_prop_ro( "position", - [](vc::Sphere &sphere) { - return std::array{sphere.x, sphere.y, sphere.z}; - }) + [](vc::Sphere &sphere) { return std::array{sphere.x, sphere.y, sphere.z}; }) .def_ro("min_distance", &vc::Sphere::min_distance) .def_rw("name", &vc::Sphere::name); diff --git a/src/impl/vamp/bindings/settings.cc b/src/impl/vamp/bindings/settings.cc index e016fba..84a7f65 100644 --- a/src/impl/vamp/bindings/settings.cc +++ b/src/impl/vamp/bindings/settings.cc @@ -21,8 +21,7 @@ void vamp::binding::init_settings(nanobind::module_ &pymodule) .def_rw("tree_ratio", &vp::RRTCSettings::tree_ratio) .def_rw("max_iterations", &vp::RRTCSettings::max_iterations) .def_rw("max_samples", &vp::RRTCSettings::max_samples) - .def_rw("start_tree_first", &vp::RRTCSettings::start_tree_first) - .def_rw("rng_skip_iterations", &vp::RRTCSettings::rng_skip_iterations); + .def_rw("start_tree_first", &vp::RRTCSettings::start_tree_first); // TODO: Redesign a neater form of RoadmapSettings/NeighborParams // TODO: Expose the other NeighborParams types @@ -39,7 +38,6 @@ void vamp::binding::init_settings(nanobind::module_ &pymodule) .def(nb::init()) .def_rw("max_iterations", &PRMStarSettings::max_iterations) .def_rw("max_samples", &PRMStarSettings::max_samples) - .def_rw("rng_skip_iterations", &PRMStarSettings::rng_skip_iterations) .def_rw("neighbor_params", &PRMStarSettings::neighbor_params) .def("max_neighbors", &PRMStarSettings::max_neighbors) .def("neighbor_radius", &PRMStarSettings::neighbor_radius); diff --git a/src/impl/vamp/collision/capt.hh b/src/impl/vamp/collision/capt.hh index 5c22953..83c5e23 100644 --- a/src/impl/vamp/collision/capt.hh +++ b/src/impl/vamp/collision/capt.hh @@ -12,7 +12,6 @@ #include #include -#include #include namespace vamp::collision @@ -135,8 +134,6 @@ namespace vamp::collision } }; - rng::FastRNG rng; - inline auto median_partition( const std::vector &points, std::vector &argsort, diff --git a/src/impl/vamp/collision/factory.hh b/src/impl/vamp/collision/factory.hh index a812f8a..8233045 100644 --- a/src/impl/vamp/collision/factory.hh +++ b/src/impl/vamp/collision/factory.hh @@ -60,9 +60,10 @@ namespace vamp::collision::factory half_extent_z); } - inline static auto - eigen(ConstEigenRef center, ConstEigenRef euler_xyz, ConstEigenRef half_extents) noexcept - -> collision::Cuboid + inline static auto eigen( + ConstEigenRef center, + ConstEigenRef euler_xyz, + ConstEigenRef half_extents) noexcept -> collision::Cuboid { return flat( center[0], @@ -76,17 +77,19 @@ namespace vamp::collision::factory half_extents[2]); }; - inline static auto - eigen_rot(ConstEigenRef center, ConstEigenRotationRef rotation, ConstEigenRef half_extents) noexcept - -> collision::Cuboid + inline static auto eigen_rot( + ConstEigenRef center, + ConstEigenRotationRef rotation, + ConstEigenRef half_extents) noexcept -> collision::Cuboid { auto euler = rotation.toRotationMatrix().eulerAngles(0, 1, 2); return eigen(center, euler, half_extents); }; - inline static auto - array(ConstArrayRef center, ConstArrayRef euler_xyz, ConstArrayRef half_extents) noexcept - -> collision::Cuboid + inline static auto array( + ConstArrayRef center, + ConstArrayRef euler_xyz, + ConstArrayRef half_extents) noexcept -> collision::Cuboid { return flat( center[0], @@ -219,7 +222,7 @@ namespace vamp::collision::factory length); } } // namespace center - } // namespace cylinder + } // namespace cylinder namespace capsule { @@ -339,7 +342,7 @@ namespace vamp::collision::factory length); } } // namespace center - } // namespace capsule + } // namespace capsule namespace sphere { diff --git a/src/impl/vamp/collision/sphere_capsule.hh b/src/impl/vamp/collision/sphere_capsule.hh index 7ee241c..f4c91ec 100644 --- a/src/impl/vamp/collision/sphere_capsule.hh +++ b/src/impl/vamp/collision/sphere_capsule.hh @@ -44,8 +44,8 @@ namespace vamp::collision } template - inline constexpr auto sphere_z_aligned_capsule(const Capsule &c, const Sphere &s) noexcept - -> DataT + inline constexpr auto + sphere_z_aligned_capsule(const Capsule &c, const Sphere &s) noexcept -> DataT { return sphere_z_aligned_capsule(c, s.x, s.y, s.z, s.r); } diff --git a/src/impl/vamp/collision/sphere_cuboid.hh b/src/impl/vamp/collision/sphere_cuboid.hh index 0ba6a04..87fce16 100644 --- a/src/impl/vamp/collision/sphere_cuboid.hh +++ b/src/impl/vamp/collision/sphere_cuboid.hh @@ -52,8 +52,8 @@ namespace vamp::collision } template - inline constexpr auto sphere_z_aligned_cuboid(const Cuboid &c, const Sphere &s) noexcept - -> DataT + inline constexpr auto + sphere_z_aligned_cuboid(const Cuboid &c, const Sphere &s) noexcept -> DataT { return sphere_z_aligned_cuboid(c, s.x, s.y, s.z, s.r * s.r); } diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index b555c13..1778c18 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -249,8 +249,8 @@ namespace vamp } template - inline constexpr auto attachment_environment_collision(const collision::Environment &e) noexcept - -> bool + inline constexpr auto + attachment_environment_collision(const collision::Environment &e) noexcept -> bool { for (const auto &s : e.attachments->posed_spheres) { diff --git a/src/impl/vamp/planning/nn.hh b/src/impl/vamp/planning/nn.hh index 8eb5a71..e57a1e6 100644 --- a/src/impl/vamp/planning/nn.hh +++ b/src/impl/vamp/planning/nn.hh @@ -79,8 +79,8 @@ namespace vamp::planning template struct NNNodeKey { - inline auto operator()(const NNNode &node) const noexcept - -> const NNFloatArray & + inline auto + operator()(const NNNode &node) const noexcept -> const NNFloatArray & { return node.array; } diff --git a/src/impl/vamp/planning/prm.hh b/src/impl/vamp/planning/prm.hh index e0a999a..0b7a0be 100644 --- a/src/impl/vamp/planning/prm.hh +++ b/src/impl/vamp/planning/prm.hh @@ -1,6 +1,5 @@ #pragma once #include -#include #include #include #include @@ -12,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -22,7 +21,6 @@ namespace vamp::planning template < typename Robot, - typename RNG, std::size_t rake, std::size_t resolution, typename NeighborParamsT = PRMStarNeighborParams, @@ -31,21 +29,24 @@ namespace vamp::planning { 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) noexcept -> PlanningResult + const RoadmapSettings &settings, + typename RNG::Ptr rng) noexcept -> PlanningResult { - return solve(start, std::vector{goal}, environment, settings); + 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) noexcept -> PlanningResult + const RoadmapSettings &settings, + typename RNG::Ptr rng) noexcept -> PlanningResult { PlanningResult result; @@ -69,7 +70,6 @@ namespace vamp::planning } } - RNG rng; std::size_t iter = 0; std::vector, float>> neighbors; typename Robot::template ConfigurationBlock temp_block; @@ -109,7 +109,7 @@ namespace vamp::planning while (iter++ < settings.max_iterations and nodes.size() < settings.max_samples) { - auto temp = rng.next(); + auto temp = rng->next(); Robot::scale_configuration(temp); // TODO: This is a gross hack to get around the instruction cache issue...I realized // that edge sampling, while valid, wastes too much effort with our current @@ -200,7 +200,8 @@ namespace vamp::planning const Configuration &start, const Configuration &goal, const collision::Environment> &environment, - const RoadmapSettings &settings) noexcept -> Roadmap + const RoadmapSettings &settings, + typename RNG::Ptr rng) noexcept -> Roadmap { NN roadmap; @@ -209,7 +210,6 @@ namespace vamp::planning auto start_time = std::chrono::steady_clock::now(); - RNG rng; std::size_t iter = 0; std::vector, float>> neighbors; typename Robot::template ConfigurationBlock temp_block; @@ -236,7 +236,7 @@ namespace vamp::planning while (iter++ < settings.max_iterations and nodes.size() < settings.max_samples) { - auto temp = rng.next(); + auto temp = rng->next(); Robot::scale_configuration(temp); // TODO: This is a gross hack to get around the instruction cache issue...I realized diff --git a/src/impl/vamp/planning/roadmap.hh b/src/impl/vamp/planning/roadmap.hh index be9b8dd..7f65476 100644 --- a/src/impl/vamp/planning/roadmap.hh +++ b/src/impl/vamp/planning/roadmap.hh @@ -24,8 +24,8 @@ namespace vamp::planning struct ConstantNeighborParams { - [[nodiscard]] inline auto _max_neighbors_impl(std::size_t /*num_states*/) const noexcept - -> std::size_t + [[nodiscard]] inline auto + _max_neighbors_impl(std::size_t /*num_states*/) const noexcept -> std::size_t { return k; } @@ -46,8 +46,8 @@ namespace vamp::planning { } - [[nodiscard]] inline constexpr auto max_neighbors(std::size_t num_states) const noexcept - -> std::size_t + [[nodiscard]] inline constexpr auto + max_neighbors(std::size_t num_states) const noexcept -> std::size_t { const auto prmstar_constant = vamp::utils::constants::e + (vamp::utils::constants::e / dimension()); @@ -83,8 +83,8 @@ namespace vamp::planning { } - [[nodiscard]] inline constexpr auto max_neighbors(std::size_t num_states) const noexcept - -> std::size_t + [[nodiscard]] inline constexpr auto + max_neighbors(std::size_t num_states) const noexcept -> std::size_t { // NOTE: Adapted from OMPL const auto fmtstar_constant = @@ -138,7 +138,6 @@ namespace vamp::planning std::size_t max_iterations = 100000; std::size_t max_samples = 100000; - std::size_t rng_skip_iterations = 0; NeighborParams neighbor_params; }; diff --git a/src/impl/vamp/planning/rrtc.hh b/src/impl/vamp/planning/rrtc.hh index 16c5cde..bafae1a 100644 --- a/src/impl/vamp/planning/rrtc.hh +++ b/src/impl/vamp/planning/rrtc.hh @@ -7,32 +7,35 @@ #include #include #include -#include +#include #include #include namespace vamp::planning { - template + template struct RRTC { 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 RRTCSettings &settings) noexcept -> PlanningResult + const RRTCSettings &settings, + typename RNG::Ptr rng) noexcept -> PlanningResult { - return solve(start, std::vector{goal}, environment, settings); + 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 RRTCSettings &settings) noexcept -> PlanningResult + const RRTCSettings &settings, + typename RNG::Ptr rng) noexcept -> PlanningResult { PlanningResult result; @@ -75,7 +78,6 @@ namespace vamp::planning auto *tree_a = (settings.start_tree_first) ? &goal_tree : &start_tree; auto *tree_b = (settings.start_tree_first) ? &start_tree : &goal_tree; - RNG rng(settings.rng_skip_iterations); std::size_t iter = 0; std::size_t free_index = start_index + 1; @@ -106,7 +108,7 @@ namespace vamp::planning tree_a_is_start = not tree_a_is_start; } - auto temp = rng.next(); + auto temp = rng->next(); Robot::scale_configuration(temp); typename Robot::ConfigurationBuffer temp_array; diff --git a/src/impl/vamp/planning/rrtc_settings.hh b/src/impl/vamp/planning/rrtc_settings.hh index 5058f75..f48428a 100644 --- a/src/impl/vamp/planning/rrtc_settings.hh +++ b/src/impl/vamp/planning/rrtc_settings.hh @@ -16,7 +16,6 @@ namespace vamp::planning std::size_t max_iterations = 100000; std::size_t max_samples = 100000; - std::size_t rng_skip_iterations = 0; bool start_tree_first = true; }; } // namespace vamp::planning diff --git a/src/impl/vamp/planning/simplify.hh b/src/impl/vamp/planning/simplify.hh index 73a9256..cca41e0 100644 --- a/src/impl/vamp/planning/simplify.hh +++ b/src/impl/vamp/planning/simplify.hh @@ -6,8 +6,7 @@ #include #include #include -#include -#include +#include #include namespace vamp::planning @@ -57,7 +56,8 @@ namespace vamp::planning inline static auto reduce_path_vertices( Path &path, const collision::Environment> &environment, - const ReduceSettings &settings) -> bool + const ReduceSettings &settings, + const typename vamp::rng::RNG::Ptr rng) -> bool { if (path.size() < 3) { @@ -67,8 +67,6 @@ namespace vamp::planning const auto max_steps = (not settings.max_steps) ? path.size() : settings.max_steps; const auto max_empty_steps = (not settings.max_empty_steps) ? path.size() : settings.max_empty_steps; - rng::RNG rng; - bool result = false; for (auto i = 0U, no_change = 0U; i < max_steps or no_change < max_empty_steps; ++i, ++no_change) { @@ -78,9 +76,9 @@ namespace vamp::planning int range = 1 + static_cast( std::floor(0.5F + static_cast(initial_size) * settings.range_ratio)); - auto point_0 = rng.uniform_integer(0, max_n); + auto point_0 = rng->dist.uniform_integer(0, max_n); auto point_1 = - rng.uniform_integer(std::max(point_0 - range, 0), std::min(max_n, point_0 + range)); + rng->dist.uniform_integer(std::max(point_0 - range, 0), std::min(max_n, point_0 + range)); if (std::abs(point_0 - point_1) < 2) { @@ -146,7 +144,8 @@ namespace vamp::planning inline static auto perturb_path( Path &path, const collision::Environment> &environment, - const PerturbSettings &settings) -> bool + const PerturbSettings &settings, + const typename vamp::rng::RNG::Ptr rng) -> bool { if (path.size() < 3) { @@ -156,14 +155,11 @@ namespace vamp::planning const auto max_steps = (not settings.max_steps) ? path.size() : settings.max_steps; const auto max_empty_steps = (not settings.max_empty_steps) ? path.size() : settings.max_empty_steps; - rng::RNG rng; - rng::Halton halton; - bool changed = false; for (auto step = 0U, no_change = 0U; step < max_steps and no_change < max_empty_steps; ++step, ++no_change) { - auto to_perturb_idx = rng.uniform_integer(1UL, path.size() - 2); + auto to_perturb_idx = rng->dist.uniform_integer(1UL, path.size() - 2); auto perturb_state = path[to_perturb_idx]; auto before_state = path[to_perturb_idx - 1]; auto after_state = path[to_perturb_idx + 1]; @@ -172,7 +168,7 @@ namespace vamp::planning for (auto attempt = 0U; attempt < settings.perturbation_attempts; ++attempt) { - auto perturbation = halton.next(); + auto perturbation = rng->next(); Robot::scale_configuration(perturbation); const auto new_state = perturb_state.interpolate(perturbation, settings.range); @@ -197,7 +193,8 @@ namespace vamp::planning inline auto simplify( const Path &path, const collision::Environment> &environment, - const SimplifySettings &settings) -> PlanningResult + const SimplifySettings &settings, + const typename vamp::rng::RNG::Ptr rng) -> PlanningResult { auto start_time = std::chrono::steady_clock::now(); @@ -205,12 +202,14 @@ namespace vamp::planning const auto bspline = [&result, &environment, settings]() { return smooth_bspline(result.path, environment, settings.bspline); }; - const auto reduce = [&result, &environment, settings]() - { return reduce_path_vertices(result.path, environment, settings.reduce); }; + const auto reduce = [&result, &environment, settings, rng]() { + return reduce_path_vertices( + result.path, environment, settings.reduce, rng); + }; const auto shortcut = [&result, &environment, settings]() { return shortcut_path(result.path, environment, settings.shortcut); }; - const auto perturb = [&result, &environment, settings]() - { return perturb_path(result.path, environment, settings.perturb); }; + const auto perturb = [&result, &environment, settings, rng]() + { return perturb_path(result.path, environment, settings.perturb, rng); }; const std::map> operations = { {BSPLINE, bspline}, diff --git a/src/impl/vamp/planning/utils.hh b/src/impl/vamp/planning/utils.hh index e5a6ec8..b789327 100644 --- a/src/impl/vamp/planning/utils.hh +++ b/src/impl/vamp/planning/utils.hh @@ -22,8 +22,8 @@ namespace vamp::planning::utils // Basic union-find // TODO: We may be able to improve these implementations - inline auto find_root(std::vector &components, unsigned int c_idx) noexcept - -> unsigned int + inline auto + find_root(std::vector &components, unsigned int c_idx) noexcept -> unsigned int { while (c_idx != components[c_idx].parent) { @@ -161,8 +161,8 @@ namespace vamp::planning::utils std::reverse(path.begin(), path.end()); } - inline auto recover_path_indices(const std::unique_ptr parents) noexcept - -> std::vector + inline auto + recover_path_indices(const std::unique_ptr parents) noexcept -> std::vector { // 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; diff --git a/src/impl/vamp/random/uniform.hh b/src/impl/vamp/random/distribution.hh similarity index 93% rename from src/impl/vamp/random/uniform.hh rename to src/impl/vamp/random/distribution.hh index 119b3e1..687f687 100644 --- a/src/impl/vamp/random/uniform.hh +++ b/src/impl/vamp/random/distribution.hh @@ -1,15 +1,14 @@ #pragma once #include -#include #include #include namespace vamp::rng { - struct RNG + struct Distribution { - RNG() noexcept : rng(0) + Distribution() noexcept : rng(0) { } diff --git a/src/impl/vamp/random/fastrand.hh b/src/impl/vamp/random/fastrand.hh deleted file mode 100644 index d615b72..0000000 --- a/src/impl/vamp/random/fastrand.hh +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include - -namespace vamp::rng -{ - - struct FastRNG - { - FastRNG() noexcept : seed(0) - { - } - - template - inline auto uniform_integer(T low, T high) -> typename std::enable_if, T>::type - { - auto random = static_cast(std::floor(uniform_real(low, high + 1.0))); - return (random > high) ? high : random; - } - - inline auto uniform_01() -> float - { - return static_cast(fastrand()) / static_cast(std::numeric_limits::max()); - } - - inline auto uniform_real(float low, float high) -> float - { - return (high - low) * uniform_01() + low; - } - - inline auto fastrand() -> uint32_t - { - seed = (214013 * seed + 2531011); - return (seed >> 16) & 0x7FFF; - } - - uint32_t seed{0}; - }; -} // namespace vamp::rng diff --git a/src/impl/vamp/random/halton.hh b/src/impl/vamp/random/halton.hh index 7a1fde2..8f8fe8c 100644 --- a/src/impl/vamp/random/halton.hh +++ b/src/impl/vamp/random/halton.hh @@ -1,12 +1,16 @@ #pragma once -#include +#include +#include namespace vamp::rng { template - struct Halton + struct Halton : public RNG { + // Numerical precision degrades around 1.4M iterations, this value can be increased up to that point. + static constexpr const std::size_t max_iterations = 1000000U; + static constexpr const std::array primes{ 3.F, 5.F, @@ -25,42 +29,58 @@ namespace vamp::rng 53.F, 59.F}; - explicit Halton(FloatVector b_in, std::size_t skip_iterations = 0) noexcept : b(b_in) + explicit Halton(FloatVector b_in) noexcept : b_init(b_in), b(b_in) { - initialize(skip_iterations); } - Halton(std::initializer_list v, std::size_t skip_iterations = 0) noexcept - : Halton(FloatVector::pack_and_pad(v), skip_iterations) + Halton(std::initializer_list v) noexcept : Halton(FloatVector::pack_and_pad(v)) { } - explicit Halton(std::size_t skip_iterations = 0) - : Halton( - []() - { - alignas(FloatVectorAlignment) std::array a; - std::copy_n(primes.cbegin(), dim, a.begin()); - return a; - }(), - skip_iterations) + explicit Halton() : Halton(bases()) { } - inline auto initialize(std::size_t n) noexcept + inline constexpr auto bases() noexcept -> FloatVector { - for (auto i = 0U; i < n; ++i) - { - next(); - } + alignas(FloatVectorAlignment) std::array a; + std::copy_n(primes.cbegin(), dim, a.begin()); + return FloatVector(a); + } + + auto rotate_bases() noexcept + { + alignas(FloatVectorAlignment) std::array a; + b.to_array(a.data()); + std::rotate(a.begin(), a.begin() + 1, a.end()); + b = FloatVector(a); } + const FloatVector b_init; FloatVector b; FloatVector n = FloatVector::fill(0); FloatVector d = FloatVector::fill(1); + std::size_t iterations = 0; - inline auto next() noexcept -> FloatVector + inline void reset() noexcept override final { + iterations = 0; + b = b_init; + n = FloatVector::fill(0); + d = FloatVector::fill(1); + } + + inline auto next() noexcept -> FloatVector override final + { + iterations++; + if (iterations > max_iterations) + { + n = FloatVector::fill(0); + d = FloatVector::fill(1); + iterations = 0; + rotate_bases(); + } + auto xf = d - n; auto x_eq_1 = xf == 1.; auto x_neq_1 = ~x_eq_1; diff --git a/src/impl/vamp/random/rng.hh b/src/impl/vamp/random/rng.hh new file mode 100644 index 0000000..3cdcd60 --- /dev/null +++ b/src/impl/vamp/random/rng.hh @@ -0,0 +1,18 @@ +#pragma once + +#include +#include +#include + +namespace vamp::rng +{ + template + struct RNG + { + using Ptr = std::shared_ptr>; + virtual inline void reset() noexcept = 0; + virtual inline auto next() noexcept -> FloatVector = 0; + + Distribution dist; + }; +} // namespace vamp::rng diff --git a/src/impl/vamp/random/xorshift.hh b/src/impl/vamp/random/xorshift.hh index 9f0c148..fb01413 100644 --- a/src/impl/vamp/random/xorshift.hh +++ b/src/impl/vamp/random/xorshift.hh @@ -5,43 +5,40 @@ extern "C" #include } -#include #include #include +#include namespace vamp::rng { template - struct XORShift + struct XORShift : public RNG { - explicit XORShift(std::size_t skip_iterations = 0) - : XORShift(2, 3UL, 0.F, 1.F, skip_iterations) noexcept - { - } - explicit XORShift( uint64_t key1 = 2UL, uint64_t key2 = 3UL, float min_val = 0.0, - float max_val = 1.0, - std::size_t skip_iterations = 0) noexcept - : min_val{min_val}, max_val{max_val} + float max_val = 1.0) noexcept + : key1_init(key1), key2_init(key2), min_val(min_val), max_val(max_val) { avx_xorshift128plus_init(key1, key2, &key); - - for (auto i = 0U; i < skip_iterations; ++i) - { - next(); - } } - avx_xorshift128plus_key_t key{}; + uint64_t key1_init; + uint64_t key2_init; float min_val; float max_val; + + avx_xorshift128plus_key_t key{}; using IntVector = Vector, 1, dim>; IntVector buffer; - inline auto next() noexcept -> FloatVector + inline void reset() noexcept override final + { + avx_xorshift128plus_init(key1_init, key2_init, &key); + } + + inline auto next() noexcept -> FloatVector override final { for (auto i = 0U; i < IntVector::num_vectors; ++i) { diff --git a/src/impl/vamp/robots/ur5/fk.hh b/src/impl/vamp/robots/ur5/fk.hh index e241a36..90bbd9a 100644 --- a/src/impl/vamp/robots/ur5/fk.hh +++ b/src/impl/vamp/robots/ur5/fk.hh @@ -4432,7 +4432,7 @@ namespace vamp::robots::ur5 { return false; } - } // (555, 583) + } // (555, 583) if (/*forearm_link vs. robotiq_85_left_knuckle_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_1667, ADD_1668, ADD_1669, 0.02)) @@ -4467,7 +4467,7 @@ namespace vamp::robots::ur5 environment, ADD_1667, ADD_1668, ADD_1669, 0.02)) { return false; - } // (583, 583) + } // (583, 583) if (/*shoulder_link vs. robotiq_85_left_knuckle_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_1667, ADD_1668, ADD_1669, 0.02)) @@ -4477,7 +4477,7 @@ namespace vamp::robots::ur5 { return false; } - } // (583, 583) + } // (583, 583) if (/*upper_arm_link vs. robotiq_85_left_knuckle_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_1667, ADD_1668, ADD_1669, 0.02)) @@ -4571,7 +4571,7 @@ namespace vamp::robots::ur5 { return false; } - } // (583, 627) + } // (583, 627) if (/*forearm_link vs. robotiq_85_left_finger_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4157, ADD_4158, ADD_4159, 0.035)) @@ -4667,7 +4667,7 @@ namespace vamp::robots::ur5 { return false; } - } // (627, 627) + } // (627, 627) if (/*shoulder_link vs. robotiq_85_left_finger_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4157, ADD_4158, ADD_4159, 0.035)) @@ -4687,7 +4687,7 @@ namespace vamp::robots::ur5 { return false; } - } // (627, 627) + } // (627, 627) if (/*upper_arm_link vs. robotiq_85_left_finger_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4157, ADD_4158, ADD_4159, 0.035)) @@ -4831,7 +4831,7 @@ namespace vamp::robots::ur5 { return false; } - } // (627, 681) + } // (627, 681) if (/*forearm_link vs. robotiq_85_left_inner_knuckle_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4321, ADD_4322, ADD_4323, 0.02)) @@ -4866,7 +4866,7 @@ namespace vamp::robots::ur5 environment, ADD_4321, ADD_4322, ADD_4323, 0.02)) { return false; - } // (681, 681) + } // (681, 681) if (/*shoulder_link vs. robotiq_85_left_inner_knuckle_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4321, ADD_4322, ADD_4323, 0.02)) @@ -4876,7 +4876,7 @@ namespace vamp::robots::ur5 { return false; } - } // (681, 681) + } // (681, 681) if (/*upper_arm_link vs. robotiq_85_left_inner_knuckle_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4321, ADD_4322, ADD_4323, 0.02)) @@ -4965,7 +4965,7 @@ namespace vamp::robots::ur5 { return false; } - } // (681, 725) + } // (681, 725) if (/*forearm_link vs. robotiq_85_left_finger_tip_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4392, ADD_4393, ADD_4394, 0.033)) @@ -5032,7 +5032,7 @@ namespace vamp::robots::ur5 { return false; } - } // (725, 725) + } // (725, 725) if (/*shoulder_link vs. robotiq_85_left_finger_tip_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4392, ADD_4393, ADD_4394, 0.033)) @@ -5047,7 +5047,7 @@ namespace vamp::robots::ur5 { return false; } - } // (725, 725) + } // (725, 725) if (/*upper_arm_link vs. robotiq_85_left_finger_tip_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4392, ADD_4393, ADD_4394, 0.033)) @@ -5163,7 +5163,7 @@ namespace vamp::robots::ur5 { return false; } - } // (725, 776) + } // (725, 776) if (/*forearm_link vs. robotiq_85_right_inner_knuckle_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4543, ADD_4544, ADD_4545, 0.02)) @@ -5198,7 +5198,7 @@ namespace vamp::robots::ur5 environment, ADD_4543, ADD_4544, ADD_4545, 0.02)) { return false; - } // (776, 776) + } // (776, 776) if (/*shoulder_link vs. robotiq_85_right_inner_knuckle_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4543, ADD_4544, ADD_4545, 0.02)) @@ -5208,7 +5208,7 @@ namespace vamp::robots::ur5 { return false; } - } // (776, 776) + } // (776, 776) if (/*upper_arm_link vs. robotiq_85_right_inner_knuckle_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4543, ADD_4544, ADD_4545, 0.02)) @@ -5297,7 +5297,7 @@ namespace vamp::robots::ur5 { return false; } - } // (776, 820) + } // (776, 820) if (/*forearm_link vs. robotiq_85_right_finger_tip_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4622, ADD_4623, ADD_4624, 0.033)) @@ -5364,7 +5364,7 @@ namespace vamp::robots::ur5 { return false; } - } // (820, 820) + } // (820, 820) if (/*shoulder_link vs. robotiq_85_right_finger_tip_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4622, ADD_4623, ADD_4624, 0.033)) @@ -5379,7 +5379,7 @@ namespace vamp::robots::ur5 { return false; } - } // (820, 820) + } // (820, 820) if (/*upper_arm_link vs. robotiq_85_right_finger_tip_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4622, ADD_4623, ADD_4624, 0.033)) @@ -5462,7 +5462,7 @@ namespace vamp::robots::ur5 { return false; } - } // (820, 838) + } // (820, 838) if (/*forearm_link vs. robotiq_85_right_knuckle_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_2504, ADD_2505, ADD_2506, 0.02)) @@ -5497,7 +5497,7 @@ namespace vamp::robots::ur5 environment, ADD_2504, ADD_2505, ADD_2506, 0.02)) { return false; - } // (838, 838) + } // (838, 838) if (/*shoulder_link vs. robotiq_85_right_knuckle_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_2504, ADD_2505, ADD_2506, 0.02)) @@ -5507,7 +5507,7 @@ namespace vamp::robots::ur5 { return false; } - } // (838, 838) + } // (838, 838) if (/*upper_arm_link vs. robotiq_85_right_knuckle_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_2504, ADD_2505, ADD_2506, 0.02)) @@ -5601,7 +5601,7 @@ namespace vamp::robots::ur5 { return false; } - } // (838, 882) + } // (838, 882) if (/*forearm_link vs. robotiq_85_right_finger_link*/ sphere_sphere_self_collision( ADD_3067, ADD_3068, ADD_3069, 0.265, ADD_4838, ADD_4839, ADD_4840, 0.035)) @@ -5697,7 +5697,7 @@ namespace vamp::robots::ur5 { return false; } - } // (882, 882) + } // (882, 882) if (/*shoulder_link vs. robotiq_85_right_finger_link*/ sphere_sphere_self_collision( 0.0, 0.0, 1.003559, 0.08, ADD_4838, ADD_4839, ADD_4840, 0.035)) @@ -5717,7 +5717,7 @@ namespace vamp::robots::ur5 { return false; } - } // (882, 882) + } // (882, 882) if (/*upper_arm_link vs. robotiq_85_right_finger_link*/ sphere_sphere_self_collision( SUB_2869, ADD_2870, ADD_2871, 0.29, ADD_4838, ADD_4839, ADD_4840, 0.035)) diff --git a/src/impl/vamp/utils.hh b/src/impl/vamp/utils.hh index 9c4e84a..8c54816 100644 --- a/src/impl/vamp/utils.hh +++ b/src/impl/vamp/utils.hh @@ -32,14 +32,14 @@ namespace vamp::utils } // Same deal with div()... - inline constexpr auto c_div(std::size_t idx, std::size_t dim) noexcept - -> std::pair + inline constexpr auto + c_div(std::size_t idx, std::size_t dim) noexcept -> std::pair { return {idx / dim, idx % dim}; } - inline auto get_elapsed_nanoseconds(const std::chrono::time_point &start) - -> std::size_t + inline auto + get_elapsed_nanoseconds(const std::chrono::time_point &start) -> std::size_t { return std::chrono::duration_cast(std::chrono::steady_clock::now() - start) .count(); diff --git a/src/impl/vamp/vector/interface.hh b/src/impl/vamp/vector/interface.hh index 0446546..b68d016 100644 --- a/src/impl/vamp/vector/interface.hh +++ b/src/impl/vamp/vector/interface.hh @@ -59,8 +59,8 @@ namespace vamp inline static constexpr std::size_t num_rows = Sig::num_rows; using DataT = typename Sig::DataT; - inline constexpr auto to_array() const noexcept - -> std::array + inline constexpr auto + to_array() const noexcept -> std::array { alignas(S::Alignment) std::array result = {}; to_array(result); @@ -459,8 +459,14 @@ namespace vamp template inline static constexpr auto map_to_range(OtherT v, BoundsT min_v, BoundsT max_v) noexcept -> D { + constexpr typename S::ScalarT lo = -0.5F; + constexpr typename S::ScalarT hi = 0.5F; + + // maps [-INT_MAX, INT_MAX] to [-0.5, 0.5] const auto normalized = D(apply>(v.data)); - return min_v + (normalized * (max_v - min_v)); + + // adjust to desired range + return min_v + ((normalized - lo) / (hi - lo)) * (max_v - min_v); } template < diff --git a/src/impl/vamp/vector/utils.hh b/src/impl/vamp/vector/utils.hh index 6e8e3fc..434b039 100644 --- a/src/impl/vamp/vector/utils.hh +++ b/src/impl/vamp/vector/utils.hh @@ -84,8 +84,8 @@ namespace vamp } template - inline static auto run(std::index_sequence, const DataT l_data, const DataT r_data) noexcept - -> RetT + inline static auto + run(std::index_sequence, const DataT l_data, const DataT r_data) noexcept -> RetT { RetT result; (..., void(result[I] = fn(std::get(l_data), std::get(r_data)))); @@ -122,8 +122,9 @@ namespace vamp template inline static auto - run(std::index_sequence, const IndexT i_data, const FixedArgT... fixed_args) noexcept - -> RetT + run(std::index_sequence, + const IndexT i_data, + const FixedArgT... fixed_args) noexcept -> RetT { RetT result; (..., void(result[I] = fn(std::get(i_data), fixed_args...)));