diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1d3e666192..b368ce0c51 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1890,8 +1890,8 @@ void JointTrajectoryController::update_pids() const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i])); control_toolbox::AntiWindupStrategy antiwindup_strat; antiwindup_strat.set_type(gains.antiwindup_strategy); - antiwindup_strat.i_max = gains.i_clamp; - antiwindup_strat.i_min = -gains.i_clamp; + antiwindup_strat.i_max = gains.i_clamp_max; + antiwindup_strat.i_min = gains.i_clamp_min; antiwindup_strat.error_deadband = gains.error_deadband; antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; if (pids_[i]) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 837da37865..5e7947d15d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -150,11 +150,6 @@ joint_trajectory_controller: default_value: 0.0, description: "Derivative gain :math:`k_d` for PID" } - i_clamp: { - type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." - } ff_velocity_scale: { type: double, default_value: 0.0, @@ -172,26 +167,24 @@ joint_trajectory_controller: } i_clamp_max: { type: double, - default_value: 0.0, - description: "[Deprecated, see antiwindup_strategy] Upper integral clamp." + default_value: .inf, + description: "Upper integral clamp." } i_clamp_min: { type: double, - default_value: 0.0, - description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." + default_value: -.inf, + description: "Lower integral clamp." } antiwindup_strategy: { type: string, - default_value: "legacy", - read_only: true, + 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/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0fe3ed69f9..3b25c99961 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -304,9 +304,8 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); - const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + node->set_parameters({k_p, k_i, k_d, ff_velocity_scale}); } } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 947bb7e9df..6e77f23867 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -70,35 +70,26 @@ 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." + default_value: .inf, + description: "Upper integral clamp." } i_clamp_min: { type: double, - default_value: 0.0, - description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." + default_value: -.inf, + description: "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/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 7ba51dbf2a..d1e9754830 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);