diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 947bb7e9df..5c508eb078 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -70,35 +70,16 @@ pid_controller: default_value: -.inf, description: "Lower output clamp." } - antiwindup: { - type: bool, - default_value: false, - description: "[Deprecated, see antiwindup_strategy] Anti-windup functionality. When set to true, limits - the integral error to prevent windup; otherwise, constrains the - integral contribution to the control output. i_clamp_max and - i_clamp_min are applied in both scenarios." - } - i_clamp_max: { - type: double, - default_value: 0.0, - description: "[Deprecated, see antiwindup_strategy] Upper integral clamp." - } - i_clamp_min: { - type: double, - default_value: 0.0, - description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." - } antiwindup_strategy: { type: string, - default_value: "legacy", + default_value: "none", description: "Specifies the anti-windup strategy. Options: 'back_calculation', - 'conditional_integration', 'legacy' or 'none'. Note that the 'back_calculation' strategy use the + 'conditional_integration' or 'none'. Note that the 'back_calculation' strategy use the tracking_time_constant parameter to tune the anti-windup behavior.", validation: { one_of<>: [[ "back_calculation", "conditional_integration", - "legacy", "none" ]] } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index f8b27f1d49..1c606008f5 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -2,8 +2,8 @@ ros__parameters: # TODO(christohfroehlich): Remove this global parameters once the deprecated antiwindup parameters are removed. gains: - joint1: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf} - joint2: {antiwindup_strategy: "none", i_clamp_max: .inf, i_clamp_min: -.inf} + joint1: {antiwindup_strategy: "none"} + joint2: {antiwindup_strategy: "none"} test_pid_controller: ros__parameters: @@ -15,7 +15,7 @@ test_pid_controller: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0} + joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0, antiwindup_strategy: "none"} test_pid_controller_unlimited: ros__parameters: @@ -27,7 +27,7 @@ test_pid_controller_unlimited: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0} + joint1: {p: 1.0, i: 2.0, d: 3.0, antiwindup_strategy: "none"} test_pid_controller_angle_wraparound_on: ros__parameters: @@ -39,7 +39,7 @@ test_pid_controller_angle_wraparound_on: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, angle_wraparound: true} + joint1: {p: 1.0, i: 2.0, d: 3.0, antiwindup_strategy: "none", angle_wraparound: true} test_pid_controller_with_feedforward_gain: ros__parameters: @@ -51,7 +51,7 @@ test_pid_controller_with_feedforward_gain: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 0.5, i: 0.0, d: 0.0, feedforward_gain: 1.0} + joint1: {p: 0.5, i: 0.0, d: 0.0, antiwindup_strategy: "none", feedforward_gain: 1.0} test_pid_controller_with_feedforward_gain_dual_interface: ros__parameters: @@ -64,8 +64,8 @@ test_pid_controller_with_feedforward_gain_dual_interface: reference_and_state_interfaces: ["position", "velocity"] 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} + joint1: {p: 0.5, i: 0.3, d: 0.4, antiwindup_strategy: "none", feedforward_gain: 1.0} + joint2: {p: 0.5, i: 0.3, d: 0.4, antiwindup_strategy: "none", feedforward_gain: 1.0} test_save_i_term_off: ros__parameters: @@ -77,7 +77,7 @@ test_save_i_term_off: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: false} + joint1: {p: 1.0, i: 2.0, d: 3.0, antiwindup_strategy: "none", save_i_term: false} test_save_i_term_on: ros__parameters: @@ -89,4 +89,4 @@ test_save_i_term_on: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true} + joint1: {p: 1.0, i: 2.0, d: 3.0, antiwindup_strategy: "none", save_i_term: true} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 7ba51dbf2a..69a1c3d85b 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -45,7 +45,6 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 3.0); - ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_max, 5.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_clamp_min, -5.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); @@ -238,10 +237,11 @@ TEST_F(PidControllerTest, test_update_logic_zero_feedforward_gain) // check the command value: // ref = 101.101, state = 1.1, ds = 0.01 // error = ref - state = 100.001, error_dot = error/ds = 10000.1, - // p_term = 100.001 * 1, i_term = 0.0 at first update call, d_term = error/ds = 10000.1 * 3 + // p_term = 100.001 * 1, i_term = 100.001 * 0.01 * 2, d_term = error/ds = 10000.1 * 3 // feedforward ON, feedforward_gain = 0 - // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3 - const double expected_command_value = 30100.301000; + // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.30102 + 0 * 101.101 = + // 30102.30102 + const double expected_command_value = 30102.30102; double actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; @@ -294,11 +294,11 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // ref = 5.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 // error = ref - state = 5.0 - 1.1 = 3.9, error_dot = error/ds = 3.9/0.01 = 390.0, // p_term = error * p_gain = 3.9 * 1.0 = 3.9, - // i_term = zero at first update + // i_term = error * ds * i_gain = 3.9 * 0.01 * 2.0 = 0.078, // d_term = error_dot * d_gain = 390.0 * 3.0 = 1170.0 // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 { - const double expected_command_value = 1173.9; + const double expected_command_value = 1173.978; EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } } @@ -328,10 +328,10 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 // error = ref - state = 10.0 - 1.1 = 8.9, error_dot = error/ds = 8.9/0.01 = 890.0, // p_term = error * p_gain = 8.9 * 1.0 = 8.9, - // i_term = zero at first update + // i_term = error * ds * i_gain = 8.9 * 0.01 * 2.0 = 0.178, // d_term = error_dot * d_gain = 890.0 * 3.0 = 2670.0 - // feedforward OFF -> cmd = p_term + i_term + d_term = 8.9 + 0.0 + 2670.0 = 2678.9 - const double expected_command_value = 2678.9; + // feedforward OFF -> cmd = p_term + i_term + d_term = 8.9 + 0.178 + 2670.0 = 2679.078 + const double expected_command_value = 2679.078; EXPECT_NEAR( controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } @@ -364,10 +364,12 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) // Check the command value with wrapped error // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 // error = ref - state = wrap(10.0 - 1.1) = 8.9-2*pi = 2.616814, error_dot = error/ds - // = 2.6168/0.01 = 261.6814, p_term = error * p_gain = 2.6168 * 1.0 = 2.6168, i_term = zero at - // first update d_term = error_dot * d_gain = 261.6814 * 3.0 = 785.0444079 feedforward OFF -> cmd - // = p_term + i_term + d_term = 2.616814, + 0.0 + 785.0444079 = 787.6612219 - const double expected_command_value = 787.6612219; + // = 2.6168/0.01 = 261.6814, + // p_term = error * p_gain = 2.6168 * 1.0 = 2.6168, + // i_term = error * ds * i_gain = 2.6168 * 0.01 * 2.0 = 0.05233628, + // d_term = error_dot * d_gain = 261.6814 * 3.0 = 785.0444079 feedforward OFF -> + // cmd = p_term + i_term + d_term = 2.616814, + 0.05233628 + 785.0444079 = 787.71355818 + const double expected_command_value = 787.71355818; EXPECT_NEAR( controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } @@ -619,9 +621,9 @@ TEST_F(PidControllerTest, test_save_i_term_off) // check the command value // error = ref - state = 100.001, error_dot = error/ds = 10000.1, - // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3 - // feedforward OFF -> cmd = p_term + i_term + d_term = 30100.301 - const double expected_command_value = 30100.3010; + // p_term = 100.001 * 1, i_term = 100.001 * 0.01 * 2, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.30102 + const double expected_command_value = 30102.30102; double actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; @@ -669,9 +671,9 @@ TEST_F(PidControllerTest, test_save_i_term_on) // check the command value // error = ref - state = 100.001, error_dot = error/ds = 10000.1, - // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3 - // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.301 - const double expected_command_value = 30100.3010; + // p_term = 100.001 * 1, i_term = 100.001 * 0.01 * 2, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.30102 + const double expected_command_value = 30102.30102; double actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index 75eb531863..bfb7054c10 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -88,8 +88,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i controller_interface::return_type::OK); // check the commands - const double joint1_expected_cmd = 8.90; - const double joint2_expected_cmd = 9.90; + 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); }