From dbfa74a647adc4be08cf718ba92674800e8c9918 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Mon, 3 Feb 2025 20:30:56 -0800 Subject: [PATCH 1/6] Fix logic for feedforward_mode with single reference interface --- .gitignore | 3 +++ pid_controller/src/pid_controller.cpp | 23 ++++++++++++++++------- 2 files changed, 19 insertions(+), 7 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..c3a5bcf2b8 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ + +/.vscode +diff_drive_controller/.vscode/settings.json diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 521227b49c..e6ec0bb236 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -498,7 +498,7 @@ controller_interface::return_type PidController::update_and_write_commands( for (size_t i = 0; i < dof_; ++i) { - double tmp_command = std::numeric_limits::quiet_NaN(); + double tmp_command = 0.0; // Using feedforward if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) @@ -506,12 +506,21 @@ controller_interface::return_type PidController::update_and_write_commands( // calculate feed-forward if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) { - tmp_command = reference_interfaces_[dof_ + i] * - params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; - } - else - { - tmp_command = 0.0; + // two interfaces + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + if (!std::isnan(reference_interfaces_[dof_ + i]) && + !std::isnan(measured_state_values_[dof_ + i])) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + else // one interface + { + tmp_command = reference_interfaces_[i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } } double error = reference_interfaces_[i] - measured_state_values_[i]; From 9c692b66500fa35e3a85b42d41310e970e278947 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Mon, 3 Feb 2025 22:24:11 -0800 Subject: [PATCH 2/6] Fix pre-commit issue --- pid_controller/src/pid_controller.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index e6ec0bb236..7afb2f27b0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -509,14 +509,15 @@ controller_interface::return_type PidController::update_and_write_commands( // two interfaces if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { - if (!std::isnan(reference_interfaces_[dof_ + i]) && - !std::isnan(measured_state_values_[dof_ + i])) + if ( + !std::isnan(reference_interfaces_[dof_ + i]) && + !std::isnan(measured_state_values_[dof_ + i])) { tmp_command = reference_interfaces_[dof_ + i] * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; } } - else // one interface + else // one interface { tmp_command = reference_interfaces_[i] * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; From f4a72a675b7df44f4ae6f1a95ef0401ec35d0fd2 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Tue, 4 Feb 2025 08:29:44 -0800 Subject: [PATCH 3/6] Update per review feedback. --- .gitignore | 3 --- pid_controller/src/pid_controller.cpp | 4 ++-- 2 files changed, 2 insertions(+), 5 deletions(-) delete mode 100644 .gitignore diff --git a/.gitignore b/.gitignore deleted file mode 100644 index c3a5bcf2b8..0000000000 --- a/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ - -/.vscode -diff_drive_controller/.vscode/settings.json diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 7afb2f27b0..ce71e04547 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -510,8 +510,8 @@ controller_interface::return_type PidController::update_and_write_commands( if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { if ( - !std::isnan(reference_interfaces_[dof_ + i]) && - !std::isnan(measured_state_values_[dof_ + i])) + std::isfinite(reference_interfaces_[dof_ + i]) && + std::isfinite(measured_state_values_[dof_ + i])) { tmp_command = reference_interfaces_[dof_ + i] * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; From db7433c062747f988e1e0efb6570d4eabbb82099 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Feb 2025 23:09:43 -0800 Subject: [PATCH 4/6] Add test case for chained pid controller with non zero feedforward gain --- pid_controller/src/pid_controller.cpp | 5 +- .../test/pid_controller_params.yaml | 13 ++++ pid_controller/test/test_pid_controller.cpp | 65 +++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 1 + 4 files changed, 80 insertions(+), 4 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index ce71e04547..3fe5c3bb17 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -500,7 +500,6 @@ controller_interface::return_type PidController::update_and_write_commands( { double tmp_command = 0.0; - // Using feedforward if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) { // calculate feed-forward @@ -509,9 +508,7 @@ controller_interface::return_type PidController::update_and_write_commands( // two interfaces if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { - if ( - std::isfinite(reference_interfaces_[dof_ + i]) && - std::isfinite(measured_state_values_[dof_ + i])) + if (std::isfinite(reference_interfaces_[dof_ + i])) { tmp_command = reference_interfaces_[dof_ + i] * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 7555cfc156..cf0a3f4173 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -9,3 +9,16 @@ test_pid_controller: gains: joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} + + +test_pid_controller_with_feedforward_gain: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 60d484e463..b0c935815b 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -521,6 +521,71 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } } +/** + * @brief check chained pid controller with feedforward and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95 + auto target_value = 5.0; + auto exepected_command_value = 6.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), exepected_command_value); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 4471f35a12..c90e1406b1 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -59,6 +59,7 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); public: controller_interface::CallbackReturn on_configure( From fdfff9628e1a56aa8b33031cf46dafcc9731d59e Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Tue, 11 Feb 2025 12:44:48 -0800 Subject: [PATCH 5/6] Add test case for feedforward off and non zero gain --- pid_controller/test/test_pid_controller.cpp | 66 +++++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 1 + 2 files changed, 67 insertions(+) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index b0c935815b..c36e8c0405 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -586,6 +586,72 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_EQ(controller_->command_interfaces_[0].get_value(), exepected_command_value); } +/** + * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // given target value 5.0 + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward off, the command value should be still 1.95 even though feedforward gain + // is 1.0 + auto target_value = 5.0; + auto exepected_command_value = 1.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // feedforward by default is OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), exepected_command_value); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index c90e1406b1..dd5ae029b9 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -60,6 +60,7 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); public: controller_interface::CallbackReturn on_configure( From fcb41c99da0c3b5f001776c4e20061321c15f7a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 13 Feb 2025 17:13:51 +0100 Subject: [PATCH 6/6] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- pid_controller/test/test_pid_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9ba9ce4a02..67ac680eae 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -550,8 +550,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) // state interface value is 1.1 as defined in test fixture // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95 - auto target_value = 5.0; - auto exepected_command_value = 6.95; + const double target_value = 5.0; + const double expected_command_value = 6.95; SetUpController("test_pid_controller_with_feedforward_gain"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -604,7 +604,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), exepected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -617,8 +617,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 // with feedforward off, the command value should be still 1.95 even though feedforward gain // is 1.0 - auto target_value = 5.0; - auto exepected_command_value = 1.95; + const double target_value = 5.0; + const double expected_command_value = 1.95; SetUpController("test_pid_controller_with_feedforward_gain"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -670,7 +670,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), exepected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } int main(int argc, char ** argv)