diff --git a/src/impl/vamp/bindings/python/robot_helper.hh b/src/impl/vamp/bindings/python/robot_helper.hh index b4d1cef4..553e6a5e 100644 --- a/src/impl/vamp/bindings/python/robot_helper.hh +++ b/src/impl/vamp/bindings/python/robot_helper.hh @@ -516,18 +516,19 @@ namespace vamp::binding "numpy", [](const Path &p) noexcept { + constexpr auto state_dimension = Robot::State::num_scalars; auto *path_arr = new FloatT - [Robot::dimension * p.size() + - (Robot::Configuration::num_scalars_rounded - Robot::dimension)]; + [state_dimension * p.size() + + (Robot::State::num_scalars_rounded - state_dimension)]; for (auto i = 0U; i < p.size(); ++i) { - p[i].to_array_unaligned(path_arr + i * Robot::dimension); + p[i].to_array_unaligned(path_arr + i * state_dimension); } nb::capsule arr_owner( path_arr, [](void *a) noexcept { delete[] reinterpret_cast(a); }); return nb::ndarray( - path_arr, {p.size(), Robot::dimension}, arr_owner); + path_arr, {p.size(), state_dimension}, arr_owner); }, "Convert this path to a numpy matrix."); diff --git a/src/impl/vamp/planning/aorrtc.hh b/src/impl/vamp/planning/aorrtc.hh index 692ac40c..3274da04 100644 --- a/src/impl/vamp/planning/aorrtc.hh +++ b/src/impl/vamp/planning/aorrtc.hh @@ -20,8 +20,8 @@ namespace vamp::planning template struct AOX_RRTC { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; using NNNode = GNATNode; @@ -356,8 +356,8 @@ namespace vamp::planning template struct AORRTC { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; using AOX_RRTC = typename vamp::planning::AOX_RRTC; using RRTC = typename vamp::planning::RRTC; diff --git a/src/impl/vamp/planning/fcit.hh b/src/impl/vamp/planning/fcit.hh index 23fd65dc..d7692410 100644 --- a/src/impl/vamp/planning/fcit.hh +++ b/src/impl/vamp/planning/fcit.hh @@ -65,8 +65,8 @@ namespace vamp::planning typename NeighborParamsT = FCITStarNeighborParams> struct FCIT { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; inline static auto solve( @@ -92,7 +92,7 @@ namespace vamp::planning NN roadmap; std::size_t iter = 0; - typename Robot::template ConfigurationBlock temp_block; + typename Robot::template StateBlock temp_block; auto states = std::unique_ptr( vamp::utils::vector_alloc( settings.max_samples * Configuration::num_scalars_rounded), diff --git a/src/impl/vamp/planning/grrtstar.hh b/src/impl/vamp/planning/grrtstar.hh index 46b14b5f..73d0fbfd 100644 --- a/src/impl/vamp/planning/grrtstar.hh +++ b/src/impl/vamp/planning/grrtstar.hh @@ -48,8 +48,8 @@ namespace vamp::planning template struct GRRTStar { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; using NNNodeType = NNNode; using NNTree = NN; @@ -343,7 +343,7 @@ namespace vamp::planning continue; } - typename Robot::ConfigurationBuffer temp_array; + typename Robot::StateBuffer temp_array; temp.to_array(temp_array.data()); auto nearest_result = tree_a->nearest(NNFloatArray{temp_array.data()}); diff --git a/src/impl/vamp/planning/phs.hh b/src/impl/vamp/planning/phs.hh index 48e4f2b1..f468eeb8 100644 --- a/src/impl/vamp/planning/phs.hh +++ b/src/impl/vamp/planning/phs.hh @@ -35,7 +35,7 @@ namespace vamp::planning template class ProlateHyperspheroid { - static constexpr auto dimension = Robot::dimension; + static constexpr auto dimension = Robot::State::num_scalars; EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: @@ -160,7 +160,7 @@ namespace vamp::planning rng->dist.reset(); } - inline auto next() noexcept -> FloatVector override + inline auto next() noexcept -> FloatVector override { auto x = phs.transform(uniform_in_ball()); @@ -172,22 +172,22 @@ namespace vamp::planning return x; } - inline auto logit() noexcept -> vamp::FloatVector + inline auto logit() noexcept -> vamp::FloatVector { auto U1 = rng->next(); Robot::descale_configuration(U1); return (U1 * (1 - U1).rcp()).log() * std::sqrt(vamp::utils::constants::pi / 8.F); } - inline auto uniform_on_ball() noexcept -> vamp::FloatVector + inline auto uniform_on_ball() noexcept -> vamp::FloatVector { const auto unv = logit().trim(); return unv / unv.l2_norm(); } - inline auto uniform_in_ball() noexcept -> vamp::FloatVector + inline auto uniform_in_ball() noexcept -> vamp::FloatVector { - return std::pow(rng->dist.uniform_01(), 1.0 / static_cast(Robot::dimension)) * + return std::pow(rng->dist.uniform_01(), 1.0 / static_cast(Robot::State::num_scalars)) * uniform_on_ball(); } diff --git a/src/impl/vamp/planning/plan.hh b/src/impl/vamp/planning/plan.hh index 7b9ac60f..6849fda1 100644 --- a/src/impl/vamp/planning/plan.hh +++ b/src/impl/vamp/planning/plan.hh @@ -8,7 +8,7 @@ namespace vamp::planning { template - struct Path : public std::vector> + struct Path : public std::vector> { [[nodiscard]] inline auto cost() const noexcept -> float { @@ -181,7 +181,7 @@ namespace vamp::planning template struct Roadmap { - std::vector> vertices; + std::vector> vertices; std::vector> edges; std::size_t nanoseconds{0}; std::size_t iterations{0}; diff --git a/src/impl/vamp/planning/prm.hh b/src/impl/vamp/planning/prm.hh index 94d68e8f..a0433641 100644 --- a/src/impl/vamp/planning/prm.hh +++ b/src/impl/vamp/planning/prm.hh @@ -26,8 +26,8 @@ namespace vamp::planning typename NeighborParamsT = PRMStarNeighborParams> struct PRM { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; inline static auto solve( @@ -71,7 +71,7 @@ namespace vamp::planning std::size_t iter = 0; std::vector, float>> neighbors; - typename Robot::template ConfigurationBlock temp_block; + typename Robot::template StateBlock temp_block; auto states = std::unique_ptr( vamp::utils::vector_alloc( settings.max_samples * Configuration::num_scalars_rounded)); @@ -211,7 +211,7 @@ namespace vamp::planning std::size_t iter = 0; std::vector, float>> neighbors; - typename Robot::template ConfigurationBlock temp_block; + typename Robot::template StateBlock temp_block; auto states = std::unique_ptr( vamp::utils::vector_alloc( settings.max_samples * Configuration::num_scalars_rounded), diff --git a/src/impl/vamp/planning/rrtc.hh b/src/impl/vamp/planning/rrtc.hh index 7c769d63..9ed13c01 100644 --- a/src/impl/vamp/planning/rrtc.hh +++ b/src/impl/vamp/planning/rrtc.hh @@ -16,8 +16,8 @@ namespace vamp::planning template struct RRTC { - using Configuration = typename Robot::Configuration; - static constexpr auto dimension = Robot::dimension; + using Configuration = typename Robot::State; + static constexpr auto dimension = Robot::State::num_scalars; using RNG = typename vamp::rng::RNG; inline static auto solve( @@ -108,7 +108,7 @@ namespace vamp::planning } auto temp = rng->next(); - typename Robot::ConfigurationBuffer temp_array; + typename Robot::StateBuffer temp_array; temp.to_array(temp_array.data()); const auto nearest = tree_a->nearest(NNFloatArray{temp_array.data()}); diff --git a/src/impl/vamp/random/halton.hh b/src/impl/vamp/random/halton.hh index cb6a53af..429b029d 100644 --- a/src/impl/vamp/random/halton.hh +++ b/src/impl/vamp/random/halton.hh @@ -11,7 +11,7 @@ namespace vamp::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; - using Configuration = typename Robot::Configuration; + using Configuration = typename Robot::State; static constexpr const std::array primes{ 3.F, @@ -47,8 +47,8 @@ namespace vamp::rng inline constexpr auto bases() noexcept -> Configuration { - alignas(FloatVectorAlignment) std::array a; - std::copy_n(primes.cbegin(), Robot::dimension, a.begin()); + alignas(FloatVectorAlignment) std::array a; + std::copy_n(primes.cbegin(), Robot::State::num_scalars, a.begin()); return Configuration(a); } @@ -56,7 +56,7 @@ namespace vamp::rng { alignas(FloatVectorAlignment) std::array a; b.to_array(a.data()); - std::rotate(a.begin(), a.begin() + 1, a.begin() + Robot::dimension); + std::rotate(a.begin(), a.begin() + 1, a.begin() + Robot::State::num_scalars); b = Configuration(a.data()); } diff --git a/src/impl/vamp/random/rng.hh b/src/impl/vamp/random/rng.hh index d3b1ea76..e35dbd20 100644 --- a/src/impl/vamp/random/rng.hh +++ b/src/impl/vamp/random/rng.hh @@ -11,7 +11,7 @@ namespace vamp::rng { using Ptr = std::shared_ptr>; virtual inline void reset() noexcept = 0; - virtual inline auto next() noexcept -> FloatVector = 0; + virtual inline auto next() noexcept -> FloatVector = 0; Distribution dist; }; diff --git a/src/impl/vamp/random/xorshift.hh b/src/impl/vamp/random/xorshift.hh index cd307a52..e45154e9 100644 --- a/src/impl/vamp/random/xorshift.hh +++ b/src/impl/vamp/random/xorshift.hh @@ -26,7 +26,7 @@ namespace vamp::rng uint64_t key2_init; avx_xorshift128plus_key_t key{}; - using IntVector = Vector, 1, Robot::dimension>; + using IntVector = Vector, 1, Robot::State::num_scalars>; IntVector buffer; inline void reset() noexcept override final @@ -34,14 +34,14 @@ namespace vamp::rng avx_xorshift128plus_init(key1_init, key2_init, &key); } - inline auto next() noexcept -> FloatVector override final + inline auto next() noexcept -> FloatVector override final { for (auto i = 0U; i < IntVector::num_vectors; ++i) { buffer.data[i] = avx_xorshift128plus(&key); } - auto result = FloatVector::map_to_range(buffer, 0.F, 1.F); + auto result = FloatVector::map_to_range(buffer, 0.F, 1.F); Robot::scale_configuration(result); return result; } diff --git a/src/impl/vamp/robots/baxter.hh b/src/impl/vamp/robots/baxter.hh index 8ab0783b..719eba85 100644 --- a/src/impl/vamp/robots/baxter.hh +++ b/src/impl/vamp/robots/baxter.hh @@ -45,6 +45,15 @@ namespace vamp::robots template using ConfigurationBlock = FloatVector; + // Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over. + // Defaults to Configuration; robots that plan in a different space (e.g. task space) can + // redefine this family independently of the FK/FKCC-facing Configuration types above. + using State = Configuration; + using StateArray = ConfigurationArray; + using StateBuffer = ConfigurationBuffer; + template + using StateBlock = ConfigurationBlock; + template struct Spheres { diff --git a/src/impl/vamp/robots/fetch.hh b/src/impl/vamp/robots/fetch.hh index 7cfa1d3e..2dbf94d9 100644 --- a/src/impl/vamp/robots/fetch.hh +++ b/src/impl/vamp/robots/fetch.hh @@ -39,6 +39,15 @@ namespace vamp::robots template using ConfigurationBlock = FloatVector; + // Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over. + // Defaults to Configuration; robots that plan in a different space (e.g. task space) can + // redefine this family independently of the FK/FKCC-facing Configuration types above. + using State = Configuration; + using StateArray = ConfigurationArray; + using StateBuffer = ConfigurationBuffer; + template + using StateBlock = ConfigurationBlock; + template struct Spheres { diff --git a/src/impl/vamp/robots/panda.hh b/src/impl/vamp/robots/panda.hh index a08827f4..a8d40a20 100644 --- a/src/impl/vamp/robots/panda.hh +++ b/src/impl/vamp/robots/panda.hh @@ -38,6 +38,15 @@ namespace vamp::robots template using ConfigurationBlock = FloatVector; + // Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over. + // Defaults to Configuration; robots that plan in a different space (e.g. task space) can + // redefine this family independently of the FK/FKCC-facing Configuration types above. + using State = Configuration; + using StateArray = ConfigurationArray; + using StateBuffer = ConfigurationBuffer; + template + using StateBlock = ConfigurationBlock; + template struct Spheres { diff --git a/src/impl/vamp/robots/sphere.hh b/src/impl/vamp/robots/sphere.hh index 01a85d4d..8d8299df 100644 --- a/src/impl/vamp/robots/sphere.hh +++ b/src/impl/vamp/robots/sphere.hh @@ -33,6 +33,15 @@ namespace vamp::robots { }; + // Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over. + // Defaults to Configuration; robots that plan in a different space (e.g. task space) can + // redefine this family independently of the FK/FKCC-facing Configuration types above. + using State = Configuration; + using StateArray = ConfigurationArray; + using StateBuffer = ConfigurationBuffer; + template + using StateBlock = ConfigurationBlock; + static constexpr std::array joint_names = {"x", "y", "z"}; static constexpr const char *end_effector = ""; diff --git a/src/impl/vamp/robots/ur5.hh b/src/impl/vamp/robots/ur5.hh index 39a89f59..a2ecfc67 100644 --- a/src/impl/vamp/robots/ur5.hh +++ b/src/impl/vamp/robots/ur5.hh @@ -37,6 +37,15 @@ namespace vamp::robots template using ConfigurationBlock = FloatVector; + // Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over. + // Defaults to Configuration; robots that plan in a different space (e.g. task space) can + // redefine this family independently of the FK/FKCC-facing Configuration types above. + using State = Configuration; + using StateArray = ConfigurationArray; + using StateBuffer = ConfigurationBuffer; + template + using StateBlock = ConfigurationBlock; + template struct Spheres {