From eae07e55a81a71b87d8d79d3535d0c9b523842b1 Mon Sep 17 00:00:00 2001 From: Hugal31 Date: Mon, 26 Feb 2024 19:27:18 +0100 Subject: [PATCH 1/5] Add joints_allowed_start_tolerance parameter (moveit/moveit#3287) ... for joint-specific start tolerances for TrajectoryExecutionManager Co-authored-by: Robert Haschke --- .../trajectory_execution_manager.hpp | 5 ++ .../src/trajectory_execution_manager.cpp | 47 +++++++++++++--- .../test/test_execution_manager.cpp | 54 ++++++++++++++++++- 3 files changed, 96 insertions(+), 10 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index 86c190b3ca..bcf9f308a2 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -292,6 +292,9 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager void loadControllerParams(); + double getJointAllowedStartTolerance(std::string const& jointName) const; + void updateJointsAllowedStartTolerance(); + // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; @@ -340,6 +343,8 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager std::map controller_allowed_goal_duration_margin_; double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints + // tolerance per joint, overrides global allowed_start_tolerance_. + std::map joints_allowed_start_tolerance_; double execution_velocity_scaling_; bool wait_for_trajectory_completion_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 00fa65ea17..2c9ddb5e48 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -99,6 +99,7 @@ void TrajectoryExecutionManager::initialize() execution_duration_monitoring_ = true; execution_velocity_scaling_ = 1.0; allowed_start_tolerance_ = 0.01; + joints_allowed_start_tolerance_.clear(); wait_for_trajectory_completion_ = true; control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES; @@ -210,6 +211,8 @@ void TrajectoryExecutionManager::initialize() controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables", control_multi_dof_joint_variables_); + updateJointsAllowedStartTolerance(); + if (manage_controllers_) { RCLCPP_INFO(logger_, "Trajectory execution is managing controllers"); @@ -914,7 +917,7 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const { - if (allowed_start_tolerance_ == 0) // skip validation on this magic number + if (allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) // skip validation on this magic number return true; RCLCPP_INFO(logger_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); @@ -960,14 +963,15 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (const auto joint : joints) { + double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); reference_state.enforcePositionBounds(joint); current_state->enforcePositionBounds(joint); - if (reference_state.distance(*current_state, joint) > allowed_start_tolerance_) + if (joint_start_tolerance != 0 && reference_state.distance(*current_state, joint) > joint_start_tolerance) { RCLCPP_ERROR(logger_, "Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'." "\nEnable DEBUG for detailed state info.", - allowed_start_tolerance_, joint->getName().c_str()); + joint_start_tolerance, joint->getName().c_str()); RCLCPP_DEBUG(logger_, "| Joint | Expected | Current |"); for (const auto& joint_name : joint_names) { @@ -1012,10 +1016,12 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); - if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) + double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); + if (joint_start_tolerance != 0 && + ((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance)) { RCLCPP_ERROR_STREAM(logger_, "\nInvalid Trajectory: start point deviates from current robot state more than " - << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i] + << joint_start_tolerance << "\nmulti-dof joint '" << joint_names[i] << "': pos delta: " << offset.transpose() << " rot delta: " << rotation.angle()); return false; @@ -1236,6 +1242,8 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callba stopExecution(false); + updateJointsAllowedStartTolerance(); + // check whether first trajectory starts at current robot state if (!trajectories_.empty() && !validate(*trajectories_.front())) { @@ -1568,7 +1576,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time) { // skip waiting for convergence? - if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_) + if ((allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) || !wait_for_trajectory_completion_) { RCLCPP_INFO(logger_, "Not waiting for trajectory completion"); return true; @@ -1607,8 +1615,8 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care - if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > - allowed_start_tolerance_) + double joint_tolerance = getJointAllowedStartTolerance(joint_names[i]); + if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > joint_tolerance) { moved = true; no_motion_count = 0; @@ -1834,4 +1842,27 @@ void TrajectoryExecutionManager::loadControllerParams() // } // } } + +double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string const& jointName) const +{ + auto start_tolerance_it = joints_allowed_start_tolerance_.find(jointName); + return start_tolerance_it != joints_allowed_start_tolerance_.end() ? start_tolerance_it->second : + allowed_start_tolerance_; +} + +void TrajectoryExecutionManager::updateJointsAllowedStartTolerance() +{ + joints_allowed_start_tolerance_.clear(); + node_handle_.getParam("trajectory_execution/joints_allowed_start_tolerance", joints_allowed_start_tolerance_); + + // remove negative values + for (auto it = joints_allowed_start_tolerance_.begin(); it != joints_allowed_start_tolerance_.end();) + { + if (it->second < 0) + it = joints_allowed_start_tolerance_.erase(it); + else + ++it; + } +} + } // namespace trajectory_execution_manager diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index 00963d0792..e222fb4562 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -60,15 +60,21 @@ class MoveItCppTest : public ::testing::Test trajectory_execution_manager_ptr = moveit_cpp_ptr->getTrajectoryExecutionManagerNonConst(); traj1.joint_trajectory.joint_names.push_back("panda_joint1"); - traj1.joint_trajectory.points.resize(1); + traj1.joint_trajectory.points.resize(2); traj1.joint_trajectory.points[0].positions.push_back(0.0); + traj1.joint_trajectory.points[1].positions.push_back(0.5); + traj1.joint_trajectory.points[1].time_from_start.fromSec(0.5); traj2 = traj1; traj2.joint_trajectory.joint_names.push_back("panda_joint2"); traj2.joint_trajectory.points[0].positions.push_back(1.0); + traj2.joint_trajectory.points[1].positions.push_back(1.5); traj2.multi_dof_joint_trajectory.joint_names.push_back("panda_joint3"); - traj2.multi_dof_joint_trajectory.points.resize(1); + traj2.multi_dof_joint_trajectory.points.resize(2); traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1); + traj2.multi_dof_joint_trajectory.points[1].transforms.resize(1); + + ros::param::del("~/trajectory_execution/joints_allowed_start_tolerance"); } protected: @@ -108,6 +114,50 @@ TEST_F(MoveItCppTest, PushExecuteAndWaitTest) ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); } +TEST_F(MoveItCppTest, RejectTooFarFromStart) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.01); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::ABORTED); +} + +TEST_F(MoveItCppTest, AcceptAllowedJointStartTolerance) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.01); + ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0.5); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); +} + +TEST_F(MoveItCppTest, DoNotValidateJointStartToleranceZero) +{ + moveit_msgs::RobotTrajectory traj = traj1; + traj.joint_trajectory.points[0].positions[0] = 0.3; + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); + + trajectory_execution_manager_ptr->setAllowedStartTolerance(0.1); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::ABORTED); + + ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0); + ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); + last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); + ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); +} + } // namespace moveit_cpp int main(int argc, char** argv) From 858442e62445fb06ed88bcaf51e0fe9e6cb42e41 Mon Sep 17 00:00:00 2001 From: DaniGarciaLopez Date: Tue, 4 Feb 2025 18:52:55 +0100 Subject: [PATCH 2/5] Port to moveit2 --- .../trajectory_execution_manager.hpp | 2 +- .../src/trajectory_execution_manager.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index bcf9f308a2..2d8305ae45 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -292,7 +292,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager void loadControllerParams(); - double getJointAllowedStartTolerance(std::string const& jointName) const; + double getJointAllowedStartTolerance(std::string const& joint_name) const; void updateJointsAllowedStartTolerance(); // Name of this class for logging diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 2c9ddb5e48..6103e3766f 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -963,7 +963,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (const auto joint : joints) { - double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); + const double joint_start_tolerance = getJointAllowedStartTolerance(joint->getName()); reference_state.enforcePositionBounds(joint); current_state->enforcePositionBounds(joint); if (joint_start_tolerance != 0 && reference_state.distance(*current_state, joint) > joint_start_tolerance) @@ -1016,7 +1016,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); - double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); + const double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); if (joint_start_tolerance != 0 && ((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance)) { @@ -1615,7 +1615,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care - double joint_tolerance = getJointAllowedStartTolerance(joint_names[i]); + const double joint_tolerance = getJointAllowedStartTolerance(joint_names[i]); if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > joint_tolerance) { moved = true; @@ -1843,9 +1843,9 @@ void TrajectoryExecutionManager::loadControllerParams() // } } -double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string const& jointName) const +double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string const& joint_name) const { - auto start_tolerance_it = joints_allowed_start_tolerance_.find(jointName); + auto start_tolerance_it = joints_allowed_start_tolerance_.find(joint_name); return start_tolerance_it != joints_allowed_start_tolerance_.end() ? start_tolerance_it->second : allowed_start_tolerance_; } @@ -1853,7 +1853,12 @@ double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string con void TrajectoryExecutionManager::updateJointsAllowedStartTolerance() { joints_allowed_start_tolerance_.clear(); - node_handle_.getParam("trajectory_execution/joints_allowed_start_tolerance", joints_allowed_start_tolerance_); + + for (const auto& joint_name : robot_model_->getJointModelNames()) { + double joint_start_tolerance; + if (node_->get_parameter("trajectory_execution.joints_allowed_start_tolerance." + joint_name, joint_start_tolerance)) + joints_allowed_start_tolerance_.insert_or_assign(joint_name, joint_start_tolerance); + } // remove negative values for (auto it = joints_allowed_start_tolerance_.begin(); it != joints_allowed_start_tolerance_.end();) From 0fedc17d53ee021b18add9032f43d123391cfb71 Mon Sep 17 00:00:00 2001 From: DaniGarciaLopez Date: Tue, 4 Feb 2025 20:52:40 +0100 Subject: [PATCH 3/5] Allow joints_allowed_start_tolerance to be updated dynamically --- .../trajectory_execution_manager.hpp | 3 +- .../src/trajectory_execution_manager.cpp | 70 ++++++++++++++----- 2 files changed, 55 insertions(+), 18 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index 2d8305ae45..ee28f15aad 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -293,7 +293,8 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager void loadControllerParams(); double getJointAllowedStartTolerance(std::string const& joint_name) const; - void updateJointsAllowedStartTolerance(); + void setJointAllowedStartTolerance(std::string const& joint_name, double joint_start_tolerance); + void initializeJointsAllowedStartTolerance(); // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 6103e3766f..c3923e1677 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -211,7 +211,7 @@ void TrajectoryExecutionManager::initialize() controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables", control_multi_dof_joint_variables_); - updateJointsAllowedStartTolerance(); + initializeJointsAllowedStartTolerance(); if (manage_controllers_) { @@ -248,6 +248,10 @@ void TrajectoryExecutionManager::initialize() { setAllowedStartTolerance(parameter.as_double()); } + else if (name.find("trajectory_execution.joints_allowed_start_tolerance.") == 0) + { + setJointAllowedStartTolerance(name, parameter.as_double()); + } else if (name == "trajectory_execution.wait_for_trajectory_completion") { setWaitForTrajectoryCompletion(parameter.as_bool()); @@ -920,7 +924,17 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont if (allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) // skip validation on this magic number return true; - RCLCPP_INFO(logger_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_); + if (joints_allowed_start_tolerance_.empty()) { + RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_); + } else { + std::ostringstream ss; + for (const auto& [joint_name, joints_start_tolerance] : joints_allowed_start_tolerance_) { + if (ss.tellp() > 1) ss << ", "; // skip the comma at the end + ss << joint_name << ": " << joints_start_tolerance; + } + RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_<< + " and joints_allowed_start_tolerance {" << ss.str() << "}"); + } moveit::core::RobotStatePtr current_state; if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState())) @@ -1242,8 +1256,6 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callba stopExecution(false); - updateJointsAllowedStartTolerance(); - // check whether first trajectory starts at current robot state if (!trajectories_.empty() && !validate(*trajectories_.front())) { @@ -1850,23 +1862,47 @@ double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string con allowed_start_tolerance_; } -void TrajectoryExecutionManager::updateJointsAllowedStartTolerance() +void TrajectoryExecutionManager::setJointAllowedStartTolerance(std::string const& parameter_name, + double joint_start_tolerance) { - joints_allowed_start_tolerance_.clear(); + if (joint_start_tolerance < 0) { + RCLCPP_WARN(logger_, "%s has a negative value. " + "The start tolerance value for that joint was not updated.", parameter_name.c_str()); + return; + } - for (const auto& joint_name : robot_model_->getJointModelNames()) { - double joint_start_tolerance; - if (node_->get_parameter("trajectory_execution.joints_allowed_start_tolerance." + joint_name, joint_start_tolerance)) - joints_allowed_start_tolerance_.insert_or_assign(joint_name, joint_start_tolerance); + // get the joint name by removing the parameter prefix if necessary + std::string joint_name = parameter_name; + const std::string parameter_prefix = "trajectory_execution.joints_allowed_start_tolerance."; + if (parameter_name.find(parameter_prefix) == 0) + joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix + + if (!robot_model_->hasJointModel(joint_name)) { + RCLCPP_WARN(logger_, "Joint '%s' was not found in the robot model. " + "The start tolerance value for that joint was not updated.", joint_name.c_str()); + return; } - // remove negative values - for (auto it = joints_allowed_start_tolerance_.begin(); it != joints_allowed_start_tolerance_.end();) - { - if (it->second < 0) - it = joints_allowed_start_tolerance_.erase(it); - else - ++it; + joints_allowed_start_tolerance_.insert_or_assign(joint_name, joint_start_tolerance); +} + +void TrajectoryExecutionManager::initializeJointsAllowedStartTolerance() +{ + joints_allowed_start_tolerance_.clear(); + + // retrieve all parameters under "trajectory_execution.joints_allowed_start_tolerance" + // that correspond to existing joints in the robot model + for (const auto& joint_name : robot_model_->getJointModelNames()) { + double start_joint_tolerance; + const std::string parameter_name = "trajectory_execution.joints_allowed_start_tolerance." + joint_name; + if (node_->get_parameter(parameter_name, start_joint_tolerance)) { + if (start_joint_tolerance < 0) { + RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint " + "will default to allowed_start_tolerance.", parameter_name.c_str()); + continue; + } + joints_allowed_start_tolerance_.insert({joint_name, start_joint_tolerance}); + } } } From 274b1bd025a8ce68323476d6c801d5dd1b993128 Mon Sep 17 00:00:00 2001 From: DaniGarciaLopez Date: Tue, 4 Feb 2025 22:12:45 +0100 Subject: [PATCH 4/5] Apply formatting --- .../trajectory_execution_manager.hpp | 4 +- .../src/trajectory_execution_manager.cpp | 59 ++++++++++++------- 2 files changed, 39 insertions(+), 24 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index ee28f15aad..6b4d498677 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -292,8 +292,8 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager void loadControllerParams(); - double getJointAllowedStartTolerance(std::string const& joint_name) const; - void setJointAllowedStartTolerance(std::string const& joint_name, double joint_start_tolerance); + double getJointAllowedStartTolerance(const std::string& joint_name) const; + void setJointAllowedStartTolerance(const std::string& joint_name, double joint_start_tolerance); void initializeJointsAllowedStartTolerance(); // Name of this class for logging diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index c3923e1677..de581e9f90 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -924,16 +924,22 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont if (allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) // skip validation on this magic number return true; - if (joints_allowed_start_tolerance_.empty()) { + if (joints_allowed_start_tolerance_.empty()) + { RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_); - } else { + } + else + { std::ostringstream ss; - for (const auto& [joint_name, joints_start_tolerance] : joints_allowed_start_tolerance_) { - if (ss.tellp() > 1) ss << ", "; // skip the comma at the end - ss << joint_name << ": " << joints_start_tolerance; + for (const auto& [joint_name, joints_start_tolerance] : joints_allowed_start_tolerance_) + { + if (ss.tellp() > 1) + ss << ", "; // skip the comma at the end + ss << joint_name << ": " << joints_start_tolerance; } - RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_<< - " and joints_allowed_start_tolerance {" << ss.str() << "}"); + RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " + << allowed_start_tolerance_ << " and joints_allowed_start_tolerance {" << ss.str() + << "}"); } moveit::core::RobotStatePtr current_state; @@ -1855,19 +1861,20 @@ void TrajectoryExecutionManager::loadControllerParams() // } } -double TrajectoryExecutionManager::getJointAllowedStartTolerance(std::string const& joint_name) const +double TrajectoryExecutionManager::getJointAllowedStartTolerance(const std::string& joint_name) const { auto start_tolerance_it = joints_allowed_start_tolerance_.find(joint_name); return start_tolerance_it != joints_allowed_start_tolerance_.end() ? start_tolerance_it->second : allowed_start_tolerance_; } -void TrajectoryExecutionManager::setJointAllowedStartTolerance(std::string const& parameter_name, +void TrajectoryExecutionManager::setJointAllowedStartTolerance(const std::string& parameter_name, double joint_start_tolerance) { - if (joint_start_tolerance < 0) { - RCLCPP_WARN(logger_, "%s has a negative value. " - "The start tolerance value for that joint was not updated.", parameter_name.c_str()); + if (joint_start_tolerance < 0) + { + RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint was not updated.", + parameter_name.c_str()); return; } @@ -1875,11 +1882,14 @@ void TrajectoryExecutionManager::setJointAllowedStartTolerance(std::string const std::string joint_name = parameter_name; const std::string parameter_prefix = "trajectory_execution.joints_allowed_start_tolerance."; if (parameter_name.find(parameter_prefix) == 0) - joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix + joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix - if (!robot_model_->hasJointModel(joint_name)) { - RCLCPP_WARN(logger_, "Joint '%s' was not found in the robot model. " - "The start tolerance value for that joint was not updated.", joint_name.c_str()); + if (!robot_model_->hasJointModel(joint_name)) + { + RCLCPP_WARN(logger_, + "Joint '%s' was not found in the robot model. " + "The start tolerance value for that joint was not updated.", + joint_name.c_str()); return; } @@ -1892,16 +1902,21 @@ void TrajectoryExecutionManager::initializeJointsAllowedStartTolerance() // retrieve all parameters under "trajectory_execution.joints_allowed_start_tolerance" // that correspond to existing joints in the robot model - for (const auto& joint_name : robot_model_->getJointModelNames()) { + for (const auto& joint_name : robot_model_->getJointModelNames()) + { double start_joint_tolerance; const std::string parameter_name = "trajectory_execution.joints_allowed_start_tolerance." + joint_name; - if (node_->get_parameter(parameter_name, start_joint_tolerance)) { - if (start_joint_tolerance < 0) { - RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint " - "will default to allowed_start_tolerance.", parameter_name.c_str()); + if (node_->get_parameter(parameter_name, start_joint_tolerance)) + { + if (start_joint_tolerance < 0) + { + RCLCPP_WARN(logger_, + "%s has a negative value. The start tolerance value for that joint " + "will default to allowed_start_tolerance.", + parameter_name.c_str()); continue; } - joints_allowed_start_tolerance_.insert({joint_name, start_joint_tolerance}); + joints_allowed_start_tolerance_.insert({ joint_name, start_joint_tolerance }); } } } From 7dc0087affa7f8d9fedfaf74f5b570e8016ade58 Mon Sep 17 00:00:00 2001 From: DaniGarciaLopez Date: Wed, 5 Feb 2025 13:16:32 +0100 Subject: [PATCH 5/5] Rename joints_allowed_start_tolerance to allowed_start_tolerance_joints --- .../trajectory_execution_manager.hpp | 8 +-- .../src/trajectory_execution_manager.cpp | 57 ++++++++++--------- .../test/test_execution_manager.cpp | 6 +- 3 files changed, 36 insertions(+), 35 deletions(-) diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index 6b4d498677..de453558b7 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -292,9 +292,9 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager void loadControllerParams(); - double getJointAllowedStartTolerance(const std::string& joint_name) const; - void setJointAllowedStartTolerance(const std::string& joint_name, double joint_start_tolerance); - void initializeJointsAllowedStartTolerance(); + double getAllowedStartToleranceJoint(const std::string& joint_name) const; + void setAllowedStartToleranceJoint(const std::string& joint_name, double joint_start_tolerance); + void initializeAllowedStartToleranceJoints(); // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; @@ -345,7 +345,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints // tolerance per joint, overrides global allowed_start_tolerance_. - std::map joints_allowed_start_tolerance_; + std::map allowed_start_tolerance_joints_; double execution_velocity_scaling_; bool wait_for_trajectory_completion_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index de581e9f90..02da59b12f 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -99,7 +99,7 @@ void TrajectoryExecutionManager::initialize() execution_duration_monitoring_ = true; execution_velocity_scaling_ = 1.0; allowed_start_tolerance_ = 0.01; - joints_allowed_start_tolerance_.clear(); + allowed_start_tolerance_joints_.clear(); wait_for_trajectory_completion_ = true; control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES; @@ -211,7 +211,7 @@ void TrajectoryExecutionManager::initialize() controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables", control_multi_dof_joint_variables_); - initializeJointsAllowedStartTolerance(); + initializeAllowedStartToleranceJoints(); if (manage_controllers_) { @@ -248,9 +248,9 @@ void TrajectoryExecutionManager::initialize() { setAllowedStartTolerance(parameter.as_double()); } - else if (name.find("trajectory_execution.joints_allowed_start_tolerance.") == 0) + else if (name.find("trajectory_execution.allowed_start_tolerance_joints.") == 0) { - setJointAllowedStartTolerance(name, parameter.as_double()); + setAllowedStartToleranceJoint(name, parameter.as_double()); } else if (name == "trajectory_execution.wait_for_trajectory_completion") { @@ -921,24 +921,24 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const { - if (allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) // skip validation on this magic number + if (allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) // skip validation on this magic number return true; - if (joints_allowed_start_tolerance_.empty()) + if (allowed_start_tolerance_joints_.empty()) { RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_); } else { std::ostringstream ss; - for (const auto& [joint_name, joints_start_tolerance] : joints_allowed_start_tolerance_) + for (const auto& [joint_name, joint_start_tolerance] : allowed_start_tolerance_joints_) { if (ss.tellp() > 1) ss << ", "; // skip the comma at the end - ss << joint_name << ": " << joints_start_tolerance; + ss << joint_name << ": " << joint_start_tolerance; } RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " - << allowed_start_tolerance_ << " and joints_allowed_start_tolerance {" << ss.str() + << allowed_start_tolerance_ << " and allowed_start_tolerance_joints {" << ss.str() << "}"); } @@ -983,7 +983,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont for (const auto joint : joints) { - const double joint_start_tolerance = getJointAllowedStartTolerance(joint->getName()); + const double joint_start_tolerance = getAllowedStartToleranceJoint(joint->getName()); reference_state.enforcePositionBounds(joint); current_state->enforcePositionBounds(joint); if (joint_start_tolerance != 0 && reference_state.distance(*current_state, joint) > joint_start_tolerance) @@ -1036,7 +1036,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); - const double joint_start_tolerance = getJointAllowedStartTolerance(joint_names[i]); + const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]); if (joint_start_tolerance != 0 && ((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance)) { @@ -1594,7 +1594,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index) bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time) { // skip waiting for convergence? - if ((allowed_start_tolerance_ == 0 && joints_allowed_start_tolerance_.empty()) || !wait_for_trajectory_completion_) + if ((allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) || !wait_for_trajectory_completion_) { RCLCPP_INFO(logger_, "Not waiting for trajectory completion"); return true; @@ -1633,8 +1633,9 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care - const double joint_tolerance = getJointAllowedStartTolerance(joint_names[i]); - if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > joint_tolerance) + const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]); + if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > + joint_start_tolerance) { moved = true; no_motion_count = 0; @@ -1861,14 +1862,14 @@ void TrajectoryExecutionManager::loadControllerParams() // } } -double TrajectoryExecutionManager::getJointAllowedStartTolerance(const std::string& joint_name) const +double TrajectoryExecutionManager::getAllowedStartToleranceJoint(const std::string& joint_name) const { - auto start_tolerance_it = joints_allowed_start_tolerance_.find(joint_name); - return start_tolerance_it != joints_allowed_start_tolerance_.end() ? start_tolerance_it->second : + auto start_tolerance_it = allowed_start_tolerance_joints_.find(joint_name); + return start_tolerance_it != allowed_start_tolerance_joints_.end() ? start_tolerance_it->second : allowed_start_tolerance_; } -void TrajectoryExecutionManager::setJointAllowedStartTolerance(const std::string& parameter_name, +void TrajectoryExecutionManager::setAllowedStartToleranceJoint(const std::string& parameter_name, double joint_start_tolerance) { if (joint_start_tolerance < 0) @@ -1880,7 +1881,7 @@ void TrajectoryExecutionManager::setJointAllowedStartTolerance(const std::string // get the joint name by removing the parameter prefix if necessary std::string joint_name = parameter_name; - const std::string parameter_prefix = "trajectory_execution.joints_allowed_start_tolerance."; + const std::string parameter_prefix = "trajectory_execution.allowed_start_tolerance_joints."; if (parameter_name.find(parameter_prefix) == 0) joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix @@ -1893,22 +1894,22 @@ void TrajectoryExecutionManager::setJointAllowedStartTolerance(const std::string return; } - joints_allowed_start_tolerance_.insert_or_assign(joint_name, joint_start_tolerance); + allowed_start_tolerance_joints_.insert_or_assign(joint_name, joint_start_tolerance); } -void TrajectoryExecutionManager::initializeJointsAllowedStartTolerance() +void TrajectoryExecutionManager::initializeAllowedStartToleranceJoints() { - joints_allowed_start_tolerance_.clear(); + allowed_start_tolerance_joints_.clear(); - // retrieve all parameters under "trajectory_execution.joints_allowed_start_tolerance" + // retrieve all parameters under "trajectory_execution.allowed_start_tolerance_joints" // that correspond to existing joints in the robot model for (const auto& joint_name : robot_model_->getJointModelNames()) { - double start_joint_tolerance; - const std::string parameter_name = "trajectory_execution.joints_allowed_start_tolerance." + joint_name; - if (node_->get_parameter(parameter_name, start_joint_tolerance)) + double joint_start_tolerance; + const std::string parameter_name = "trajectory_execution.allowed_start_tolerance_joints." + joint_name; + if (node_->get_parameter(parameter_name, joint_start_tolerance)) { - if (start_joint_tolerance < 0) + if (joint_start_tolerance < 0) { RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint " @@ -1916,7 +1917,7 @@ void TrajectoryExecutionManager::initializeJointsAllowedStartTolerance() parameter_name.c_str()); continue; } - joints_allowed_start_tolerance_.insert({ joint_name, start_joint_tolerance }); + allowed_start_tolerance_joints_.insert({ joint_name, joint_start_tolerance }); } } } diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index e222fb4562..5ea24f10b4 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -74,7 +74,7 @@ class MoveItCppTest : public ::testing::Test traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1); traj2.multi_dof_joint_trajectory.points[1].transforms.resize(1); - ros::param::del("~/trajectory_execution/joints_allowed_start_tolerance"); + ros::param::del("~/trajectory_execution/allowed_start_tolerance_joints"); } protected: @@ -131,7 +131,7 @@ TEST_F(MoveItCppTest, AcceptAllowedJointStartTolerance) traj.joint_trajectory.points[0].positions[0] = 0.3; trajectory_execution_manager_ptr->setAllowedStartTolerance(0.01); - ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0.5); + ros::param::set("~/trajectory_execution/allowed_start_tolerance_joints/panda_joint1", 0.5); ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED); @@ -152,7 +152,7 @@ TEST_F(MoveItCppTest, DoNotValidateJointStartToleranceZero) last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::ABORTED); - ros::param::set("~/trajectory_execution/joints_allowed_start_tolerance/panda_joint1", 0); + ros::param::set("~/trajectory_execution/allowed_start_tolerance_joints/panda_joint1", 0); ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj)); last_execution_status = trajectory_execution_manager_ptr->executeAndWait(); ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED);