From a9cae03e57f05d1f4d9b3174e89190e58b955d5a Mon Sep 17 00:00:00 2001 From: Steven Schreck Date: Wed, 19 Jun 2024 09:17:55 +0200 Subject: [PATCH] added possibility to optionally optimize time during cartesian path planning --- .../cartesian_path_service_capability.cpp | 12 +++++++++--- .../move_group_interface/move_group_interface.h | 11 ++++++++--- .../src/move_group_interface.cpp | 15 +++++++++------ 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index b02da0b48c..89ba6f2bfa 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -176,12 +176,18 @@ bool MoveGroupCartesianPathService::computeService( // time trajectory // \todo optionally compute timing to move the eef with constant speed - trajectory_processing::TimeOptimalTrajectoryGeneration time_param; - time_param.computeTimeStamps(rt, 1.0); + if (req->time_optimize) + { + auto orig_point_count = rt.getWayPointCount(); + trajectory_processing::TimeOptimalTrajectoryGeneration time_param; + time_param.computeTimeStamps(rt, 1.0); + RCLCPP_INFO(LOGGER, "Optimized trajectory with %u points to %u points", (unsigned int)orig_point_count, + (unsigned int)rt.getWayPointCount()); + } rt.getRobotTrajectoryMsg(res->solution); RCLCPP_INFO(LOGGER, "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", - (unsigned int)traj.size(), res->fraction * 100.0); + (unsigned int)rt.getWayPointCount(), res->fraction * 100.0); if (display_computed_paths_ && rt.getWayPointCount() > 0) { moveit_msgs::msg::DisplayTrajectory disp; diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index a41dad2dfe..a21da210f2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -739,10 +739,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. - Return -1.0 in case of error. */ + Time optimization is performed if \e time_optimize is set to true. Else the trajectory will be returned + unoptimized. + Return -1.0 in case of error. */ double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + bool avoid_collisions = true, bool time_optimize = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -755,11 +758,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface met, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. + Time optimization is performed if \e time_optimize is set to true. Else the trajectory will be returned + unoptimized. Return -1.0 in case of error. */ double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + bool time_optimize = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); /** \brief Stop any trajectory execution, if one is active */ void stop(); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index f722681ca1..71cf0bf9e1 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -950,7 +950,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl double computeCartesianPath(const std::vector& waypoints, double step, double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, - moveit_msgs::msg::MoveItErrorCodes& error_code) + bool time_optimize, moveit_msgs::msg::MoveItErrorCodes& error_code) { auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; @@ -969,6 +969,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->path_constraints = path_constraints; req->avoid_collisions = avoid_collisions; req->link_name = getEndEffectorLink(); + req->time_optimize = time_optimize; auto future_response = cartesian_path_service_->async_send_request(req); if (future_response.valid()) @@ -1594,28 +1595,30 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) + bool avoid_collisions, bool time_optimize, + moveit_msgs::msg::MoveItErrorCodes* error_code) { moveit_msgs::msg::Constraints path_constraints_tmp; return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions, - error_code); + time_optimize, error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, - bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) + bool avoid_collisions, bool time_optimize, + moveit_msgs::msg::MoveItErrorCodes* error_code) { if (error_code) { return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, *error_code); + avoid_collisions, time_optimize, *error_code); } else { moveit_msgs::msg::MoveItErrorCodes error_code_tmp; return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, error_code_tmp); + avoid_collisions, time_optimize, error_code_tmp); } }