Update API: JumpThreshold -> CartesianPrecision (#611)

Python: Access properties via writable references
This allows to modify properties in place, e.g. cartesian_solver.precision.translational = 0.01

Author: rhaschke

core/include/moveit/python/task_constructor/properties.h

@@ -64,9 +64,9 @@ class class_ : public pybind11::classh<type_, options...>  // NOLINT(readability
 	template <typename PropertyType, typename... Extra>
 	class_& property(const char* name, const Extra&... extra) {
 		PropertyConverter<PropertyType>();  // register corresponding property converter
-		auto getter = [name](const type_& self) {
-			const moveit::task_constructor::PropertyMap& props = self.properties();
-			return props.get<PropertyType>(name);
+		auto getter = [name](type_& self) -> PropertyType& {
+			moveit::task_constructor::PropertyMap& props = self.properties();
+			return const_cast<PropertyType&>(props.get<PropertyType>(name));
 		};
 		auto setter = [name](type_& self, const PropertyType& value) {
 			moveit::task_constructor::PropertyMap& props = self.properties();

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

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

core/python/bindings/src/solvers.cpp

@@ -38,6 +38,7 @@
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/multi_planner.h>
 #include <moveit_msgs/WorkspaceParameters.h>
+#include <fmt/core.h>
 #include "utils.h"
 
 namespace py = pybind11;
@@ -99,6 +100,22 @@ 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. ::
@@ -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<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<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<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
 	    .property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
 	    .def(py::init<>());
 

core/src/solvers/cartesian_path.cpp

@@ -54,7 +54,8 @@ CartesianPath::CartesianPath() {
 	auto& p = properties();
 	p.declare<geometry_msgs::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<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
+	p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
+	                                            "precision of linear path");
 	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");
@@ -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<double>("step_size")),
-	    moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
+	    props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
 	    props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
 
 	assert(!trajectory.empty());  // there should be at least the start state

demo/src/fallbacks_move_to.cpp

@@ -35,7 +35,6 @@ int main(int argc, char** argv) {
 
 	// setup solvers
 	auto cartesian = std::make_shared<solvers::CartesianPath>();
-	cartesian->setJumpThreshold(2.0);
 
 	auto ptp = std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner");
 	ptp->setPlannerId("PTP");