diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a1c20a7c9b..7daee51926 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -103,6 +103,7 @@ pid_controller * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. * A new ``error_deadband`` parameter stops integration when the error is within a specified range. * PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 `_). +* 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 (`#2205 `_). steering_controllers_library ******************************** diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 38601c26d6..2de3dd0529 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -442,11 +442,49 @@ controller_interface::CallbackReturn PidController::on_activate( measured_state_.try_set(current_state_); } - reference_interfaces_.assign( - reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); measured_state_values_.assign( measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + // Initialize reference interfaces from current state so initial reference equals current state + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::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_) { diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 46d7d3748a..1987631488 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/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, diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 87b0cb3176..ee07d9f9c5 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/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} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index bd3fefcb60..5fede3d3c1 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -143,10 +143,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]); } } @@ -176,16 +178,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); @@ -287,7 +291,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(std::isfinite(interface)); } controller_->set_reference(dof_command_values_); @@ -296,7 +300,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) { 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( @@ -344,7 +348,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(std::isfinite(interface)); } controller_->set_reference(dof_command_values_); @@ -357,7 +361,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward { 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( @@ -530,7 +534,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]); } } @@ -555,13 +559,13 @@ 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(); @@ -569,7 +573,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) 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( @@ -810,6 +814,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); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 7ce5ddc4e6..3d48249c13 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -67,6 +67,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(