From cc89625a7361431400ebb5552147a0dea2d97d87 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 17 Sep 2024 11:57:44 +0200 Subject: [PATCH] Revert "Update API: JumpThreshold -> CartesianPrecision (#611)" This reverts commit 99ccc115e041c5e80b8e653815a62936c3d392ec. --- .../task_constructor/solvers/cartesian_path.h | 7 +----- core/python/bindings/src/solvers.cpp | 23 ++++--------------- core/src/solvers/cartesian_path.cpp | 5 ++-- demo/src/fallbacks_move_to.cpp | 1 + 4 files changed, 9 insertions(+), 27 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 0d4ef8a46..b3dee0319 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,7 +39,6 @@ #pragma once #include -#include namespace moveit { namespace task_constructor { @@ -58,11 +57,7 @@ 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 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 setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 8d64a2077..98452233d 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -99,22 +99,6 @@ 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. :: @@ -124,12 +108,15 @@ void export_solvers(py::module& m) { # Instantiate Cartesian-space interpolation planner cartesianPlanner = core.CartesianPath() cartesianPlanner.step_size = 0.01 - cartesianPlanner.precision.translational = 0.001 + cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. )") .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("precision", "Cartesian interpolation precision") + .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("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 0a5494a43..d5d37c5c3 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -60,8 +60,7 @@ 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("precision", moveit::core::CartesianPrecision(), - "precision of linear path"); + p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); p.declare("kinematics_options", kinematics::KinematicsQueryOptions(), "KinematicsQueryOptions to pass to CartesianInterpolator"); @@ -121,7 +120,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")), - props.get("precision"), is_valid, + moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, props.get("kinematics_options"), props.get("kinematics_cost_fn"), offset); diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 781c02571..4e834ee7b 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -34,6 +34,7 @@ int main(int argc, char** argv) { // setup solvers auto cartesian = std::make_shared(); + cartesian->setJumpThreshold(2.0); const auto ptp = [&node]() { auto pp{ std::make_shared(node, "pilz_industrial_motion_planner") };