Revert "Update API: JumpThreshold -> CartesianPrecision (#611)"

This reverts commit 99ccc11.

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -39,7 +39,6 @@
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
-#include <moveit/robot_state/cartesian_interpolator.h>
 
 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 <typename T = float>
-	void setJumpThreshold(double) {
-		static_assert(std::is_integral<T>::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

core/python/bindings/src/solvers.cpp

@@ -38,7 +38,6 @@
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/multi_planner.h>
 #include <moveit_msgs/msg/workspace_parameters.hpp>
-#include <fmt/core.h>
 #include "utils.h"
 
 namespace py = pybind11;
@@ -99,22 +98,6 @@ void export_solvers(py::module& m) {
 	    .property<double>("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_<moveit::core::CartesianPrecision>(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_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
 			Perform linear interpolation between Cartesian poses.
 		 	Fails on collision along the interpolation path. There is no obstacle avoidance. ::
@@ -124,12 +107,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<double>("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<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
+	    .property<double>(
+	        "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<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
 	    .def(py::init<>());
 

core/src/solvers/cartesian_path.cpp

@@ -60,8 +60,7 @@ CartesianPath::CartesianPath() {
 	auto& p = properties();
 	p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
 	p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
-	p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
-	                                            "precision of linear path");
+	p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
 	p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
 	p.declare<kinematics::KinematicsQueryOptions>("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<double>("step_size")),
-	    props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
+	    moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
 	    props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
 	    props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
 

demo/src/fallbacks_move_to.cpp

@@ -34,6 +34,7 @@ int main(int argc, char** argv) {
 
 	// setup solvers
 	auto cartesian = std::make_shared<solvers::CartesianPath>();
+	cartesian->setJumpThreshold(2.0);
 
 	const auto ptp = [&node]() {
 		auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner") };