Merge branch 'main' into fix/collision-monitoring-attached-objects-not-updating

Author: MarcoMagriDev

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp

@@ -292,6 +292,10 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
 
   void loadControllerParams();
 
+  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";
 
@@ -340,6 +344,8 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   std::map<std::string, double> 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<std::string, double> allowed_start_tolerance_joints_;
   double execution_velocity_scaling_;
   bool wait_for_trajectory_completion_;
 

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;
+  allowed_start_tolerance_joints_.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_);
 
+  initializeAllowedStartToleranceJoints();
+
   if (manage_controllers_)
   {
     RCLCPP_INFO(logger_, "Trajectory execution is managing controllers");
@@ -245,6 +248,10 @@ void TrajectoryExecutionManager::initialize()
       {
         setAllowedStartTolerance(parameter.as_double());
       }
+      else if (name.find("trajectory_execution.allowed_start_tolerance_joints.") == 0)
+      {
+        setAllowedStartToleranceJoint(name, parameter.as_double());
+      }
       else if (name == "trajectory_execution.wait_for_trajectory_completion")
       {
         setWaitForTrajectoryCompletion(parameter.as_bool());
@@ -914,10 +921,26 @@ 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 && allowed_start_tolerance_joints_.empty())  // skip validation on this magic number
     return true;
 
-  RCLCPP_INFO(logger_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
+  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, joint_start_tolerance] : allowed_start_tolerance_joints_)
+    {
+      if (ss.tellp() > 1)
+        ss << ", ";  // skip the comma at the end
+      ss << joint_name << ": " << joint_start_tolerance;
+    }
+    RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance "
+                                    << allowed_start_tolerance_ << " and allowed_start_tolerance_joints {" << ss.str()
+                                    << "}");
+  }
 
   moveit::core::RobotStatePtr current_state;
   if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
@@ -960,14 +983,15 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
 
       for (const auto joint : joints)
       {
+        const double joint_start_tolerance = getAllowedStartToleranceJoint(joint->getName());
         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 +1036,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_)
+        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))
         {
           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;
@@ -1568,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 || !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;
@@ -1607,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_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]);
         if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) >
-            allowed_start_tolerance_)
+            joint_start_tolerance)
         {
           moved = true;
           no_motion_count = 0;
@@ -1834,4 +1861,65 @@ void TrajectoryExecutionManager::loadControllerParams()
   //   }
   // }
 }
+
+double TrajectoryExecutionManager::getAllowedStartToleranceJoint(const std::string& joint_name) const
+{
+  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::setAllowedStartToleranceJoint(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());
+    return;
+  }
+
+  // get the joint name by removing the parameter prefix if necessary
+  std::string joint_name = parameter_name;
+  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
+
+  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;
+  }
+
+  allowed_start_tolerance_joints_.insert_or_assign(joint_name, joint_start_tolerance);
+}
+
+void TrajectoryExecutionManager::initializeAllowedStartToleranceJoints()
+{
+  allowed_start_tolerance_joints_.clear();
+
+  // 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 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 (joint_start_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;
+      }
+      allowed_start_tolerance_joints_.insert({ joint_name, joint_start_tolerance });
+    }
+  }
+}
+
 }  // namespace trajectory_execution_manager

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/allowed_start_tolerance_joints");
   }
 
 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/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);
+}
+
+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/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);
+}
+
 }  // namespace moveit_cpp
 
 int main(int argc, char** argv)