From b39a7dc9824d106c3006fe9be2e7410503ef2038 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Fri, 25 Jun 2021 09:56:42 +0200 Subject: [PATCH 1/4] 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/4] 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/4] 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/4] 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");