[PID Controllers] Set first set point to current measurement (#2205)

(cherry picked from commit bb214dc)

# Conflicts:
#	doc/release_notes.rst

Author: saikishor

doc/release_notes.rst

@@ -39,9 +39,15 @@ joint_trajectory_controller
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
   <https://github.com/ros-controls/ros2_controllers/pull/2043>`_)
 
+<<<<<<< HEAD
 omni_wheel_drive_controller
 *********************************
 * 🚀 The omni_wheel_drive_controller was added 🎉 (`#1535 <https://github.com/ros-controls/ros2_controllers/pull/1535>`_).
+=======
+pid_controller
+**************
+* Added parameter ``set_current_state_as_first_setpoint`` (default: true) to set the current state as the first setpoint when the controller is activated, helping to avoid large initial errors and sudden jumps in control output.
+>>>>>>> bb214dc ([PID Controllers] Set first set point to current measurement (#2205))
 
 pid_controller
 *******************************

pid_controller/src/pid_controller.cpp

@@ -411,11 +411,49 @@ controller_interface::CallbackReturn PidController::on_activate(
     measured_state_.try_set(current_state_);
   }
 
-  reference_interfaces_.assign(
-    reference_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
   measured_state_values_.assign(
     measured_state_values_.size(), std::numeric_limits<double>::quiet_NaN());
 
+  // Initialize reference interfaces from current state so initial reference equals current state
+  reference_interfaces_.assign(
+    reference_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
+
+  if (params_.set_current_state_as_first_setpoint)
+  {
+    if (params_.use_external_measured_states)
+    {
+      auto measured_state_opt = measured_state_.try_get();
+      if (measured_state_opt.has_value())
+      {
+        const auto & state = measured_state_opt.value();
+        for (size_t i = 0; i < dof_; ++i)
+        {
+          if (i < state.values.size() && !std::isnan(state.values[i]))
+          {
+            reference_interfaces_[i] = state.values[i];
+          }
+          if (
+            reference_interfaces_.size() == 2 * dof_ && i < state.values_dot.size() &&
+            !std::isnan(state.values_dot[i]))
+          {
+            reference_interfaces_[dof_ + i] = state.values_dot[i];
+          }
+        }
+      }
+    }
+    else
+    {
+      for (size_t i = 0; i < measured_state_values_.size(); ++i)
+      {
+        const auto state_interface_value_op = state_interfaces_[i].get_optional();
+        if (state_interface_value_op.has_value())
+        {
+          reference_interfaces_[i] = state_interface_value_op.value();
+        }
+      }
+    }
+  }
+
   // prefixed save_i_term parameter is read from ROS parameters
   for (auto & pid : pids_)
   {

pid_controller/src/pid_controller.yaml

@@ -38,6 +38,12 @@ pid_controller:
       size_lt<>: 3,
     }
   }
+  set_current_state_as_first_setpoint: {
+    type: bool,
+    default_value: true,
+    read_only: true,
+    description: "When true, the controller will set the current state as the first setpoint when the controller is activated. This can help to avoid large initial errors and sudden jumps in the control output when the controller starts."
+  }
   use_external_measured_states: {
     type: bool,
     default_value: false,

pid_controller/test/pid_controller_params.yaml

@@ -14,6 +14,8 @@ test_pid_controller:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0}
 
@@ -26,6 +28,8 @@ test_pid_controller_unlimited:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0}
 
@@ -38,6 +42,8 @@ test_pid_controller_angle_wraparound_on:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, angle_wraparound: true}
 
@@ -50,6 +56,8 @@ test_pid_controller_with_feedforward_gain:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 0.5, i: 0.0, d: 0.0, feedforward_gain: 1.0}
 
@@ -63,6 +71,8 @@ test_pid_controller_with_feedforward_gain_dual_interface:
 
     reference_and_state_interfaces: ["position", "velocity"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0}
       joint2: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0}
@@ -76,6 +86,8 @@ test_save_i_term_off:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: false}
 
@@ -88,5 +100,21 @@ test_save_i_term_on:
 
     reference_and_state_interfaces: ["position"]
 
+    set_current_state_as_first_setpoint: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true}
+
+test_pid_controller_no_first_setpoint:
+  ros__parameters:
+    dof_names:
+      - joint1
+
+    command_interface: position
+
+    reference_and_state_interfaces: ["position"]
+
+    set_current_state_as_first_setpoint: false
+
+    gains:
+      joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0}

pid_controller/test/test_pid_controller.cpp

@@ -142,10 +142,12 @@ TEST_F(PidControllerTest, activate_success)
     EXPECT_TRUE(std::isnan(cmd));
   }
 
+  // With set_current_state_as_first_setpoint=true (default), reference interfaces are initialized
+  // to the current state values on activation
   EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
-  for (const auto & interface : controller_->reference_interfaces_)
+  for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i)
   {
-    EXPECT_TRUE(std::isnan(interface));
+    EXPECT_EQ(controller_->reference_interfaces_[i], dof_state_values_[i]);
   }
 }
 
@@ -175,16 +177,18 @@ TEST_F(PidControllerTest, reactivate_success)
   SetUpController();
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  // With set_current_state_as_first_setpoint=true (default), reference interfaces are set to
+  // current state values (dof_state_values_[0] = 1.1) on each activation
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_EQ(controller_->reference_interfaces_[0], dof_state_values_[0]);
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
   ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_EQ(controller_->reference_interfaces_[0], dof_state_values_[0]);
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
   ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  ASSERT_EQ(controller_->reference_interfaces_[0], dof_state_values_[0]);
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
   ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
 
@@ -211,7 +215,7 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
   EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0]));
   for (const auto & interface : controller_->reference_interfaces_)
   {
-    EXPECT_TRUE(std::isnan(interface));
+    EXPECT_TRUE(std::isfinite(interface));
   }
 
   controller_->set_reference(dof_command_values_);
@@ -220,7 +224,7 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
   {
     EXPECT_FALSE(std::isnan(controller_->input_ref_.get().values[i]));
     EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]);
-    EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
+    EXPECT_TRUE(std::isfinite(controller_->reference_interfaces_[i]));
   }
 
   ASSERT_EQ(
@@ -389,7 +393,7 @@ TEST_F(PidControllerTest, subscribe_and_get_messages_success)
   for (size_t i = 0; i < dof_names_.size(); ++i)
   {
     ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]);
-    EXPECT_TRUE(std::isnan(msg.dof_states[i].reference));
+    EXPECT_TRUE(std::isfinite(msg.dof_states[i].reference));
     ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]);
   }
 }
@@ -414,21 +418,21 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status)
   for (size_t i = 0; i < dof_names_.size(); ++i)
   {
     ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]);
-    EXPECT_TRUE(std::isnan(msg.dof_states[i].reference));
+    EXPECT_TRUE(std::isfinite(msg.dof_states[i].reference));
     ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]);
   }
 
   for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i)
   {
-    EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
+    EXPECT_TRUE(std::isfinite(controller_->reference_interfaces_[i]));
   }
 
   publish_commands();
   controller_->wait_for_commands(executor);
 
   for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i)
   {
-    EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
+    EXPECT_TRUE(std::isfinite(controller_->reference_interfaces_[i]));
   }
 
   ASSERT_EQ(
@@ -690,6 +694,49 @@ TEST_F(PidControllerTest, test_save_i_term_on)
   EXPECT_NEAR(actual_value, 2.00002, 1e-5);  // i_term from above
 }
 
+/**
+ * @brief Test that reference interfaces are initialized to current state on activation when
+ * set_current_state_as_first_setpoint is true (default).
+ */
+TEST_F(PidControllerTest, test_activate_set_current_state_as_first_setpoint_true)
+{
+  SetUpController();  // uses test_pid_controller: set_current_state_as_first_setpoint defaults to
+                      // true
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_TRUE(controller_->params_.set_current_state_as_first_setpoint);
+
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // reference interfaces must be initialized to the current state values (dof_state_values_)
+  ASSERT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
+  for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i)
+  {
+    EXPECT_EQ(controller_->reference_interfaces_[i], dof_state_values_[i]);
+  }
+}
+
+/**
+ * @brief Test that reference interfaces remain NaN on activation when
+ * set_current_state_as_first_setpoint is false.
+ */
+TEST_F(PidControllerTest, test_activate_set_current_state_as_first_setpoint_false)
+{
+  SetUpController("test_pid_controller_no_first_setpoint");
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->params_.set_current_state_as_first_setpoint);
+
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // reference interfaces must remain NaN since set_current_state_as_first_setpoint is false
+  ASSERT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

pid_controller/test/test_pid_controller.hpp

@@ -64,6 +64,8 @@ class TestablePidController : public pid_controller::PidController
   FRIEND_TEST(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface);
   FRIEND_TEST(PidControllerTest, test_save_i_term_on);
   FRIEND_TEST(PidControllerTest, test_save_i_term_off);
+  FRIEND_TEST(PidControllerTest, test_activate_set_current_state_as_first_setpoint_true);
+  FRIEND_TEST(PidControllerTest, test_activate_set_current_state_as_first_setpoint_false);
 
 public:
   controller_interface::CallbackReturn on_configure(