From 002d4d192b3d6ad647f71c5a05b3109122844884 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 11 Dec 2025 23:37:05 -0600 Subject: [PATCH 1/5] Add validate_motion function --- src/impl/vamp/bindings/robot_helper.hh | 27 ++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 8fa3d7c1..ee0d9c44 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -266,6 +266,25 @@ 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 or in_bounds_out) and + vamp::planning::validate_motion( + configuration_in, configuration_out, EnvironmentVector(environment)); + } inline static auto simplify( const Path &path, const EnvironmentInput &environment, @@ -552,6 +571,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 = false); + MF("filter_self_from_pointcloud", filter_self_from_pointcloud, "Removes points from pointcloud which collide with the robot and environment.", From 8f1ec78ca7f7698f51b327dd3e96ebb5af7c7e62 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 11 Dec 2025 23:37:42 -0600 Subject: [PATCH 2/5] Add upper and lower bounds functions --- src/impl/vamp/bindings/robot_helper.hh | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index ee0d9c44..07515ce8 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -375,7 +375,23 @@ namespace vamp::binding submodule.def( "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( @@ -576,8 +592,8 @@ namespace vamp::binding "Check if a configuration is valid. Returns true if valid.", "configuration_in"_a, "configuration_out"_a, - "environment"_a = vamp::collision::Environment(), - "check_bounds"_a = false); + "environment"_a = vamp::collision::Environment(), + "check_bounds"_a = false); MF("filter_self_from_pointcloud", filter_self_from_pointcloud, From f2bd81ba25afe7fed5eb3c7120d0b20ac4d3c363 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 12 Dec 2025 09:06:32 -0600 Subject: [PATCH 3/5] Clang format --- src/impl/vamp/bindings/robot_helper.hh | 30 ++++++++++++++++---------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 07515ce8..05516c3b 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -266,8 +266,11 @@ 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 + 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(); @@ -285,6 +288,7 @@ namespace vamp::binding vamp::planning::validate_motion( configuration_in, configuration_out, EnvironmentVector(environment)); } + inline static auto simplify( const Path &path, const EnvironmentInput &environment, @@ -375,9 +379,11 @@ namespace vamp::binding submodule.def( "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 { + "upper_bounds", + []() -> NDArray + { std::array ones; ones.fill(1.0f); auto one_v = typename Robot::Configuration(ones); @@ -385,7 +391,9 @@ namespace vamp::binding return NA::from(one_v); }); submodule.def( - "lower_bounds", []() -> NDArray { + "lower_bounds", + []() -> NDArray + { std::array zeros; zeros.fill(0.0f); auto zero_v = typename Robot::Configuration(zeros); @@ -588,12 +596,12 @@ namespace vamp::binding "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 = false); + 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 = false); MF("filter_self_from_pointcloud", filter_self_from_pointcloud, From 06063ca45d7ae1401bf3a7fb75ffa22b7a12d78c Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 12 Dec 2025 10:10:21 -0600 Subject: [PATCH 4/5] fix: update bounds checking logic --- src/impl/vamp/bindings/robot_helper.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 05516c3b..cd66c5e9 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -284,7 +284,7 @@ namespace vamp::binding const bool in_bounds_out = (copy_out <= 1.F).all() and (copy_out >= 0.F).all(); - return (not check_bounds or in_bounds_in or in_bounds_out) and + return (not check_bounds or (in_bounds_in and in_bounds_out)) and vamp::planning::validate_motion( configuration_in, configuration_out, EnvironmentVector(environment)); } From a9a9dd49751e9cce531303761b2d20065c28d3bf Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 12 Dec 2025 10:10:47 -0600 Subject: [PATCH 5/5] fix: enable bounds checking --- src/impl/vamp/bindings/robot_helper.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index cd66c5e9..d6c4d303 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -601,7 +601,7 @@ namespace vamp::binding "configuration_in"_a, "configuration_out"_a, "environment"_a = vamp::collision::Environment(), - "check_bounds"_a = false); + "check_bounds"_a = true); MF("filter_self_from_pointcloud", filter_self_from_pointcloud,