From f4dff7e4c09ba93eda9c5754961ff21e00834b4d Mon Sep 17 00:00:00 2001 From: Victor Coutinho Vieira Santos <69547580+ViktorCVS@users.noreply.github.com> Date: Thu, 19 Jun 2025 02:25:42 -0300 Subject: [PATCH 1/4] Add new members for PID controller parameters (#1585) (cherry picked from commit aebe6b14ec7790e09061b14831008c87b85cea90) # Conflicts: # doc/migration.rst # doc/release_notes.rst --- doc/migration.rst | 6 ++++ doc/release_notes.rst | 14 +++++++++ pid_controller/src/pid_controller.yaml | 42 ++++++++++++++++++++++++-- 3 files changed, 59 insertions(+), 3 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index ee553e9d26..5a218c6b5f 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -12,6 +12,7 @@ diff_drive_controller joint_trajectory_controller ***************************** +<<<<<<< HEAD * Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). * The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). @@ -30,3 +31,8 @@ steering_controllers_library * *bicycle_steering_controller*: Set ``traction_wheel_radius`` instead of ``front_wheel_radius``, ``rear_wheel_radius``. * *tricycle_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``. * *ackermann_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``, and ``traction_track_width`` (and optionally ``steering_track_width``, if it differs) instead of ``rear_wheel_track``, ``front_wheel_track``. +======= +* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). +* 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 (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. +>>>>>>> aebe6b1 (Add new members for PID controller parameters (#1585)) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a199c8b8af..37a6edba3a 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -86,3 +86,17 @@ 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 `__). +<<<<<<< HEAD +======= + +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 `__). + +pid_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 (`#1585 `__). + * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. + * 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. +>>>>>>> aebe6b1 (Add new members for PID controller parameters (#1585)) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 8903085667..459c935d08 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -65,10 +65,20 @@ pid_controller: default_value: 0.0, description: "Derivative gain for PID" } + u_clamp_max: { + type: double, + default_value: .inf, + description: "Upper output clamp." + } + u_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower output clamp." + } antiwindup: { type: bool, default_value: false, - description: "Antiwindup functionality. When set to true, limits + 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." @@ -76,12 +86,38 @@ pid_controller: i_clamp_max: { type: double, default_value: 0.0, - description: "Upper integral clamp." + description: "[Deprecated, see antiwindup_strategy] Upper integral clamp." } i_clamp_min: { type: double, default_value: 0.0, - description: "Lower integral clamp." + description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." + } + antiwindup_strategy: { + type: string, + default_value: "legacy", + 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: 1.e-16, + description: "Is used to stop integration when the error is within the given range." } feedforward_gain: { type: double, From 01eb5170eb580d4422ffd731e8fa20c702a80759 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 19 Jun 2025 08:26:40 +0200 Subject: [PATCH 2/4] Update migration.rst --- doc/migration.rst | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index 5a218c6b5f..4755a82bc1 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -12,8 +12,6 @@ diff_drive_controller joint_trajectory_controller ***************************** -<<<<<<< HEAD - * Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). * The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). * Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). @@ -22,6 +20,9 @@ joint_trajectory_controller * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). +* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). +* 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 (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. steering_controllers_library ******************************** @@ -31,8 +32,3 @@ steering_controllers_library * *bicycle_steering_controller*: Set ``traction_wheel_radius`` instead of ``front_wheel_radius``, ``rear_wheel_radius``. * *tricycle_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``. * *ackermann_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``, and ``traction_track_width`` (and optionally ``steering_track_width``, if it differs) instead of ``rear_wheel_track``, ``front_wheel_track``. -======= -* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). -* 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 (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. ->>>>>>> aebe6b1 (Add new members for PID controller parameters (#1585)) From 428acf75cdf42c2052d31581af8860376fdb84ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 19 Jun 2025 08:27:30 +0200 Subject: [PATCH 3/4] Update release_notes.rst --- doc/release_notes.rst | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 37a6edba3a..21dddda580 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -65,6 +65,10 @@ pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). * Add ``save_i_term`` parameter to control retention of integral state after re-activation (`#1507 `_). +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior (`#1585 `__). + * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. + * 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. steering_controllers_library ******************************** @@ -86,17 +90,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 `__). -<<<<<<< HEAD -======= - -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 `__). - -pid_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 (`#1585 `__). - * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. - * 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. ->>>>>>> aebe6b1 (Add new members for PID controller parameters (#1585)) From 2b0aed1a04151c1cbd763d1651e58588048cb482 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 19 Jun 2025 08:31:01 +0200 Subject: [PATCH 4/4] Update migration.rst --- doc/migration.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index 4755a82bc1..2a7e197711 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -20,7 +20,10 @@ joint_trajectory_controller * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). -* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). + +pid_controller +************************ +* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are deprecated. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). * 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 (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly.