From 7384702448650781111bfcd2f7878d91bb430024 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz <64833674+gautz@users.noreply.github.com> Date: Wed, 10 Sep 2025 14:30:26 +0200 Subject: [PATCH] Allow max Cartesian link speed in PlannerInterface (#277) --- .../task_constructor/solvers/planner_interface.h | 4 ++++ core/python/bindings/src/solvers.cpp | 2 ++ core/src/solvers/cartesian_path.cpp | 14 +++++++++++++- core/src/solvers/pipeline_planner.cpp | 2 ++ core/src/solvers/planner_interface.cpp | 2 ++ demo/scripts/cartesian.py | 2 ++ 6 files changed, 25 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index ddc5a2ab..970583c8 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -89,6 +89,10 @@ public: 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); } diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 26ac6c09..f4017922 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -54,6 +54,8 @@ void export_solvers(py::module& m) { .property("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") .property("max_acceleration_scaling_factor", "float: Reduce the maximum acceleration by scaling between (0,1]") + .property("max_cartesian_speed", "float: maximum cartesian speed") + .property("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"); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index c87bbef1..0d604fab 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -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("max_cartesian_speed"); auto timing = props.get("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("cartesian_speed_limited_link"))) { + ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link " + << props.get("cartesian_speed_limited_link")); + } else { + ROS_ERROR_STREAM("failed to set max speed for link_ " + << props.get("cartesian_speed_limited_link")); + } + } else if (timing) timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 99c5912c..34e71a07 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa req.num_planning_attempts = p.get("num_planning_attempts"); req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); + req.cartesian_speed_limited_link = p.get("cartesian_speed_limited_link"); + req.max_cartesian_speed = p.get("max_cartesian_speed"); req.workspace_parameters = p.get("workspace_parameters"); } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index a58b110d..8475699d 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() { p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("max_cartesian_speed", 0.0, "maximum cartesian speed"); + p.declare("cartesian_speed_limited_link", "", "link with limited cartesian speed"); p.declare("time_parameterization", std::make_shared()); } } // namespace solvers diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index a7936012..8047034a 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -18,6 +18,8 @@ group = "panda_arm" # [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]