From 182436dbbb2e0558fa50ce21e716574f23c28cf2 Mon Sep 17 00:00:00 2001 From: Wil Thomason Date: Wed, 5 Feb 2025 00:57:55 -0500 Subject: [PATCH] WIP porting batch validation --- src/impl/vamp/bindings/common.hh | 92 +++++++++++++++++++--- src/impl/vamp/collision/validity.hh | 118 +++++++++++++++++++++++++++- 2 files changed, 197 insertions(+), 13 deletions(-) diff --git a/src/impl/vamp/bindings/common.hh b/src/impl/vamp/bindings/common.hh index 528b0790..20587fad 100644 --- a/src/impl/vamp/bindings/common.hh +++ b/src/impl/vamp/bindings/common.hh @@ -118,8 +118,8 @@ namespace vamp::binding } #endif - inline static auto - fk(const ConfigurationArray &configuration) -> std::vector> + inline static auto fk(const ConfigurationArray &configuration) + -> std::vector> { typename Robot::template Spheres<1> out; typename Robot::template ConfigurationBlock<1> block; @@ -140,9 +140,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; @@ -158,9 +158,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)); @@ -174,6 +174,75 @@ namespace vamp::binding configuration_v, configuration_v, EnvironmentVector(environment)); } + // TODO: nb::ndarray overload + inline static auto validate_batch( + const EnvironmentInput &environment, + const std::vector &configurations, + bool is_sequence = true) noexcept -> std::vector + { + EnvironmentVector environment_v(environment); + std::vector result(configurations.size(), 0.0); + // TODO: Figure out if cache blocking can help here, given the double-indirection of a vector of + // arrays + std::array block_scalars; + bool sequence_valid = true; + unsigned int num_residual_elements = configurations.size() % rake; + for (auto i = 0U; sequence_valid and i < configurations.size() - num_residual_elements; i += rake) + { + for (auto j = 0U; j < rake; ++j) + { + const auto &q = configurations[i + j]; + for (auto k = 0U; k < Robot::dimension; ++k) + { + block_scalars[k * rake + j] = q[k]; + } + } + + typename Robot::template ConfigurationBlock block(block_scalars); + auto block_validation = vamp::planning::validate_block(environment_v, block); + for (auto j = 0U; j < rake; ++j) + { + const auto config_valid = block_validation[{0, j}]; + if (is_sequence and config_valid == 0.0) + { + sequence_valid = false; + break; + } + + result[i + j] = is_sequence ? static_cast(sequence_valid) : config_valid; + } + } + + if (sequence_valid) + { + const auto offset = configurations.size() - num_residual_elements; + for (auto i = 0U; i < num_residual_elements; ++i) + { + const auto &q = configurations[i + offset]; + for (auto k = 0U; k < Robot::dimension; ++k) + { + block_scalars[k * rake + i] = q[k]; + } + } + + typename Robot::template ConfigurationBlock block(block_scalars); + auto block_validation = vamp::planning::validate_block(environment_v, block); + for (auto i = 0U; i < num_residual_elements; ++i) + { + const auto config_valid = block_validation[{0, i}]; + if (is_sequence and config_valid == 0.0) + { + sequence_valid = false; + break; + } + + result[offset + i] = is_sequence ? static_cast(sequence_valid) : config_valid; + } + } + + return result; + } + inline static auto rrtc_single( const ConfigurationArray &start, const ConfigurationArray &goal, @@ -295,8 +364,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); @@ -352,7 +421,8 @@ 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.") diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index 1778c185..8efc1334 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -149,6 +149,111 @@ namespace vamp return false; } + template + inline constexpr auto sphere_block_environment_validate( + const collision::Environment &e, // + ArgT1 sx_, + ArgT2 sy_, + ArgT3 sz_, + ArgT4 sr_) noexcept -> DataT + { + // TODO: Figure out a way to avoid needing to upcast floats to vectors + auto sx = static_cast(sx_); + auto sy = static_cast(sy_); + auto sz = static_cast(sz_); + auto sr = static_cast(sr_); + const auto max_extent = collision::sqrt(collision::dot_3(sx, sy, sz, sx, sy, sz)) + sr; + + DataT result(0.5); + const DataT offset(0.5); + constexpr float SIGN_MASK = -0.0; + for (const auto &es : e.spheres) + { + const auto diff = es.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + result = result | collision::sphere_sphere_sql2(es, sx, sy, sz, sr) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + for (const auto &ec : e.capsules) + { + const auto diff = ec.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + result = result | collision::sphere_capsule(ec, sx, sy, sz, sr) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + for (const auto &ec : e.z_aligned_capsules) + { + const auto diff = ec.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + result = result | collision::sphere_z_aligned_capsule(ec, sx, sy, sz, sr) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + const auto rsq = sr * sr; + for (const auto &ec : e.cuboids) + { + const auto diff = ec.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + result = result | collision::sphere_cuboid(ec, sx, sy, sz, rsq) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + for (const auto &ec : e.z_aligned_cuboids) + { + const auto diff = ec.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + result = result | collision::sphere_z_aligned_cuboid(ec, sx, sy, sz, rsq) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + for (const auto &eh : e.heightfields) + { + result = result | collision::sphere_heightfield(eh, sx, sy, sz, sr) & SIGN_MASK; + if (result.all()) + { + return result + offset; + } + } + + return result + offset; + } + template inline auto sphere_environment_get_collisions( const collision::Environment &e, // @@ -249,8 +354,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) { @@ -291,4 +396,13 @@ namespace vamp return false; } + + template + inline constexpr auto validate_block( + const collision::Environment> &environment, + const typename Robot::template ConfigurationBlock &block) -> FloatVector + { + return Robot::template validate_block(environment, block); + } + } // namespace vamp