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..f9d82dd 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`: An implementation an [XOR shift](https://en.wikipedia.org/wiki/Xorshift) generator, only available on x86 machines. + ## 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..6eb0691 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`: specify which PRNG sampler to use for generating random configurations (see [Supported RNG](../README.md#supported-rng)). +- `skip_rng_iterations`: skip $n$ random numbers to offset the PRNG sampler. - `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..eb67f32 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: 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)() + 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..04a26eb 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: 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)() + 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..98e581e 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: 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)() + 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..7b0dd6d 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: 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)() + 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..6309227 100644 --- a/src/impl/vamp/bindings/common.hh +++ b/src/impl/vamp/bindings/common.hh @@ -1,5 +1,12 @@ #pragma once +#include +#include + +#if defined(__x86_64__) +#include +#endif + #include #include #include @@ -86,12 +93,28 @@ 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; - inline static auto fk(const ConfigurationArray &configuration) - -> std::vector> + using PRM = vamp::planning::PRM; + using RRTC = vamp::planning::RRTC; + + inline static auto halton() -> typename RNG::Ptr + { + return std::make_shared(); + } + +#if defined(__x86_64__) + using XORShift = vamp::rng::XORShift; + + 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 +135,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 +153,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 +173,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 +196,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 +227,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 +260,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 +282,29 @@ 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.") + .def( + "next", + [](typename RH::RNG::Ptr &rng) + { + auto x = rng->next(); + Robot::scale_configuration(x); + return x; + }, + "Sample the next configuration.") + .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 +315,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 +473,20 @@ 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."); +#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 +495,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 +505,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 +515,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 +534,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..e636470 100644 --- a/src/impl/vamp/planning/prm.hh +++ b/src/impl/vamp/planning/prm.hh @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include @@ -22,7 +22,6 @@ namespace vamp::planning template < typename Robot, - typename RNG, std::size_t rake, std::size_t resolution, typename NeighborParamsT = PRMStarNeighborParams, @@ -31,21 +30,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 +71,6 @@ namespace vamp::planning } } - RNG rng; std::size_t iter = 0; std::vector, float>> neighbors; typename Robot::template ConfigurationBlock temp_block; @@ -109,7 +110,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 +201,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 +211,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 +237,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..d1023aa 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..dfc4de0 100644 --- a/src/impl/vamp/planning/simplify.hh +++ b/src/impl/vamp/planning/simplify.hh @@ -67,7 +67,7 @@ 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::Random random; bool result = false; for (auto i = 0U, no_change = 0U; i < max_steps or no_change < max_empty_steps; ++i, ++no_change) @@ -78,9 +78,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 = random.uniform_integer(0, max_n); auto point_1 = - rng.uniform_integer(std::max(point_0 - range, 0), std::min(max_n, point_0 + range)); + random.uniform_integer(std::max(point_0 - range, 0), std::min(max_n, point_0 + range)); if (std::abs(point_0 - point_1) < 2) { @@ -146,6 +146,7 @@ namespace vamp::planning inline static auto perturb_path( Path &path, const collision::Environment> &environment, + const typename vamp::rng::RNG::Ptr &rng, const PerturbSettings &settings) -> bool { if (path.size() < 3) @@ -156,14 +157,13 @@ 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; + rng::Random random; 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 = random.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 +172,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 +197,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(); @@ -209,8 +210,8 @@ namespace vamp::planning { return reduce_path_vertices(result.path, environment, settings.reduce); }; 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, rng, settings]() + { return perturb_path(result.path, environment, rng, settings.perturb); }; 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/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..ee95543 100644 --- a/src/impl/vamp/random/halton.hh +++ b/src/impl/vamp/random/halton.hh @@ -1,12 +1,15 @@ #pragma once -#include +#include +#include namespace vamp::rng { template - struct Halton + struct Halton : public RNG { + static constexpr const std::size_t max_iterations = 1000000U; + static constexpr const std::array primes{ 3.F, 5.F, @@ -25,42 +28,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); } + std::size_t iterations = 0; + FloatVector b_init; FloatVector b; FloatVector n = FloatVector::fill(0); FloatVector d = FloatVector::fill(1); - inline auto next() noexcept -> FloatVector + inline void reset() noexcept override { + iterations = 0; + b = b_init; + n = FloatVector::fill(0); + d = FloatVector::fill(1); + } + + inline auto next() noexcept -> FloatVector override + { + 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..09f6f11 --- /dev/null +++ b/src/impl/vamp/random/rng.hh @@ -0,0 +1,15 @@ +#pragma once + +#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; + }; +} // namespace vamp::rng diff --git a/src/impl/vamp/random/uniform.hh b/src/impl/vamp/random/uniform.hh index 119b3e1..4fcb56f 100644 --- a/src/impl/vamp/random/uniform.hh +++ b/src/impl/vamp/random/uniform.hh @@ -7,9 +7,9 @@ namespace vamp::rng { - struct RNG + struct Random { - RNG() noexcept : rng(0) + Random() noexcept : rng(0) { } diff --git a/src/impl/vamp/random/xorshift.hh b/src/impl/vamp/random/xorshift.hh index 9f0c148..c0e1152 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 + { + avx_xorshift128plus_init(key1_init, key2_init, &key); + } + + inline auto next() noexcept -> FloatVector override { 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..02b5346 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,11 @@ namespace vamp template inline static constexpr auto map_to_range(OtherT v, BoundsT min_v, BoundsT max_v) noexcept -> D { + constexpr float lo = -0.5F; + constexpr float hi = 0.5F; const auto normalized = D(apply>(v.data)); - return min_v + (normalized * (max_v - min_v)); + + 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...)));