From aeafdcfb781f304c18fec12079e03bb82616e62c Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Sat, 7 Jun 2025 16:28:49 +0000 Subject: [PATCH 1/4] Refresh migration notes post-PR #298 merge Clarify and expand migration instructions now that pull request #298 has been merged, ensuring users know how to adjust their configurations accordingly. --- doc/migration.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/migration.rst b/doc/migration.rst index 6e281f80..c10b64f3 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -3,3 +3,7 @@ Migration Guides: Jazzy to Kilted ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary. + +Pid/PidRos +*********************************************************** +* The parameter :paramref:`antiwindup` has been removed. The anti-windup behavior is now configured via the :paramref:`AntiwindupStrategy` enum. (`#298 `_). From 847f9eb57c767bfe898f95aeff967d5c32441be8 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Fri, 13 Jun 2025 13:34:43 +0000 Subject: [PATCH 2/4] Document PID controller in control_toolbox.md Adds a comprehensive PID section to control_toolbox.md, covering the standard PID equation, descriptions of proportional, integral, and derivative gains, output limits, detailed explanations of anti-windup strategies (back-calculation and conditional integration) with update rules and default values, and a C++ usage example. --- doc/control_toolbox.md | 70 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 1 deletion(-) diff --git a/doc/control_toolbox.md b/doc/control_toolbox.md index 72c774ec..00540053 100644 --- a/doc/control_toolbox.md +++ b/doc/control_toolbox.md @@ -1,3 +1,71 @@ # Base classes -Tbd. + +## PID + + +## PID Controller + +The PID (Proportional-Integral-Derivative) controller is a widely used feedback control loop mechanism that calculates an "error" value as the difference between a measured process variable and a desired setpoint. The controller attempts to minimize the error by adjusting the process control inputs. This class implements a generic structure that can be used to create a wide range of PID controllers. It can function independently or be subclassed to provide more specific controls based on a particular control loop. + +### PID Equation + +The standard PID equation is given by: + +command = pterm + iterm + dterm + +where: +* pterm = pgain * error +* iterm = iterm + igain * error * dt +* dterm = dgain * derror + +and: +* error = desired_state - actual_state +* derror = (error - errorlast) / dt + +### Parameters + +* `p` (Proportional gain): This gain determines the reaction to the current error. A larger proportional gain results in a larger change in the controller output for a given change in the error. +* `i` (Integral gain): This gain determines the reaction based on the sum of recent errors. The integral term accounts for past values of the error and integrates them over time to produce the `i_term`. This helps in eliminating steady-state errors. +* `d` (Derivative gain): This gain determines the reaction based on the rate at which the error has been changing. The derivative term predicts future errors based on the rate of change of the current error. This helps in reducing overshoot, settling time, and other transient performance variables. +* `u_clamp` (Minimum and maximum bounds for the controller output): These bounds are applied to the final command output of the controller, ensuring the output stays within acceptable physical limits. +* `trk_tc` (Tracking time constant): This parameter is specific to the 'back_calculation' anti-windup strategy. If set to 0.0 when this strategy is selected, a recommended default value will be applied. + +### Anti-Windup Strategies + +Anti-windup functionality is crucial for PID controllers, especially when the control output is subject to saturation (clamping). Without anti-windup, the integral term can accumulate excessively when the controller output is saturated, leading to large overshoots and sluggish response once the error changes direction. The `control_toolbox::Pid` class offers two anti-windup strategies: + +* **`BACK_CALCULATION`**: This strategy adjusts the integral term based on the difference between the saturated and unsaturated controller output. When the controller output `command` exceeds the output limits (`u_max` or `u_min`), the integral term `i_term` is adjusted by subtracting a value proportional to the difference between the saturated output `command_sat` and the unsaturated output `command`. This prevents the integral term from accumulating beyond what is necessary to maintain the output at its saturation limit. The `tracking_time_constant` (`trk_tc`) parameter is used to tune the speed of this adjustment. A smaller `trk_tc` results in faster anti-windup action. + + The update rule for the integral term with back-calculation is: + + iterm += dt * (igain * error + (1 / trktc) * (commandsat - command)) + + If `trk_tc` is set to 0.0, a default value is calculated based on the proportional and derivative gains: + * If `d_gain` is not zero: trktc = √(dgain / igain) + * If `d_gain` is zero: trktc = pgain / igain + +* **`CONDITIONAL_INTEGRATION`**: In this strategy, the integral term is only updated when the controller is not in saturation or when the error has a sign that would lead the controller out of saturation. Specifically, the integral term is frozen (not updated) if the controller output is saturated and the error has the same sign as the saturated output. This prevents further accumulation of the integral term in the direction of saturation. + + The integral term is updated only if the following condition is met: + + (command - commandsat = 0) ∨ (error * command ≤ 0) + + This means the integral term `i_term` is updated as `i_term += dt * i_gain * error` only when the controller is not saturated, or when it is saturated but the error is driving the output away from the saturation limit. + +### Usage Example + +To use the `Pid` class, you should first call some version of `initialize()` and then call `compute_command()` at every update step. For example: + +```cpp +control_toolbox::Pid pid; +pid.initialize(6.0, 1.0, 2.0, 5, -5,2,control_toolbox::AntiwindupStrategy::BACK_CALCULATION); +double position_desired = 0.5; +... +rclcpp::Time last_time = get_clock()->now(); +while (true) { + rclcpp::Time time = get_clock()->now(); + double effort = pid.compute_command(position_desired - currentPosition(), time - last_time); + last_time = time; +} +``` From f6e6c48a8b5472ff7247416e233303f00d1f6858 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Fri, 13 Jun 2025 13:39:55 +0000 Subject: [PATCH 3/4] Refresh migration notes post-PR #298 merge Clarify and expand migration instructions now that pull request #298 has been merged, ensuring users know how to adjust their configurations accordingly. --- doc/migration.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index c10b64f3..36f300cb 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -6,4 +6,4 @@ This list summarizes important changes between Jazzy (previous) and Kilted (curr Pid/PidRos *********************************************************** -* The parameter :paramref:`antiwindup` has been removed. The anti-windup behavior is now configured via the :paramref:`AntiwindupStrategy` enum. (`#298 `_). +* The parameters :paramref:`antiwindup`, :paramref:`i_clamp_max`, and :paramref:`i_clamp_min` have been removed. The anti-windup behavior is now configured via the :paramref:`AntiwindupStrategy` enum. (`#298 `_). From 2b290b0ab3d36eda33944c6708f2b11965a7ce14 Mon Sep 17 00:00:00 2001 From: ViktorCVS Date: Fri, 13 Jun 2025 13:55:56 +0000 Subject: [PATCH 4/4] Document PID controller in control_toolbox.md Adds a comprehensive PID section to control_toolbox.md, covering the standard PID equation, descriptions of proportional, integral, and derivative gains, output limits, detailed explanations of anti-windup strategies (back-calculation and conditional integration) with update rules and default values, and a C++ usage example. --- doc/control_toolbox.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/control_toolbox.md b/doc/control_toolbox.md index 00540053..75a367b9 100644 --- a/doc/control_toolbox.md +++ b/doc/control_toolbox.md @@ -30,6 +30,10 @@ and: * `d` (Derivative gain): This gain determines the reaction based on the rate at which the error has been changing. The derivative term predicts future errors based on the rate of change of the current error. This helps in reducing overshoot, settling time, and other transient performance variables. * `u_clamp` (Minimum and maximum bounds for the controller output): These bounds are applied to the final command output of the controller, ensuring the output stays within acceptable physical limits. * `trk_tc` (Tracking time constant): This parameter is specific to the 'back_calculation' anti-windup strategy. If set to 0.0 when this strategy is selected, a recommended default value will be applied. +* `antiwindup_strat` (Anti-windup strategy): This parameter selects how the integrator is prevented from winding up when the controller output saturates. Available options are: + * `NONE`: no anti-windup technique; the integral term accumulates without correction. + * `BACK_CALCULATION`: adjusts the integral term based on the difference between the unsaturated and saturated outputs using the tracking time constant `trk_tc`. Faster correction for smaller `trk_tc`. + * `CONDITIONAL_INTEGRATION`: only updates the integral term when the controller is not in saturation or when the error drives the output away from saturation, freezing integration otherwise. ### Anti-Windup Strategies