From ac309f12ff8d37f22ffbcd3bc853c836b7afc6b8 Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 29 Apr 2025 09:57:57 +0000 Subject: [PATCH 1/3] reset pid at rest --- .../include/pid_controller/pid_controller.hpp | 2 ++ pid_controller/src/pid_controller.cpp | 21 +++++++++++++++++-- pid_controller/src/pid_controller.yaml | 14 +++++++++++++ 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 39ec58be6b..c1f56a3b74 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -86,6 +86,8 @@ class PidController : public controller_interface::ChainableControllerInterface // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector feedforward_gain_; + double reset_pid_time_; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index e0ef70ffac..009f73b0be 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -496,7 +496,7 @@ controller_interface::return_type PidController::update_and_write_commands( { for (size_t i = 0; i < measured_state_values_.size(); ++i) { - measured_state_values_[i] = state_interfaces_[i].get_value(); + measured_state_values_[i] = state_interfaces_[i].get_optional().value(); } } @@ -533,6 +533,23 @@ controller_interface::return_type PidController::update_and_write_commands( } double error = reference_interfaces_[i] - measured_state_values_[i]; + + const auto zero_threshold = params_.gains.dof_names_map[params_.dof_names[i]].zero_threshold; + if (std::abs(reference_interfaces_[i]) < zero_threshold && std::abs(error) < zero_threshold) + { + reset_pid_time_ += period.seconds(); + if ( + reset_pid_time_ > + params_.gains.dof_names_map[params_.dof_names[i]].zero_threshold_duration) + { + pids_[i]->reset(); + } + } + else + { + reset_pid_time_ = 0.0; + } + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) { // for continuous angles the error is normalized between -pimsg_.dof_states[i].time_step = period.seconds(); // Command can store the old calculated values. This should be obvious because at least one // another value is NaN. - state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_optional().value(); } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 02448f2a19..0b3f889c2b 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -94,3 +94,17 @@ pid_controller: default_value: true, description: "Indicating if integral term is retained after re-activation" } + zero_threshold: { + type: double, + default_value: 0.0, + description: "If the output is below this threshold for a duration specified by + zero_threshold_duration, the PID will be reset. If set to 0.0, this functionality is + disabled. To reset integral term, set the save_i_term parameter to false." + } + zero_threshold_duration: { + type: double, + default_value: 0.5, + description: "Duration in seconds for which the output must be below the zero_threshold + before the PID will be reset. This is used to prevent the PID from resetting when the + output is only briefly below the threshold." + } From ad17dfdc5edee82aa65530894ba4d53dc5edd69a Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 29 Apr 2025 11:05:53 +0000 Subject: [PATCH 2/3] add test for zero threshold --- .../test/pid_controller_params.yaml | 12 +++ pid_controller/test/test_pid_controller.cpp | 87 ++++++++++++++++--- pid_controller/test/test_pid_controller.hpp | 1 + .../test_pid_controller_dual_interface.cpp | 4 +- 4 files changed, 88 insertions(+), 16 deletions(-) diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 15c4eaf049..44dc8535f7 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -71,3 +71,15 @@ test_save_i_term_on: gains: joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true} + +test_zero_threshold: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false, zero_threshold: 0.1, zero_threshold_duration: 0.01} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 1c0494a002..114d664e9d 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -181,15 +181,15 @@ TEST_F(PidControllerTest, reactivate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + 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_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + 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_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -272,7 +272,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } @@ -331,7 +332,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30102.301020; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } @@ -388,7 +390,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 const double expected_command_value = 1173.978; - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -415,7 +417,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // check the result of the commands - the values are not wrapped const double expected_command_value = 2679.078; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } /** @@ -446,7 +449,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // Check the command value const double expected_command_value = 787.713559; - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -583,7 +587,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(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -641,7 +645,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(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -677,7 +681,8 @@ TEST_F(PidControllerTest, test_save_i_term_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -690,7 +695,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 0.0, 1e-5); } @@ -727,7 +732,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -740,10 +746,63 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } +/** + * @brief Test if zero_threshold is working + * + */ + +TEST_F(PidControllerTest, test_zero_threshold) +{ + SetUpController("test_zero_threshold"); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + for (const auto & dof_name : dof_names_) + { + ASSERT_NE(controller_->params_.gains.dof_names_map[dof_name].zero_threshold, 0.0); + } + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + controller_->set_reference(dof_command_values_); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the command value + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 + const double expected_command_value = 30102.30102; + + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + + // set command and state to a value slightly less than zero threshold + const auto near_zero_value = + controller_->params_.gains.dof_names_map[dof_names_[0]].zero_threshold * 0.9; + controller_->set_reference({near_zero_value}); + dof_state_values_[0] = near_zero_value; + + // set the duration to a value larger than the zero threshold duration + const auto duration = + controller_->params_.gains.dof_names_map[dof_names_[0]].zero_threshold_duration * 1.1; + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(duration)), + controller_interface::return_type::OK); + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, 0.0, 1e-5); +} + 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 87b9e1de9a..eec8b2e0dd 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -66,6 +66,7 @@ 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_zero_threshold); public: controller_interface::CallbackReturn on_configure( diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index e006986473..f5b1442a57 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -96,8 +96,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i // check the commands const double joint1_expected_cmd = 8.915; const double joint2_expected_cmd = 9.915; - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); - ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd); } int main(int argc, char ** argv) From 436c6388afa7e4685a7b3280ccff47b4e6c1985c Mon Sep 17 00:00:00 2001 From: kmakd Date: Tue, 29 Apr 2025 11:11:54 +0000 Subject: [PATCH 3/3] revert get_optional to avoid conflicts --- pid_controller/src/pid_controller.cpp | 4 +-- pid_controller/test/test_pid_controller.cpp | 34 ++++++++----------- .../test_pid_controller_dual_interface.cpp | 4 +-- 3 files changed, 18 insertions(+), 24 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 009f73b0be..dcabc80a32 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -496,7 +496,7 @@ controller_interface::return_type PidController::update_and_write_commands( { for (size_t i = 0; i < measured_state_values_.size(); ++i) { - measured_state_values_[i] = state_interfaces_[i].get_optional().value(); + measured_state_values_[i] = state_interfaces_[i].get_value(); } } @@ -618,7 +618,7 @@ controller_interface::return_type PidController::update_and_write_commands( state_publisher_->msg_.dof_states[i].time_step = period.seconds(); // Command can store the old calculated values. This should be obvious because at least one // another value is NaN. - state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_optional().value(); + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 114d664e9d..dfb6f83874 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -181,15 +181,15 @@ TEST_F(PidControllerTest, reactivate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -272,8 +272,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = - std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } @@ -332,8 +331,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 const double expected_command_value = 30102.301020; - double actual_value = - std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } @@ -390,7 +388,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 const double expected_command_value = 1173.978; - EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -417,8 +415,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // check the result of the commands - the values are not wrapped const double expected_command_value = 2679.078; - EXPECT_NEAR( - controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } /** @@ -449,8 +446,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // Check the command value const double expected_command_value = 787.713559; - EXPECT_NEAR( - controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -587,7 +583,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_optional().value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -645,7 +641,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_optional().value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -681,8 +677,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = - std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -695,7 +690,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 0.0, 1e-5); } @@ -732,8 +727,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 const double expected_command_value = 30102.30102; - double actual_value = - std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -746,7 +740,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index f5b1442a57..e006986473 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -96,8 +96,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i // check the commands const double joint1_expected_cmd = 8.915; const double joint2_expected_cmd = 9.915; - ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd); - ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); } int main(int argc, char ** argv)