[JTC] Fix hold position mode with goal_time>0 (backport #758)

Author: christophfroehlich

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -173,6 +173,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
+  realtime_tools::RealtimeBuffer<bool> rt_is_holding_;  ///< are we holding position?
   // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
@@ -186,6 +187,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
     traj_msg_external_point_ptr_;
 
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> hold_position_msg_ptr_ = nullptr;
+
   using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
   using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
   using StatePublisherPtr = std::unique_ptr<StatePublisher>;
@@ -274,6 +277,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool contains_interface_type(
     const std::vector<std::string> & interface_type_list, const std::string & interface_type);
 
+  void init_hold_position_msg();
   void resize_joint_trajectory_point(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
   void resize_joint_trajectory_point_command(

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -230,15 +230,17 @@ controller_interface::return_type JointTrajectoryController::update(
         if (
           (before_last_point || first_sample) &&
           !check_state_tolerance_per_joint(
-            state_error_, index, default_tolerances_.state_tolerance[index], false))
+            state_error_, index, default_tolerances_.state_tolerance[index], false) &&
+          *(rt_is_holding_.readFromRT()) == false)
         {
           tolerance_violated_while_moving = true;
         }
         // past the final point, check that we end up inside goal tolerance
         if (
           !before_last_point &&
           !check_state_tolerance_per_joint(
-            state_error_, index, default_tolerances_.goal_state_tolerance[index], false))
+            state_error_, index, default_tolerances_.goal_state_tolerance[index], false) &&
+          *(rt_is_holding_.readFromRT()) == false)
         {
           outside_goal_tolerance = true;
 
@@ -831,6 +833,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     logger, "Using '%s' interpolation method.",
     interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str());
 
+  // prepare hold_position_msg
+  init_hold_position_msg();
+
+  // create subscriber and publishers
   joint_command_subscriber_ =
     get_node()->create_subscription<trajectory_msgs::msg::JointTrajectory>(
       "~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
@@ -1018,6 +1024,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   {
     add_new_trajectory_msg(set_hold_position());
   }
+  rt_is_holding_.writeFromNonRT(true);
 
   return CallbackReturn::SUCCESS;
 }
@@ -1203,6 +1210,7 @@ void JointTrajectoryController::topic_callback(
   if (subscriber_is_active_)
   {
     add_new_trajectory_msg(msg);
+    rt_is_holding_.writeFromNonRT(false);
   }
 };
 
@@ -1265,6 +1273,7 @@ void JointTrajectoryController::goal_accepted_callback(
       std::make_shared<trajectory_msgs::msg::JointTrajectory>(goal_handle->get_goal()->trajectory);
 
     add_new_trajectory_msg(traj_msg);
+    rt_is_holding_.writeFromNonRT(false);
   }
 
   // Update the active goal
@@ -1559,31 +1568,12 @@ std::shared_ptr<trajectory_msgs::msg::JointTrajectory>
 JointTrajectoryController::set_hold_position()
 {
   // Command to stay at current position
-  trajectory_msgs::msg::JointTrajectory current_pose_msg;
-  current_pose_msg.header.stamp =
-    rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type());  // start immediately
-  current_pose_msg.joint_names = params_.joints;
-  current_pose_msg.points.push_back(state_current_);
-  current_pose_msg.points[0].velocities.clear();
-  current_pose_msg.points[0].accelerations.clear();
-  current_pose_msg.points[0].effort.clear();
-  if (has_velocity_command_interface_)
-  {
-    // ensure no velocity (PID will fix this)
-    current_pose_msg.points[0].velocities.resize(dof_, 0.0);
-  }
-  if (has_acceleration_command_interface_)
-  {
-    // ensure no acceleration
-    current_pose_msg.points[0].accelerations.resize(dof_, 0.0);
-  }
-  if (has_effort_command_interface_)
-  {
-    // ensure no explicit effort (PID will fix this)
-    current_pose_msg.points[0].effort.resize(dof_, 0.0);
-  }
+  hold_position_msg_ptr_->points[0].positions = state_current_.positions;
 
-  return std::make_shared<trajectory_msgs::msg::JointTrajectory>(current_pose_msg);
+  // set flag, otherwise tolerances will be checked with holding position too
+  rt_is_holding_.writeFromNonRT(true);
+
+  return hold_position_msg_ptr_;
 }
 
 bool JointTrajectoryController::contains_interface_type(
@@ -1633,6 +1623,28 @@ bool JointTrajectoryController::has_active_trajectory()
   return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();
 }
 
+void JointTrajectoryController::init_hold_position_msg()
+{
+  hold_position_msg_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
+  hold_position_msg_ptr_->header.stamp =
+    rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type());  // start immediately
+  hold_position_msg_ptr_->joint_names = params_.joints;
+  hold_position_msg_ptr_->points.resize(1);  // a trivial msg only
+  hold_position_msg_ptr_->points[0].velocities.clear();
+  hold_position_msg_ptr_->points[0].accelerations.clear();
+  hold_position_msg_ptr_->points[0].effort.clear();
+  if (has_velocity_command_interface_ || has_acceleration_command_interface_)
+  {
+    // add velocity, so that trajectory sampling returns velocity points in any case
+    hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0);
+  }
+  if (has_acceleration_command_interface_)
+  {
+    // add velocity, so that trajectory sampling returns acceleration points in any case
+    hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0);
+  }
+}
+
 }  // namespace joint_trajectory_controller
 
 #include "pluginlib/class_list_macros.hpp"

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -68,11 +68,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest
 
   void SetUpExecutor(
     const std::vector<rclcpp::Parameter> & parameters = {},
-    bool separate_cmd_and_state_values = false)
+    bool separate_cmd_and_state_values = false, double kp = 0.0)
   {
     setup_executor_ = true;
 
-    SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values);
+    SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp);
 
     SetUpActionClient();
 
@@ -247,7 +247,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
   // note: the action goal also is a trivial trajectory
-  expectHoldingPoint(point_positions);
+  if (traj_controller_->has_position_command_interface())
+  {
+    expectHoldingPoint(point_positions);
+  }
+  else
+  {
+    // no integration to position state interface from velocity/acceleration
+    expectHoldingPoint(INITIAL_POS_JOINTS);
+  }
 }
 
 TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal)
@@ -294,7 +302,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal
 
   // it should be holding the last position goal
   // i.e., active but trivial trajectory (one point only)
-  expectHoldingPoint(points_positions.at(1));
+  if (traj_controller_->has_position_command_interface())
+  {
+    expectHoldingPoint(points_positions.at(1));
+  }
+  else
+  {
+    // no integration to position state interface from velocity/acceleration
+    expectHoldingPoint(INITIAL_POS_JOINTS);
+  }
 }
 
 /**

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -1673,7 +1673,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
     rclcpp::Parameter("constraints.joint3.trajectory", state_tol)};
 
   rclcpp::executors::MultiThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor, params, true);
+  double kp = 1.0;  // activate feedback control for testing velocity/effort PID
+  SetUpAndActivateTrajectoryController(executor, params, true, kp);
 
   // send msg
   constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100);
@@ -1705,7 +1706,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
     rclcpp::Parameter("constraints.goal_time", goal_time)};
 
   rclcpp::executors::MultiThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor, params, true);
+  SetUpAndActivateTrajectoryController(executor, params, true, 1.0);
 
   // send msg
   constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100);
@@ -1722,6 +1723,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
 
   // it should have aborted and be holding now
   expectHoldingPoint(joint_state_pos_);
+
+  // what happens if we wait longer but it harms the tolerance again?
+  auto hold_position = joint_state_pos_;
+  joint_state_pos_.at(0) = -3.3;
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+  // it should be still holding the old point
+  expectHoldingPoint(hold_position);
 }
 
 // position controllers

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -38,6 +38,12 @@ const std::vector<double> INITIAL_POS_JOINTS = {
 const std::vector<double> INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0};
 const std::vector<double> INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0};
 const std::vector<double> INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0};
+
+bool is_same_sign_or_zero(double val1, double val2)
+{
+  return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0);
+}
+
 }  // namespace
 
 namespace test_trajectory_controllers
@@ -465,32 +471,65 @@ class TrajectoryControllerTest : public ::testing::Test
     // i.e., active but trivial trajectory (one point only)
     EXPECT_TRUE(traj_controller_->has_trivial_traj());
 
-    if (traj_controller_->has_position_command_interface())
+    if (traj_controller_->use_closed_loop_pid_adapter() == false)
     {
-      EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD);
-      EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD);
-      EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD);
-    }
+      if (traj_controller_->has_position_command_interface())
+      {
+        EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD);
+        EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD);
+        EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD);
+      }
 
-    if (traj_controller_->has_velocity_command_interface())
-    {
-      EXPECT_EQ(0.0, joint_vel_[0]);
-      EXPECT_EQ(0.0, joint_vel_[1]);
-      EXPECT_EQ(0.0, joint_vel_[2]);
-    }
+      if (traj_controller_->has_velocity_command_interface())
+      {
+        EXPECT_EQ(0.0, joint_vel_[0]);
+        EXPECT_EQ(0.0, joint_vel_[1]);
+        EXPECT_EQ(0.0, joint_vel_[2]);
+      }
 
-    if (traj_controller_->has_acceleration_command_interface())
-    {
-      EXPECT_EQ(0.0, joint_acc_[0]);
-      EXPECT_EQ(0.0, joint_acc_[1]);
-      EXPECT_EQ(0.0, joint_acc_[2]);
-    }
+      if (traj_controller_->has_acceleration_command_interface())
+      {
+        EXPECT_EQ(0.0, joint_acc_[0]);
+        EXPECT_EQ(0.0, joint_acc_[1]);
+        EXPECT_EQ(0.0, joint_acc_[2]);
+      }
 
-    if (traj_controller_->has_effort_command_interface())
+      if (traj_controller_->has_effort_command_interface())
+      {
+        EXPECT_EQ(0.0, joint_eff_[0]);
+        EXPECT_EQ(0.0, joint_eff_[1]);
+        EXPECT_EQ(0.0, joint_eff_[2]);
+      }
+    }
+    else
     {
-      EXPECT_EQ(0.0, joint_eff_[0]);
-      EXPECT_EQ(0.0, joint_eff_[1]);
-      EXPECT_EQ(0.0, joint_eff_[2]);
+      // velocity or effort PID?
+      // velocity setpoint is always zero -> feedforward term does not have an effect
+      // --> set kp > 0.0 in test
+      if (traj_controller_->has_velocity_command_interface())
+      {
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0]))
+          << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is "
+          << joint_vel_[0];
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1]))
+          << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is "
+          << joint_vel_[1];
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2]))
+          << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is "
+          << joint_vel_[2];
+      }
+      if (traj_controller_->has_effort_command_interface())
+      {
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0]))
+          << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is "
+          << joint_eff_[0];
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1]))
+          << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is "
+          << joint_eff_[1];
+        EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2]))
+          << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is "
+          << joint_eff_[2];
+      }
     }
   }