Add option to hold position on last command

joint_trajectory_controller/doc/parameters.rst

@@ -63,6 +63,11 @@ start_with_holding (bool)
 
   Default: true
 
+holding_position_open_loop: (bool)
+  If true, holding position is set to the last command instead of the current position from the hardware.
+
+  Default: false
+
 allow_nonzero_velocity_at_trajectory_end (boolean)
   If false, the last velocity point has to be zero or the goal will be rejected.
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1508,7 +1508,15 @@ std::shared_ptr<trajectory_msgs::msg::JointTrajectory>
 JointTrajectoryController::set_hold_position()
 {
   // Command to stay at current position
-  hold_position_msg_ptr_->points[0].positions = state_current_.positions;
+  // Position states always exist
+  if (params_.holding_position_open_loop)
+  {
+    hold_position_msg_ptr_->points[0].positions = last_commanded_state_.positions;
+  }
+  else
+  {
+    hold_position_msg_ptr_->points[0].positions = state_current_.positions;
+  }
 
   // set flag, otherwise tolerances will be checked with holding position too
   rt_is_holding_.writeFromNonRT(true);

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -78,6 +78,11 @@ joint_trajectory_controller:
     default_value: true,
     description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.",
   }
+  holding_position_open_loop: {
+    type: bool,
+    default_value: false,
+    description: "If true, holding position is set to the last command instead of the current position from the hardware.",
+  }
   allow_nonzero_velocity_at_trajectory_end: {
     type: bool,
     default_value: true,