diff --git a/doc/migration.rst b/doc/migration.rst index c660da3c5d..f264e650ef 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -15,3 +15,5 @@ diff_drive_controller pid_controller ***************************** * 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. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8812c5377b..8e5b2d3ea3 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -11,3 +11,10 @@ force_torque_sensor_broadcaster 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. diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 02448f2a19..7ea8ec25b0 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -60,10 +60,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." @@ -71,12 +81,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,