diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index a479366ae4..a64e18a070 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -48,6 +48,11 @@ References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, - / [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**: + + * ``/p`` [double] + * ``/i`` [double] + * ``/d`` [double] Commands ,,,,,,,,, diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 2b08b9dc15..c7d7fc75ae 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -76,6 +76,9 @@ class PidController : public controller_interface::ChainableControllerInterface size_t dof_; std::vector measured_state_values_; + static inline const std::vector GAIN_INTERFACES = {"p", "i", "d"}; + static inline const std::vector GAIN_TYPES_INDEX = {0, 1, 2}; + using PidPtr = std::shared_ptr; std::vector pids_; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 0831951aab..38f61b65f9 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -346,11 +346,16 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf std::vector PidController::on_export_reference_interfaces() { - reference_interfaces_.resize( - dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::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::quiet_NaN()); std::vector 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 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::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]; diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 6e77f23867..d258b07625 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/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 `///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 + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index f8b27f1d49..0e84b1fca4 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/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} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml index 673abfe08e..e5baa8a651 100644 --- a/pid_controller/test/pid_controller_preceding_params.yaml +++ b/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 diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index d1e9754830..567617dde8 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/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); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 5d2afc9871..5d9c5e036f 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/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); diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 31629ed7b7..d58ad5dd51 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/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_) {