From 5d8a4bde719f31a04a2fed550292fcfb8499e31f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Jun 2025 23:06:06 +0200 Subject: [PATCH 1/2] Add new AntiWindup parameters to JTC (#1759) Co-authored-by: Sai Kishor Kothakota (cherry picked from commit a4cc58d61eb74f5120c2ef848f226e9470db9b98) --- doc/release_notes.rst | 6 ++- .../src/joint_trajectory_controller.cpp | 11 ++++- ...oint_trajectory_controller_parameters.yaml | 47 +++++++++++++++++++ 3 files changed, 61 insertions(+), 3 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index fe2f7e5d5b..12151d7e37 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -83,4 +83,8 @@ gpio_controllers force_torque_sensor_broadcaster ******************************* -* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__. +* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__). + +joint_trajectory_controller +******************************* +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 403df0563c..dfcbc02d76 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1673,16 +1673,23 @@ void JointTrajectoryController::update_pids() for (size_t i = 0; i < num_cmd_joints_; ++i) { 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.error_deadband = gains.error_deadband; + antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; if (pids_[i]) { // update PIDs with gains from ROS parameters - pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + pids_[i]->set_gains( + gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); } else { // Init PIDs with gains from ROS parameters pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); } ff_velocity_scale_[i] = gains.ff_velocity_scale; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 926366cde6..44748368ef 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -140,6 +140,53 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling :math:`k_{ff}` of velocity" } + u_clamp_max: { + type: double, + default_value: .inf, + description: "Upper output clamp." + } + u_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower output clamp." + } + 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", + read_only: true, + description: "Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', 'legacy' 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" + ]] + } + } + tracking_time_constant: { + type: double, + default_value: 0.0, + description: "Specifies the tracking time constant for the 'back_calculation' strategy. If + set to 0.0 when this strategy is selected, a recommended default value will be applied." + } + error_deadband: { + type: double, + default_value: 0.0, + description: "Is used to stop integration when the error is within the given range." + } constraints: stopped_velocity_tolerance: { type: double, From 25a99e73ad9f51ef44bda67613d89d50021a1a4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Jun 2025 23:08:25 +0200 Subject: [PATCH 2/2] Update release_notes.rst --- doc/release_notes.rst | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 12151d7e37..a199c8b8af 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -54,6 +54,8 @@ joint_trajectory_controller * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). * Feed-forward effort trajectories are supported now (`#1200 `_). * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). + mecanum_drive_controller ************************ @@ -84,7 +86,3 @@ gpio_controllers force_torque_sensor_broadcaster ******************************* * Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__). - -joint_trajectory_controller -******************************* -* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__).