[JTC] Add Parameter to Toggle State Setting on Activation

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -924,7 +924,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   // running already)
   trajectory_msgs::msg::JointTrajectoryPoint state;
   resize_joint_trajectory_point(state, dof_);
-  if (read_state_from_command_interfaces(state))
+  if (
+    params_.set_last_command_interface_value_as_state_on_activation &&
+    read_state_from_command_interfaces(state))
   {
     state_current_ = state;
     last_commanded_state_ = state;

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -64,6 +64,11 @@ joint_trajectory_controller:
     default_value: false,
     description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
   }
+  set_last_command_interface_value_as_state_on_activation: {
+    type: bool,
+    default_value: true,
+    description: "If enabled, set the last command interface value as both the current state and the last commanded state upon activation.",
+  }
   action_monitor_rate: {
     type: double,
     default_value: 20.0,