diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index 9e6a884cc..d11197112 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -64,9 +64,9 @@ class class_ : public pybind11::classh // NOLINT(readability template class_& property(const char* name, const Extra&... extra) { PropertyConverter(); // register corresponding property converter - auto getter = [name](const type_& self) { - const moveit::task_constructor::PropertyMap& props = self.properties(); - return props.get(name); + auto getter = [name](type_& self) -> PropertyType& { + moveit::task_constructor::PropertyMap& props = self.properties(); + return const_cast(props.get(name)); }; auto setter = [name](type_& self, const PropertyType& value) { moveit::task_constructor::PropertyMap& props = self.properties(); diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 3595bc651..8de351da8 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,6 +39,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { @@ -57,13 +58,17 @@ class CartesianPath : public PlannerInterface void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setStepSize(double step_size) { setProperty("step_size", step_size); } - void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } + void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); } + template + void setJumpThreshold(double) { + static_assert(std::is_integral::value, "setJumpThreshold() is deprecated. Replace with setPrecision."); + } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off - void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off - void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -72,8 +77,8 @@ class CartesianPath : public PlannerInterface const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index ef60fb822..572cf71e2 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "utils.h" namespace py = pybind11; @@ -99,6 +100,22 @@ void export_solvers(py::module& m) { .property("max_step", "float: Limit any (single) joint change between two waypoints to this amount") .def(py::init<>()); + const moveit::core::CartesianPrecision default_precision; + py::class_(m, "CartesianPrecision", "precision for Cartesian interpolation") + .def(py::init([](double translational, double rotational, double max_resolution) { + return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution }; + }), + py::arg("translational") = default_precision.translational, + py::arg("rotational") = default_precision.rotational, + py::arg("max_resolution") = default_precision.max_resolution) + .def_readwrite("translational", &moveit::core::CartesianPrecision::translational) + .def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational) + .def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution) + .def("__str__", [](const moveit::core::CartesianPrecision& self) { + return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}", + self.translational, self.rotational, self.max_resolution); + }); + properties::class_(m, "CartesianPath", R"( Perform linear interpolation between Cartesian poses. Fails on collision along the interpolation path. There is no obstacle avoidance. :: @@ -108,15 +125,12 @@ void export_solvers(py::module& m) { # Instantiate Cartesian-space interpolation planner cartesianPlanner = core.CartesianPath() cartesianPlanner.step_size = 0.01 - cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. + cartesianPlanner.precision.translational = 0.001 )") .property("step_size", "float: Limit the Cartesian displacement between consecutive waypoints " "In contrast to joint-space interpolation, the Cartesian planner can also " "succeed when only a fraction of the linear path was feasible.") - .property( - "jump_threshold", - "float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. " - "This values specifies the fraction of mean acceptable joint motion per step.") + .property("precision", "Cartesian interpolation precision") .property("min_fraction", "float: Fraction of overall distance required to succeed.") .def(py::init<>()); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e2c0c80c8..e2aadf665 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -54,7 +54,8 @@ CartesianPath::CartesianPath() { auto& p = properties(); p.declare("ik_frame", "frame to move linearly (use for joint-space target)"); p.declare("step_size", 0.01, "step size between consecutive waypoints"); - p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); + p.declare("precision", moveit::core::CartesianPrecision(), + "precision of linear path"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); p.declare("kinematics_options", kinematics::KinematicsQueryOptions(), "KinematicsQueryOptions to pass to CartesianInterpolator"); @@ -112,7 +113,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), - moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, + props.get("precision"), is_valid, props.get("kinematics_options"), offset); assert(!trajectory.empty()); // there should be at least the start state diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 25ab8738b..05731ee19 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -35,7 +35,6 @@ int main(int argc, char** argv) { // setup solvers auto cartesian = std::make_shared(); - cartesian->setJumpThreshold(2.0); auto ptp = std::make_shared("pilz_industrial_motion_planner"); ptp->setPlannerId("PTP");