Export pid gains as reference

pid_controller/doc/userdoc.rst

@@ -48,6 +48,11 @@ References (from a preceding controller)
 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
 - <reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]>  [double]
   **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``.
+- If the parameter **`export_params.gain_references`** is set to **`true`**, the following interfaces are also exported as references for preceding controllers to claim, **when the PID controller is used in a chained mode**:
+
+   * ``<dof_names[i]>/p`` [double]
+   * ``<dof_names[i]>/i`` [double]
+   * ``<dof_names[i]>/d`` [double]
 
 Commands
 ,,,,,,,,,

pid_controller/include/pid_controller/pid_controller.hpp

@@ -76,6 +76,9 @@ class PidController : public controller_interface::ChainableControllerInterface
   size_t dof_;
   std::vector<double> measured_state_values_;
 
+  static inline const std::vector<std::string> GAIN_INTERFACES = {"p", "i", "d"};
+  static inline const std::vector<size_t> GAIN_TYPES_INDEX = {0, 1, 2};
+
   using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
   std::vector<PidPtr> pids_;
 

pid_controller/src/pid_controller.cpp

@@ -346,11 +346,16 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf
 
 std::vector<hardware_interface::CommandInterface> PidController::on_export_reference_interfaces()
 {
-  reference_interfaces_.resize(
-    dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits<double>::quiet_NaN());
+  const size_t dof_reference_size = dof_ * params_.reference_and_state_interfaces.size();
 
+  size_t total_reference_size = dof_reference_size;
+  if (params_.export_params.gain_references)
+  {
+    total_reference_size += dof_ * GAIN_INTERFACES.size();
+  }
+  reference_interfaces_.resize(total_reference_size, std::numeric_limits<double>::quiet_NaN());
   std::vector<hardware_interface::CommandInterface> reference_interfaces;
-  reference_interfaces.reserve(reference_interfaces_.size());
+  reference_interfaces.reserve(total_reference_size);
 
   size_t index = 0;
   for (const auto & interface : params_.reference_and_state_interfaces)
@@ -365,6 +370,22 @@ std::vector<hardware_interface::CommandInterface> PidController::on_export_refer
     }
   }
 
+  if (params_.export_params.gain_references)
+  {
+    size_t gains_start_index = dof_reference_size;
+    for (const auto & gain_name : GAIN_INTERFACES)
+    {
+      for (const auto & dof_name : reference_and_state_dof_names_)
+      {
+        reference_interfaces.push_back(
+          hardware_interface::CommandInterface(
+            std::string(get_node()->get_name()) + "/" + dof_name, gain_name,
+            &reference_interfaces_[gains_start_index]));
+        ++gains_start_index;
+      }
+    }
+  }
+
   return reference_interfaces;
 }
 
@@ -438,7 +459,8 @@ controller_interface::return_type PidController::update_reference_from_subscribe
     if (!std::isnan(current_ref_.values[i]))
     {
       reference_interfaces_[i] = current_ref_.values[i];
-      if (reference_interfaces_.size() == 2 * dof_ && !std::isnan(current_ref_.values_dot[i]))
+      const size_t dof_reference_size = dof_ * params_.reference_and_state_interfaces.size();
+      if (dof_reference_size == 2 * dof_ && !std::isnan(current_ref_.values_dot[i]))
       {
         reference_interfaces_[dof_ + i] = current_ref_.values_dot[i];
       }
@@ -495,6 +517,43 @@ controller_interface::return_type PidController::update_and_write_commands(
     state_interfaces_values_[i] = measured_state_values_[i];
   }
 
+  // Calculate size of DOF references for indexing
+  const size_t dof_reference_size = dof_ * params_.reference_and_state_interfaces.size();
+
+  if (params_.export_params.gain_references)
+  {
+    size_t gains_start_index = dof_reference_size;
+    for (size_t i = 0; i < dof_; ++i)
+    {
+      auto current_pid_gains = pids_[i]->get_gains();
+      for (size_t j = 0; j < GAIN_INTERFACES.size(); ++j)
+      {
+        const size_t buffer_index = gains_start_index + (j * dof_) + i;
+        const double new_gain_value = reference_interfaces_[buffer_index];
+        if (std::isfinite(new_gain_value))
+        {
+          const size_t gain_type = GAIN_TYPES_INDEX[j];
+          switch (gain_type)
+          {
+            case 0:  // P gain
+              current_pid_gains.p_gain_ = new_gain_value;
+              pids_[i]->set_gains(current_pid_gains);
+              break;
+            case 1:  // I gain
+              current_pid_gains.i_gain_ = new_gain_value;
+              pids_[i]->set_gains(current_pid_gains);
+              break;
+            case 2:  // D gain
+              current_pid_gains.d_gain_ = new_gain_value;
+              pids_[i]->set_gains(current_pid_gains);
+              break;
+          }
+          reference_interfaces_[buffer_index] = std::numeric_limits<double>::quiet_NaN();
+        }
+      }
+    }
+  }
+
   // Iterate through all the dofs to calculate the output command
   for (size_t i = 0; i < dof_; ++i)
   {
@@ -503,7 +562,7 @@ controller_interface::return_type PidController::update_and_write_commands(
     if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i]))
     {
       // calculate feed-forward
-      if (reference_interfaces_.size() == 2 * dof_)
+      if (dof_reference_size == 2 * dof_)
       {
         // two interfaces
         if (std::isfinite(reference_interfaces_[dof_ + i]))
@@ -527,7 +586,7 @@ controller_interface::return_type PidController::update_and_write_commands(
       }
 
       // checking if there are two interfaces
-      if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
+      if (dof_reference_size == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
       {
         if (
           std::isfinite(reference_interfaces_[dof_ + i]) &&
@@ -567,7 +626,7 @@ controller_interface::return_type PidController::update_and_write_commands(
     {
       state_msg_.dof_states[i].reference = reference_interfaces_[i];
       state_msg_.dof_states[i].feedback = measured_state_values_[i];
-      if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
+      if (dof_reference_size == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
       {
         state_msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i];
       }
@@ -578,7 +637,7 @@ controller_interface::return_type PidController::update_and_write_commands(
         state_msg_.dof_states[i].error =
           angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]);
       }
-      if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
+      if (dof_reference_size == 2 * dof_ && measured_state_values_.size() == 2 * dof_)
       {
         state_msg_.dof_states[i].error_dot =
           reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i];

pid_controller/src/pid_controller.yaml

@@ -126,3 +126,10 @@ pid_controller:
         default_value: false,
         description: "Individual state publisher activation for each DOF. If true, the controller will publish the state of each DOF to the topic `/<controller_name>/<dof_name>/pid_state`."
       }
+  export_params:
+      gain_references: {
+        type: bool,
+        default_value: false,
+        description: "If true, exports P, I, and D gains as reference interfaces to be claimed by preceding controllers",
+        read_only: true
+      }

pid_controller/test/pid_controller_params.yaml

@@ -14,6 +14,9 @@ test_pid_controller:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0}
 
@@ -26,6 +29,9 @@ test_pid_controller_unlimited:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0}
 
@@ -38,6 +44,9 @@ test_pid_controller_angle_wraparound_on:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: false
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, angle_wraparound: true}
 
@@ -50,6 +59,9 @@ test_pid_controller_with_feedforward_gain:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: true
+
     gains:
       joint1: {p: 0.5, i: 0.0, d: 0.0, feedforward_gain: 1.0}
 
@@ -63,6 +75,9 @@ test_pid_controller_with_feedforward_gain_dual_interface:
 
     reference_and_state_interfaces: ["position", "velocity"]
 
+    export_params:
+      gain_references: 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 +91,9 @@ test_save_i_term_off:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: false
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: false}
 
@@ -88,5 +106,8 @@ test_save_i_term_on:
 
     reference_and_state_interfaces: ["position"]
 
+    export_params:
+      gain_references: true
+
     gains:
       joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true}

pid_controller/test/pid_controller_preceding_params.yaml

@@ -9,3 +9,6 @@ test_pid_controller:
 
     reference_and_state_dof_names:
       - joint1state
+
+    export_params:
+      gain_references: true

pid_controller/test/test_pid_controller.cpp

@@ -85,7 +85,11 @@ TEST_F(PidControllerTest, check_exported_interfaces)
 
   // check ref itfs
   auto ref_if_conf = controller_->export_reference_interfaces();
-  ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size());
+  size_t actual_ref_size = dof_names_.size() * state_interfaces_.size() +
+                           (controller_->params_.export_params.gain_references
+                              ? dof_names_.size() * controller_->GAIN_INTERFACES.size()
+                              : 0);
+  ASSERT_EQ(ref_if_conf.size(), actual_ref_size);
   size_t ri_index = 0;
   for (const auto & interface : state_interfaces_)
   {
@@ -141,8 +145,11 @@ TEST_F(PidControllerTest, activate_success)
   {
     EXPECT_TRUE(std::isnan(cmd));
   }
-
-  EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
+  size_t actual_ref_size = dof_names_.size() * state_interfaces_.size() +
+                           (controller_->params_.export_params.gain_references
+                              ? dof_names_.size() * controller_->GAIN_INTERFACES.size()
+                              : 0);
+  EXPECT_EQ(controller_->reference_interfaces_.size(), actual_ref_size);
   for (const auto & interface : controller_->reference_interfaces_)
   {
     EXPECT_TRUE(std::isnan(interface));
@@ -226,10 +233,12 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-
-  EXPECT_EQ(
-    controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
-  EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
+  size_t actual_ref_size = dof_names_.size() * state_interfaces_.size() +
+                           (controller_->params_.export_params.gain_references
+                              ? dof_names_.size() * controller_->GAIN_INTERFACES.size()
+                              : 0);
+  EXPECT_EQ(controller_->reference_interfaces_.size(), actual_ref_size);
+  EXPECT_EQ(controller_->reference_interfaces_.size(), actual_ref_size);
   for (size_t i = 0; i < dof_command_values_.size(); ++i)
   {
     EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[i]));
@@ -284,10 +293,13 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
     controller_interface::return_type::OK);
 
   ASSERT_TRUE(controller_->is_in_chained_mode());
+  size_t actual_ref_size = dof_names_.size() * state_interfaces_.size() +
+                           (controller_->params_.export_params.gain_references
+                              ? dof_names_.size() * controller_->GAIN_INTERFACES.size()
+                              : 0);
 
-  EXPECT_EQ(
-    controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
-  EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
+  EXPECT_EQ(controller_->reference_interfaces_.size(), actual_ref_size);
+  EXPECT_EQ(controller_->reference_interfaces_.size(), actual_ref_size);
 
   // check the command value
   // ref = 5.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0
@@ -435,10 +447,20 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status)
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
-  for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i)
+  size_t num_control_references = dof_names_.size() * state_interfaces_.size();
+
+  for (size_t i = 0; i < num_control_references; ++i)
   {
     ASSERT_EQ(controller_->reference_interfaces_[i], 0.45);
   }
+  if (controller_->params_.export_params.gain_references)
+  {
+    for (size_t i = num_control_references; i < controller_->reference_interfaces_.size(); ++i)
+    {
+      // Check the remaining interfaces (P, I, D gains) are NaN
+      EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i]));
+    }
+  }
 
   subscribe_and_get_messages(msg);
 

pid_controller/test/test_pid_controller.hpp

@@ -51,6 +51,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 class TestablePidController : public pid_controller::PidController
 {
   FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success);
+  FRIEND_TEST(PidControllerTest, check_exported_interfaces);
   FRIEND_TEST(PidControllerTest, activate_success);
   FRIEND_TEST(PidControllerTest, reactivate_success);
   FRIEND_TEST(PidControllerTest, test_update_logic_zero_feedforward_gain);

pid_controller/test/test_pid_controller_preceding.cpp

@@ -74,7 +74,11 @@ TEST_F(PidControllerTest, check_exported_interfaces)
 
   // check ref itfs
   auto ref_if_conf = controller_->export_reference_interfaces();
-  ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size());
+  size_t actual_ref_size = dof_names_.size() * state_interfaces_.size() +
+                           (controller_->params_.export_params.gain_references
+                              ? dof_names_.size() * controller_->GAIN_INTERFACES.size()
+                              : 0);
+  ASSERT_EQ(ref_if_conf.size(), actual_ref_size);
   size_t ri_index = 0;
   for (const auto & interface : state_interfaces_)
   {