From 4113212031b2cbe83ef0ffcfd3a2032bb67e5d37 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 9 Oct 2023 17:48:14 +0000
Subject: [PATCH] Add option to hold position on last command

---
 joint_trajectory_controller/doc/parameters.rst         |  5 +++++
 .../src/joint_trajectory_controller.cpp                | 10 +++++++++-
 .../src/joint_trajectory_controller_parameters.yaml    |  5 +++++
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst
index 95a6599191..e6f1c4b98c 100644
--- a/joint_trajectory_controller/doc/parameters.rst
+++ b/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.
 
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 5e39bffc73..661a785e1d 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/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);
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index ce17ba8bf9..51ea8aa2e9 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/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,