Expose the validate_motion function and the robot’s lower and upper bounds to Python.

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<Robot, rake, 1>(
+                       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<float, Robot::dimension> 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<float, Robot::dimension> 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<Robot>;
         nb::class_<typename RNG::Ptr>(submodule, "RNG", "RNG for robot configurations.")
             .def(
@@ -552,6 +595,14 @@ namespace vamp::binding
            "environment"_a = vamp::collision::Environment<float>(),
            "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<float>(),
+           "check_bounds"_a = true);
+
         MF("filter_self_from_pointcloud",
            filter_self_from_pointcloud,
            "Removes points from pointcloud which collide with the robot and environment.",