diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 8fa3d7c1..d6c4d303 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -266,6 +266,29 @@ namespace vamp::binding configuration, configuration, EnvironmentVector(environment)); } + inline static auto validate_motion( + const Type &c_in, + const Type &c_out, + const EnvironmentInput &environment, + bool check_bounds = false) -> bool + { + auto configuration_in = Input::to(c_in); + auto copy_in = configuration_in.trim(); + Robot::descale_configuration(copy_in); + + const bool in_bounds_in = (copy_in <= 1.F).all() and (copy_in >= 0.F).all(); + + auto configuration_out = Input::to(c_out); + auto copy_out = configuration_out.trim(); + Robot::descale_configuration(copy_out); + + const bool in_bounds_out = (copy_out <= 1.F).all() and (copy_out >= 0.F).all(); + + return (not check_bounds or (in_bounds_in and in_bounds_out)) and + vamp::planning::validate_motion( + configuration_in, configuration_out, EnvironmentVector(environment)); + } + inline static auto simplify( const Path &path, const EnvironmentInput &environment, @@ -357,6 +380,26 @@ namespace vamp::binding "joint_names", []() { return Robot::joint_names; }, "Joint names for the robot in order of DoF"); submodule.def("end_effector", []() { return Robot::end_effector; }, "End-effector frame name."); + submodule.def( + "upper_bounds", + []() -> NDArray + { + std::array ones; + ones.fill(1.0f); + auto one_v = typename Robot::Configuration(ones); + Robot::scale_configuration(one_v); + return NA::from(one_v); + }); + submodule.def( + "lower_bounds", + []() -> NDArray + { + std::array zeros; + zeros.fill(0.0f); + auto zero_v = typename Robot::Configuration(zeros); + Robot::scale_configuration(zero_v); + return NA::from(zero_v); + }); using RNG = vamp::rng::RNG; nb::class_(submodule, "RNG", "RNG for robot configurations.") .def( @@ -552,6 +595,14 @@ namespace vamp::binding "environment"_a = vamp::collision::Environment(), "check_bounds"_a = false); + MF("validate_motion", + validate_motion, + "Check if a configuration is valid. Returns true if valid.", + "configuration_in"_a, + "configuration_out"_a, + "environment"_a = vamp::collision::Environment(), + "check_bounds"_a = true); + MF("filter_self_from_pointcloud", filter_self_from_pointcloud, "Removes points from pointcloud which collide with the robot and environment.",