From 3679a33042f12b0c6d09ee50d703b772d808e131 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 30 Dec 2025 12:46:11 +0000 Subject: [PATCH] Added max cartesian speed limiter. Partial from: https://github.com/moveit/moveit2/pull/2212 --- .../trajectory_processing/CMakeLists.txt | 7 +- .../limit_cartesian_speed.h | 15 ++ .../limit_cartesian_speed.hpp | 50 +++++ .../trajectory_tools.hpp | 9 +- .../src/limit_cartesian_speed.cpp | 127 ++++++++++++ .../src/trajectory_tools.cpp | 110 ++++++++++ .../test/test_limit_cartesian_speed.cpp | 194 ++++++++++++++++++ moveit_py/moveit/core/robot_trajectory.pyi | 1 + .../robot_trajectory/robot_trajectory.cpp | 13 ++ .../cartesian_path_service_capability.cpp | 8 + ...lt_request_adapters_plugin_description.xml | 6 + .../CMakeLists.txt | 3 +- .../src/limit_max_cartesian_link_speed.cpp | 104 ++++++++++ .../move_group_interface.hpp | 9 + .../src/move_group_interface.cpp | 29 +++ 15 files changed, 682 insertions(+), 3 deletions(-) create mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.h create mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.hpp create mode 100644 moveit_core/trajectory_processing/src/limit_cartesian_speed.cpp create mode 100644 moveit_core/trajectory_processing/test/test_limit_cartesian_speed.cpp create mode 100644 moveit_ros/planning/planning_response_adapter_plugins/src/limit_max_cartesian_link_speed.cpp diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index bbbe8d1f2c..23a8e553f3 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -1,7 +1,7 @@ add_library( moveit_trajectory_processing SHARED src/ruckig_traj_smoothing.cpp src/trajectory_tools.cpp - src/time_optimal_trajectory_generation.cpp) + src/time_optimal_trajectory_generation.cpp src/limit_cartesian_speed.cpp) target_include_directories( moveit_trajectory_processing PUBLIC $ @@ -45,6 +45,11 @@ if(BUILD_TESTING) target_link_libraries(test_ruckig_traj_smoothing moveit_trajectory_processing moveit_test_utils) + ament_add_gtest(test_limit_cartesian_speed + test/test_limit_cartesian_speed.cpp) + target_link_libraries(test_limit_cartesian_speed moveit_trajectory_processing + moveit_test_utils) + ament_add_google_benchmark(robot_trajectory_benchmark test/robot_trajectory_benchmark.cpp) target_link_libraries( diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.h new file mode 100644 index 0000000000..7f5baae421 --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.h @@ -0,0 +1,15 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.hpp new file mode 100644 index 0000000000..fcdc0a8219 --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/limit_cartesian_speed.hpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Benjamin Scholz + * Copyright (c) 2021, Thies Oelerich + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the authors nor the names of other + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Benjamin Scholz, Thies Oelerich */ + +#pragma once + +#include +#include +#include + +namespace trajectory_processing +{ +bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed, + const moveit::core::LinkModel* link_model); +bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed, + const std::string& link_name = ""); +} // namespace trajectory_processing \ No newline at end of file diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp index daa1f1f00b..c232e82858 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.hpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace trajectory_processing { @@ -80,11 +81,17 @@ bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, double acceleration_scaling_factor, bool mitigate_overshoot = false, double overshoot_threshold = 0.01); - /** * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. */ [[nodiscard]] trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector& joint_names, const trajectory_processing::Trajectory& trajectory, const int sampling_rate); + +/** + * \brief Updates the time stamps of a `robot_trajectory::RobotTrajectory` based on the provided time differences. + * \param [in,out] rob_trajectory The robot trajectory to be updated. + * \param [in] time_diff A vector containing the time differences between consecutive waypoints. + */ +void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector& time_diff); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/limit_cartesian_speed.cpp b/moveit_core/trajectory_processing/src/limit_cartesian_speed.cpp new file mode 100644 index 0000000000..fcd9868cbc --- /dev/null +++ b/moveit_core/trajectory_processing/src/limit_cartesian_speed.cpp @@ -0,0 +1,127 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Benjamin Scholz + * Copyright (c) 2021, Thies Oelerich + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the authors nor the names of other + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Benjamin Scholz, Thies Oelerich */ + +#include +#include + +#include +#include +#include + +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.core.limit_cartesian_speed"); +} + +namespace trajectory_processing +{ + +bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed, + const std::string& link_name) +{ + std::vector links; + + if (!link_name.empty()) + { + const moveit::core::RobotModel& model{ *trajectory.getRobotModel() }; + bool found = false; + const auto* link = model.getLinkModel(link_name, &found); + + if (!found) + RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"), + "Unknown link model '" << link_name << "'"); + else + links.push_back(link); + } + // In case the link name is not given but the trajectory belongs to a group, + // retrieve the end effectors from that joint model group + else if (trajectory.getGroup()) + { + trajectory.getGroup()->getEndEffectorTips(links); + if (links.empty()) + { + RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"), "No link(s) specified"); + } + } + + // Call function for speed setting using the created link model + for (const auto& link : links) + { + if (!limitMaxCartesianLinkSpeed(trajectory, max_speed, link)) + return false; + } + + return !links.empty(); +} + +bool limitMaxCartesianLinkSpeed(robot_trajectory::RobotTrajectory& trajectory, const double max_speed, + const moveit::core::LinkModel* link_model) +{ + if (max_speed <= 0.0) + { + RCLCPP_ERROR_STREAM(moveit::getLogger("moveit.core.limit_cartesian_speed"), "Link speed must be greater than 0."); + return false; + } + + size_t num_waypoints = trajectory.getWayPointCount(); + if (num_waypoints == 0) + return false; + + // do forward kinematics to get Cartesian positions of link for current waypoint + double euclidean_distance, new_time_diff, old_time_diff; + std::vector time_diff(num_waypoints - 1, 0.0); + + for (size_t i = 0; i < num_waypoints - 1; i++) + { + // get link state for current and next waypoint + const Eigen::Isometry3d& current_link_state = trajectory.getWayPointPtr(i)->getGlobalLinkTransform(link_model); + const Eigen::Isometry3d& next_link_state = trajectory.getWayPointPtr(i + 1)->getGlobalLinkTransform(link_model); + + // get Euclidean distance between the two waypoints + euclidean_distance = (next_link_state.translation() - current_link_state.translation()).norm(); + + new_time_diff = (euclidean_distance / max_speed); + old_time_diff = trajectory.getWayPointDurationFromPrevious(i + 1); + + // slow-down segment if it was too fast before + time_diff[i] = std::max(new_time_diff, old_time_diff); + } + // update time stamps, velocities and accelerations of the trajectory + updateTrajectory(trajectory, time_diff); + return true; +} +} // namespace trajectory_processing \ No newline at end of file diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index f89dae256f..9e967bc6a0 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -107,4 +107,114 @@ trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector< } return trajectory_msg; } + +// Takes the time differences, and updates the timestamps, velocities and accelerations +// in the trajectory. +void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector& time_diff) +{ + // Error check + if (time_diff.empty()) + return; + + double time_sum = 0.0; + + moveit::core::RobotStatePtr prev_waypoint; + moveit::core::RobotStatePtr curr_waypoint; + moveit::core::RobotStatePtr next_waypoint; + + const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); + const std::vector& vars = group->getVariableNames(); + const std::vector& idx = group->getVariableIndexList(); + + int num_points = rob_trajectory.getWayPointCount(); + + rob_trajectory.setWayPointDurationFromPrevious(0, time_sum); + + // Times + for (int i = 1; i < num_points; ++i) + // Update the time between the waypoints in the robot_trajectory. + rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]); + + // Return if there is only one point in the trajectory! + if (num_points <= 1) + return; + + // Accelerations + for (int i = 0; i < num_points; ++i) + { + curr_waypoint = rob_trajectory.getWayPointPtr(i); + + if (i > 0) + prev_waypoint = rob_trajectory.getWayPointPtr(i - 1); + + if (i < num_points - 1) + next_waypoint = rob_trajectory.getWayPointPtr(i + 1); + + for (std::size_t j = 0; j < vars.size(); ++j) + { + double q1; + double q2; + double q3; + double dt1; + double dt2; + + if (i == 0) + { + // First point + q1 = next_waypoint->getVariablePosition(idx[j]); + q2 = curr_waypoint->getVariablePosition(idx[j]); + q3 = q1; + + dt1 = dt2 = time_diff[i]; + } + else if (i < num_points - 1) + { + // middle points + q1 = prev_waypoint->getVariablePosition(idx[j]); + q2 = curr_waypoint->getVariablePosition(idx[j]); + q3 = next_waypoint->getVariablePosition(idx[j]); + + dt1 = time_diff[i - 1]; + dt2 = time_diff[i]; + } + else + { + // last point + q1 = prev_waypoint->getVariablePosition(idx[j]); + q2 = curr_waypoint->getVariablePosition(idx[j]); + q3 = q1; + + dt1 = dt2 = time_diff[i - 1]; + } + + double v1, v2, a; + + bool start_velocity = false; + if (dt1 == 0.0 || dt2 == 0.0) + { + v1 = 0.0; + v2 = 0.0; + a = 0.0; + } + else + { + if (i == 0) + { + if (curr_waypoint->hasVelocities()) + { + start_velocity = true; + v1 = curr_waypoint->getVariableVelocity(idx[j]); + } + } + v1 = start_velocity ? v1 : (q2 - q1) / dt1; + // v2 = (q3-q2)/dt2; + v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point + a = 2.0 * (v2 - v1) / (dt1 + dt2); + } + + curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0); + curr_waypoint->setVariableAcceleration(idx[j], a); + } + } +} } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/test/test_limit_cartesian_speed.cpp b/moveit_core/trajectory_processing/test/test_limit_cartesian_speed.cpp new file mode 100644 index 0000000000..8e18e0de97 --- /dev/null +++ b/moveit_core/trajectory_processing/test/test_limit_cartesian_speed.cpp @@ -0,0 +1,194 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Ken Anderson + * Copyright (c) 2020, Benjamin Scholz + * Copyright (c) 2021, Thies Oelerich + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the authors nor the names of other + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Benjamin Scholz, Thies Oelerich, based off test_time_parameterization.cpp by Ken Anderson */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Static variables used in all tests +moveit::core::RobotModelConstPtr RMODEL = moveit::core::loadTestingRobotModel("panda"); +robot_trajectory::RobotTrajectory TRAJECTORY(RMODEL, "panda_arm"); +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.test_limit_cartesian_speed"); + +// Vector of distances between waypoints +std::vector WAYPOINT_DISTANCES; + +// Name of logger +const char* LOGGER_NAME = "trajectory_processing"; + +// Initialize one-joint, straight-line trajectory +bool initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + if (!group) + { + RCLCPP_ERROR(LOGGER, "Need to set the group"); + return false; + } + // Get state of the robot + moveit::core::RobotState state(trajectory.getRobotModel()); + + trajectory.clear(); + // Initial waypoint + // Cartesian Position (in panda_link0 frame): + // [X: 0.30701957005161057, Y: 0, Z: 0.5902504197143968] + state.setVariablePosition("panda_joint1", 0); + state.setVariablePosition("panda_joint2", -0.785); + state.setVariablePosition("panda_joint3", 0); + state.setVariablePosition("panda_joint4", -2.356); + state.setVariablePosition("panda_joint5", 0); + state.setVariablePosition("panda_joint6", 1.571); + state.setVariablePosition("panda_joint7", 0.785); + trajectory.addSuffixWayPoint(state, 0.0); + + // First waypoint (+0.3 m in X direction) + // Cartesian Position (in panda_link0 frame): + // [X: 0.6070218670533757, Y: 0, Z: 0.5902504197143968] + state.setVariablePosition("panda_joint1", 0.00011058924053135735); + state.setVariablePosition("panda_joint2", 0.15980591412916012); + state.setVariablePosition("panda_joint3", -0.000269206763021151); + state.setVariablePosition("panda_joint4", -1.4550637907602342); + state.setVariablePosition("panda_joint5", 0.0006907805230834268); + state.setVariablePosition("panda_joint6", 1.61442119426705); + state.setVariablePosition("panda_joint7", 0.7845267455355481); + trajectory.addSuffixWayPoint(state, 0.0); + WAYPOINT_DISTANCES.push_back(0.3); + + // Second waypoint (+0.3 m in Y direction) + // Cartesian Position (in panda_link0 frame): + // [X: 0.6070218670533757, Y: 0.3, Z: 0.5902504197143968] + state.setVariablePosition("panda_joint1", 0.32516555661705315); + state.setVariablePosition("panda_joint2", 0.4668669802969372); + state.setVariablePosition("panda_joint3", 0.20650832887601522); + state.setVariablePosition("panda_joint4", -1.0166745094262262); + state.setVariablePosition("panda_joint5", -0.0931020003693296); + state.setVariablePosition("panda_joint6", 1.4764599578787032); + state.setVariablePosition("panda_joint7", 1.2855361917975465); + trajectory.addSuffixWayPoint(state, 0.0); + WAYPOINT_DISTANCES.push_back(0.3); + + // Third waypoint (-0.3 m in Z direction) + // Cartesian Position (in panda_link0 frame): + // [X: 0.6070218670533757, Y: 0.3, Z: 0.29026011026061577] + state.setVariablePosition("panda_joint1", 0.1928958411545848); + state.setVariablePosition("panda_joint2", 0.5600654280773957); + state.setVariablePosition("panda_joint3", 0.31117191776899084); + state.setVariablePosition("panda_joint4", -1.6747509079656924); + state.setVariablePosition("panda_joint5", -0.20206061876786893); + state.setVariablePosition("panda_joint6", 2.2024820844782385); + state.setVariablePosition("panda_joint7", 1.3635216856419043); + trajectory.addSuffixWayPoint(state, 0.0); + WAYPOINT_DISTANCES.push_back(0.3); + + return true; +} + +void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + const std::vector& idx = group->getVariableIndexList(); + unsigned int count = trajectory.getWayPointCount(); + + RCLCPP_INFO_STREAM(LOGGER, + "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds."); + for (unsigned i = 0; i < count; i++) + { + moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i); + RCLCPP_INFO_STREAM(LOGGER, "Waypoint " << i << " time " << trajectory.getWayPointDurationFromStart(i) << " pos " + << point->getVariablePosition(idx[0]) << " vel " + << point->getVariableVelocity(idx[0]) << " acc " + << point->getVariableAcceleration(idx[0])); + + if (i > 0) + { + moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); + RCLCPP_INFO_STREAM( + LOGGER, + "jrk " << (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / + (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); + } + } +} + +TEST(TestCartesianSpeed, TestCartesianEndEffectorSpeed) +{ + trajectory_processing::TimeOptimalTrajectoryGeneration time_parameterization; + EXPECT_EQ(initStraightTrajectory(TRAJECTORY), true); + const char* end_effector_link = "panda_link8"; + + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); + trajectory_processing::limitMaxCartesianLinkSpeed(TRAJECTORY, 0.01, end_effector_link); + printTrajectory(TRAJECTORY); + size_t num_waypoints = TRAJECTORY.getWayPointCount(); + moveit::core::RobotStatePtr kinematic_state = TRAJECTORY.getFirstWayPointPtr(); + Eigen::Isometry3d current_end_effector_state = kinematic_state->getGlobalLinkTransform(end_effector_link); + Eigen::Isometry3d next_end_effector_state; + double euclidean_distance = 0.0; + + // Check average speed of the total trajectory by exact calculations + for (size_t i = 0; i < num_waypoints - 1; i++) + { + kinematic_state = TRAJECTORY.getWayPointPtr(i + 1); + next_end_effector_state = kinematic_state->getGlobalLinkTransform(end_effector_link); + euclidean_distance += (next_end_effector_state.translation() - current_end_effector_state.translation()).norm(); + current_end_effector_state = next_end_effector_state; + } + double avg_speed = euclidean_distance / TRAJECTORY.getWayPointDurationFromStart(num_waypoints); + ASSERT_NEAR(0.01, avg_speed, 2e-4); + + // Check average speed between waypoints using the information about the hand + // designed waypoints + for (size_t i = 1; i < num_waypoints; i++) + { + double current_avg_speed = WAYPOINT_DISTANCES[i - 1] / TRAJECTORY.getWayPointDurationFromPrevious(i); + ASSERT_NEAR(0.01, current_avg_speed, 2e-4); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/moveit_py/moveit/core/robot_trajectory.pyi b/moveit_py/moveit/core/robot_trajectory.pyi index a8e1415c09..15c98381da 100644 --- a/moveit_py/moveit/core/robot_trajectory.pyi +++ b/moveit_py/moveit/core/robot_trajectory.pyi @@ -5,6 +5,7 @@ class RobotTrajectory: def __init__(self, *args, **kwargs) -> None: ... def apply_totg_time_parameterization(self, *args, **kwargs) -> Any: ... def apply_ruckig_smoothing(self, *args, **kwargs) -> Any: ... + def limit_max_cartesian_link_speed(self, *args, **kwargs) -> Any: ... def get_robot_trajectory_msg(self, *args, **kwargs) -> Any: ... def get_waypoint_durations(self, *args, **kwargs) -> Any: ... def set_robot_trajectory_msg(self, *args, **kwargs) -> Any: ... diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index b5360cbdc4..54dfd1d8e7 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -164,6 +164,19 @@ void initRobotTrajectory(py::module& m) Returns: bool: True if the trajectory was successfully retimed, false otherwise. )") + .def("limit_max_cartesian_link_speed", + py::overload_cast( + &trajectory_processing::limitMaxCartesianLinkSpeed), + py::arg("max_speed"), py::arg("link_name"), + R"( + Limits the maximum Cartesian speed of a specified link in the trajectory. If link_name is empty, the end effectors of the trajectory's joint model group will be used. + Args: + trajectory (:py:class:`moveit_py.robot_trajectory.RobotTrajectory`): The robot trajectory to be modified. + max_speed (float): The maximum allowed Cartesian speed for the specified link. + link_name (str): The name of the link whose Cartesian speed is to be limited. If empty, the end effectors of the trajectory's joint model group will be used. + Returns: + bool: True if the speed limiting was successful, false otherwise. + )") .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::getRobotTrajectoryMsg, py::arg("joint_filter") = std::vector(), R"( 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 0bf79bb987..f1061c3a91 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 @@ -46,6 +46,7 @@ #include #include #include +#include #include using moveit::getLogger; @@ -197,6 +198,13 @@ bool MoveGroupCartesianPathService::computeService( trajectory_processing::TimeOptimalTrajectoryGeneration time_param; time_param.computeTimeStamps(rt, req->max_velocity_scaling_factor, req->max_acceleration_scaling_factor); + // optionally compute timing to move the eef with constant speed + if (req->max_cartesian_speed > 0.0) + { + trajectory_processing::limitMaxCartesianLinkSpeed(rt, req->max_cartesian_speed, + req->cartesian_speed_limited_link); + } + rt.getRobotTrajectoryMsg(res->solution); RCLCPP_INFO(getLogger(), "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)", static_cast(traj.size()), res->fraction * 100.0); diff --git a/moveit_ros/planning/default_request_adapters_plugin_description.xml b/moveit_ros/planning/default_request_adapters_plugin_description.xml index 3ddcedfd35..f0db110422 100644 --- a/moveit_ros/planning/default_request_adapters_plugin_description.xml +++ b/moveit_ros/planning/default_request_adapters_plugin_description.xml @@ -30,4 +30,10 @@ + + + Reparameterize the trajectory to keep it below a desired Cartesian speed for a given link, e.g., the end effector. + + + diff --git a/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt index 7471eefa34..c281ff75bc 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt @@ -4,7 +4,8 @@ generate_parameter_library(default_response_adapter_parameters add_library( moveit_default_planning_response_adapter_plugins SHARED src/add_ruckig_traj_smoothing.cpp src/add_time_optimal_parameterization.cpp - src/display_motion_path.cpp src/validate_path.cpp) + src/display_motion_path.cpp src/validate_path.cpp + src/limit_max_cartesian_link_speed.cpp) target_link_libraries(moveit_default_planning_response_adapter_plugins default_response_adapter_parameters) diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/limit_max_cartesian_link_speed.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/limit_max_cartesian_link_speed.cpp new file mode 100644 index 0000000000..482efa83c4 --- /dev/null +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/limit_max_cartesian_link_speed.cpp @@ -0,0 +1,104 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2020, Benjamin Scholz + * Copyright (c) 2021, Thies Oelerich + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the authors nor the names of other + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Benjamin Scholz, Thies Oelerich, based off add_time_parameterization.cpp by Ioan Sucan */ + +#include +#include +#include +#include + +#include + +namespace default_planning_response_adapters +{ +using namespace trajectory_processing; + +/** @brief This adapter uses the limit max Cartesian link speed method */ +class LimitMaxCartesianLinkSpeed : public planning_interface::PlanningResponseAdapter +{ +public: + LimitMaxCartesianLinkSpeed() : logger_(moveit::getLogger("moveit.ros.limit_max_cartesian_link_speed")) + { + } + + ~LimitMaxCartesianLinkSpeed() override = default; + + void initialize(const rclcpp::Node::SharedPtr& /*node*/, const std::string& /*parameter_namespace*/) override + { + } + + [[nodiscard]] std::string getDescription() const override + { + return std::string("Limiting Max Cartesian Speed"); + } + + void adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const override + { + RCLCPP_DEBUG(logger_, " Running '%s'", getDescription().c_str()); + if (!res.trajectory) + { + RCLCPP_ERROR(logger_, "Cannot apply response adapter '%s' because MotionPlanResponse does not contain a path.", + getDescription().c_str()); + res.error_code = moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN; + return; + } + if (req.max_cartesian_speed <= 0.0) + return; + RCLCPP_DEBUG(logger_, "'%s' below '%f' [m/s] for link '%s'", getDescription().c_str(), req.max_cartesian_speed, + req.cartesian_speed_limited_link.c_str()); + if (!trajectory_processing::limitMaxCartesianLinkSpeed(*res.trajectory, req.max_cartesian_speed, + req.cartesian_speed_limited_link)) + { + RCLCPP_ERROR(logger_, "Limiting Cartesian speed for the solution path failed."); + res.error_code = moveit::core::MoveItErrorCode::FAILURE; + } + else + { + res.error_code = moveit::core::MoveItErrorCode::SUCCESS; + } + } + +protected: + // std::unique_ptr param_listener_; + rclcpp::Logger logger_; +}; +} // namespace default_planning_response_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_response_adapters::LimitMaxCartesianLinkSpeed, + planning_interface::PlanningResponseAdapter); \ No newline at end of file diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp index c68b02bb53..77f3095124 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp @@ -259,6 +259,15 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ double getMaxAccelerationScalingFactor() const; + /** \brief Limit the maximum Cartesian speed for a given link. + The unit of the speed is meter per second and must be greater than 0. + The desired speed is a maximum bound. Slower parts of the trajectory will + be left unchanged. If no link_name is given, the first end effector tip will be assumed. */ + void limitMaxCartesianLinkSpeed(const double max_speed, const std::string& link_name = ""); + + /** \brief Clear maximum Cartesian speed of the end effector. */ + void clearMaxCartesianLinkSpeed(); + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; 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 40e3fbdeed..c836686305 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 @@ -164,6 +164,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal_orientation_tolerance_ = 1e-3; // ~0.1 deg allowed_planning_time_ = 5.0; num_planning_attempts_ = 1; + max_cartesian_speed_ = 0.0; node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", max_velocity_scaling_factor_, 0.1); node_->get_parameter_or("robot_description_planning.default_acceleration_scaling_factor", @@ -410,6 +411,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } + void limitMaxCartesianLinkSpeed(const double max_speed, const std::string& link_name) + { + cartesian_speed_limited_link_ = link_name; + max_cartesian_speed_ = max_speed; + } + + void clearMaxCartesianLinkSpeed() + { + cartesian_speed_limited_link_ = ""; + max_cartesian_speed_ = 0.0; + } + moveit::core::RobotState& getTargetRobotState() { return *joint_state_target_; @@ -889,6 +902,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->link_name = getEndEffectorLink(); req->max_velocity_scaling_factor = max_velocity_scaling_factor_; req->max_acceleration_scaling_factor = max_acceleration_scaling_factor_; + req->cartesian_speed_limited_link = cartesian_speed_limited_link_; + req->max_cartesian_speed = max_cartesian_speed_; auto future_response = cartesian_path_service_->async_send_request(req); if (future_response.valid()) @@ -1032,6 +1047,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.num_planning_attempts = num_planning_attempts_; request.max_velocity_scaling_factor = max_velocity_scaling_factor_; request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; + request.cartesian_speed_limited_link = cartesian_speed_limited_link_; + request.max_cartesian_speed = max_cartesian_speed_; request.allowed_planning_time = allowed_planning_time_; request.pipeline_id = planning_pipeline_id_; request.planner_id = planner_id_; @@ -1232,6 +1249,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl unsigned int num_planning_attempts_; double max_velocity_scaling_factor_; double max_acceleration_scaling_factor_; + std::string cartesian_speed_limited_link_; + double max_cartesian_speed_; double goal_joint_tolerance_; double goal_position_tolerance_; double goal_orientation_tolerance_; @@ -1409,6 +1428,16 @@ void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } +void MoveGroupInterface::limitMaxCartesianLinkSpeed(const double max_speed, const std::string& link_name) +{ + impl_->limitMaxCartesianLinkSpeed(max_speed, link_name); +} + +void MoveGroupInterface::clearMaxCartesianLinkSpeed() +{ + impl_->clearMaxCartesianLinkSpeed(); +} + double MoveGroupInterface::getMaxAccelerationScalingFactor() const { return impl_->getMaxAccelerationScalingFactor();