From b39a7dc9824d106c3006fe9be2e7410503ef2038 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Fri, 25 Jun 2021 09:56:42 +0200 Subject: [PATCH 1/7] set max cartesian speed in planning pipeline --- core/src/solvers/pipeline_planner.cpp | 15 +++++++++++++++ core/src/solvers/planner_interface.cpp | 2 ++ 2 files changed, 17 insertions(+) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 7f0b25b46..1c7aefbc9 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -149,6 +150,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.max_cartesian_speed = p.get("max_cartesian_speed"); + req.cartesian_speed_end_effector_link = p.get("cartesian_speed_end_effector_link"); req.workspace_parameters = p.get("workspace_parameters"); } @@ -168,6 +171,12 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; + // optionally compute timing to move the eef with constant speed + if (req.max_cartesian_speed > 0.0) + { + trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, + req.cartesian_speed_end_effector_link); + } return success; } @@ -192,6 +201,12 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; + // optionally compute timing to move the eef with constant speed + if (req.max_cartesian_speed > 0.0) + { + trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, + req.cartesian_speed_end_effector_link); + } return success; } } // namespace solvers diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 23e314733..21e19d35b 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -46,6 +46,8 @@ PlannerInterface::PlannerInterface() { auto& p = properties(); 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 end-effector speed"); + p.declare("cartesian_speed_end_effector_link", "end_effector_link", "end-effector link with limited the velocity"); } } // namespace solvers } // namespace task_constructor From 0dc47803822d0e21bc89b742292625c16648d4f7 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Fri, 25 Jun 2021 15:49:33 +0200 Subject: [PATCH 2/7] pre-commit run --- core/src/solvers/pipeline_planner.cpp | 14 ++++++-------- core/src/solvers/planner_interface.cpp | 3 ++- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 1c7aefbc9..972636056 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -172,10 +172,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; // optionally compute timing to move the eef with constant speed - if (req.max_cartesian_speed > 0.0) - { - trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, - req.cartesian_speed_end_effector_link); + if (req.max_cartesian_speed > 0.0) { + trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, + req.cartesian_speed_end_effector_link); } return success; } @@ -202,10 +201,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; // optionally compute timing to move the eef with constant speed - if (req.max_cartesian_speed > 0.0) - { - trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, - req.cartesian_speed_end_effector_link); + if (req.max_cartesian_speed > 0.0) { + trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, + req.cartesian_speed_end_effector_link); } return success; } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 21e19d35b..2563e7c94 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -47,7 +47,8 @@ PlannerInterface::PlannerInterface() { 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 end-effector speed"); - p.declare("cartesian_speed_end_effector_link", "end_effector_link", "end-effector link with limited the velocity"); + p.declare("cartesian_speed_end_effector_link", "end_effector_link", + "end-effector link with limited the velocity"); } } // namespace solvers } // namespace task_constructor From cf873aa9dd9f0141a7e2bf8f104e16194e05157a Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Wed, 30 Jun 2021 16:41:05 +0200 Subject: [PATCH 3/7] use planner_request_adapter --- core/src/solvers/pipeline_planner.cpp | 13 +------------ core/src/solvers/planner_interface.cpp | 5 ++--- 2 files changed, 3 insertions(+), 15 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 972636056..89bc290c3 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -150,8 +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.cartesian_speed_end_effector_link = p.get("cartesian_speed_end_effector_link"); req.workspace_parameters = p.get("workspace_parameters"); } @@ -171,11 +170,6 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; - // optionally compute timing to move the eef with constant speed - if (req.max_cartesian_speed > 0.0) { - trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, - req.cartesian_speed_end_effector_link); - } return success; } @@ -200,11 +194,6 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; - // optionally compute timing to move the eef with constant speed - if (req.max_cartesian_speed > 0.0) { - trajectory_processing::setMaxCartesianEndEffectorSpeed(*result, req.max_cartesian_speed, - req.cartesian_speed_end_effector_link); - } return success; } } // namespace solvers diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 2563e7c94..ea568b552 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -46,9 +46,8 @@ PlannerInterface::PlannerInterface() { auto& p = properties(); 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 end-effector speed"); - p.declare("cartesian_speed_end_effector_link", "end_effector_link", - "end-effector link with limited the velocity"); + p.declare("max_cartesian_speed", 0.0, "maximum cartesian speed"); + p.declare("cartesian_speed_limited_link", "", "link with limited cartesian speed"); } } // namespace solvers } // namespace task_constructor From aa12c0ab43631b1a33bfe783c72421dc1739446c Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Wed, 30 Jun 2021 16:42:05 +0200 Subject: [PATCH 4/7] wip set cartesian path max speed --- .../task_constructor/solvers/cartesian_path.h | 5 +++++ core/src/solvers/cartesian_path.cpp | 20 ++++++++++++++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 72e63fe3a..9f3fdb888 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -59,6 +59,11 @@ class CartesianPath : public PlannerInterface void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); } void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); } + void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); } + void setCartesianSpeedLimitedLink(std::string limited_link) { + setProperty("cartesian_speed_limited_link", limited_link); + } + void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 2188a45db..591a0470e 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #if MOVEIT_MASTER #include #endif @@ -105,9 +106,22 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + // optionally compute timing to move the eef with constant speed + if (props.get("max_cartesian_speed") > 0.0) { + if (trajectory_processing::limitMaxCartesianLinkSpeed( + *result, props.get("max_cartesian_speed"), + props.get("cartesian_speed_limited_link"))) { + ROS_INFO_STREAM("successfully set max " << props.get("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 { + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); + } } return achieved_fraction >= props.get("min_fraction"); From 2f1f4f07ec206e1be47f91c71bf02024e9642af6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 10 Sep 2025 13:48:53 +0200 Subject: [PATCH 5/7] Move setter functions to base class PlannerInterface --- .../include/moveit/task_constructor/solvers/cartesian_path.h | 5 ----- .../moveit/task_constructor/solvers/planner_interface.h | 4 ++++ 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 42e3fbcd9..4214f09c0 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -70,11 +70,6 @@ class CartesianPath : public PlannerInterface [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on - void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); } - void setCartesianSpeedLimitedLink(std::string limited_link) { - setProperty("cartesian_speed_limited_link", limited_link); - } - void init(const moveit::core::RobotModelConstPtr& robot_model) override; Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index ddc5a2aba..970583c8e 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 @@ 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); } From 86e654077d9ac606afa1e693689395217d44d964 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 10 Sep 2025 13:54:12 +0200 Subject: [PATCH 6/7] Add python bindings --- core/python/bindings/src/solvers.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 26ac6c09d..f4017922f 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"); From b60149b85f4d0c430da32f557181cc60596d19f1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 10 Sep 2025 14:15:51 +0200 Subject: [PATCH 7/7] Add example code to cartesian.py --- demo/scripts/cartesian.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index a79360124..8047034a0 100755 --- a/demo/scripts/cartesian.py +++ b/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]