Allow max Cartesian link speed in PlannerInterface (#277)

Author: gautz

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

@@ -89,6 +89,10 @@ class PlannerInterface
 	void setTimeout(double timeout) { properties_.set("timeout", timeout); }
 	void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
 	void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
+
+	void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); }
+	void setCartesianSpeedLimitedLink(const std::string& link) { setProperty("cartesian_speed_limited_link", link); }
+
 	void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
 		properties_.set("time_parameterization", tp);
 	}

core/python/bindings/src/solvers.cpp

@@ -54,6 +54,8 @@ void export_solvers(py::module& m) {
 	    .property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
 	    .property<double>("max_acceleration_scaling_factor",
 	                      "float: Reduce the maximum acceleration by scaling between (0,1]")
+	    .property<double>("max_cartesian_speed", "float: maximum cartesian speed")
+	    .property<std::string>("cartesian_speed_limited_link", "str: link with limited cartesian speed")
 	    .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
 	                           py::return_value_policy::reference_internal, "Properties of the planner");
 

core/src/solvers/cartesian_path.cpp

@@ -40,6 +40,7 @@
 #include <moveit/task_constructor/utils.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_parameterization.h>
+#include <moveit/trajectory_processing/limit_cartesian_speed.h>
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <moveit/robot_state/cartesian_interpolator.h>
 #include <tf2_eigen/tf2_eigen.h>
@@ -121,8 +122,19 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
 	for (const auto& waypoint : trajectory)
 		result->addSuffixWayPoint(waypoint, 0.0);
 
+	double max_cartesian_speed = props.get<double>("max_cartesian_speed");
 	auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
-	if (timing)
+	// compute timing to move the eef with constant speed
+	if (max_cartesian_speed > 0.0) {
+		if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed,
+		                                                      props.get<std::string>("cartesian_speed_limited_link"))) {
+			ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
+			                                        << props.get<std::string>("cartesian_speed_limited_link"));
+		} else {
+			ROS_ERROR_STREAM("failed to set max speed for link_ "
+			                 << props.get<std::string>("cartesian_speed_limited_link"));
+		}
+	} else if (timing)
 		timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
 		                          props.get<double>("max_acceleration_scaling_factor"));
 

core/src/solvers/pipeline_planner.cpp

@@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
 	req.num_planning_attempts = p.get<uint>("num_planning_attempts");
 	req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
 	req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
+	req.cartesian_speed_limited_link = p.get<std::string>("cartesian_speed_limited_link");
+	req.max_cartesian_speed = p.get<double>("max_cartesian_speed");
 	req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
 }
 

core/src/solvers/planner_interface.cpp

@@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() {
 	p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
 	p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
 	p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
+	p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
+	p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
 	p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
 }
 }  // namespace solvers

demo/scripts/cartesian.py

@@ -18,6 +18,8 @@
 # [cartesianTut2]
 # Cartesian and joint-space interpolation planners
 cartesian = core.CartesianPath()
+cartesian.max_cartesian_speed = 0.1  # m/s
+cartesian.cartesian_speed_limited_link = "panda_hand"
 jointspace = core.JointInterpolationPlanner()
 # [cartesianTut2]