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"); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 7f0b25b46..89bc290c3 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 23e314733..ea568b552 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 speed"); + p.declare("cartesian_speed_limited_link", "", "link with limited cartesian speed"); } } // namespace solvers } // namespace task_constructor