You're reading the documentation for a version of ROS 2 that has reached its EOL (end-of-life), and is no longer officially supported. If you want up-to-date information, please have a look at Kilted.

control_toolbox

This package contains several C++ classes and filter plugins useful in writing controllers.

Base classes

PID

PID Controller

The PID (Proportional-Integral-Derivative) controller is a widely used feedback controller. 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. Integral retention on reset is supported, which prevents re-winding the integrator after temporary disabling in presence of constant disturbances.

PID Equation

The standard PID equation is given by:

command = p_term + i_term + d_term

where:

• p_term = p_gain * error

• i_term = i_term + i_gain * error * dt

• d_term = d_gain * d_error

and:

• error = desired_state - measured_state

• d_error = (error - error_last) / 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.

• tracking_time_constant (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 tracking_time_constant. Faster correction for smaller tracking_time_constant.

  • 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

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 parameter is used to tune the speed of this adjustment. A smaller value results in faster anti-windup action.

  The update rule for the integral term with back-calculation is:

  i_term += dt * (i_gain * error + (1 / trk_tc) * (command_sat - command))

  If trk_tc, i.e., tracking_time_constant parameter, is set to 0.0, a default value is calculated based on the proportional and derivative gains:

  • If d_gain is not zero: trk_tc = √(d_gain / i_gain)

  • If d_gain is zero: trk_tc = p_gain / i_gain

• 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 - command_sat = 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:

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;
}

References

1. Visioli, A. Practical PID Control. London: Springer-Verlag London Limited, 2006. 476 p.

2. Vrancic, D., Horowitz, R., & Hagiwara, T. “Antiwindup, Bumpless, and Conditioned Transfer Techniques for PID Controllers.” IEEE Control Systems Magazine, vol. 16, no. 4, 1996, pp. 48–57.

3. Bohn, C.; Atherton, D. “An analysis package comparing PID anti-windup strategies.” IEEE Control Systems Magazine, 1995, pp. 34–40.

4. Åström, K.; Hägglund, T. PID Controllers: Theory, Design and Tuning. Research Triangle Park, USA: ISA Press / Springer-Verlag London Limited, 1995. 343 p.

Control filters

Implement filter plugins for control purposes as ros/filters

Available filters

• Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench).

• Low Pass: implements a low-pass filter based on a time-invariant Infinite Impulse Response (IIR) filter, for different data types (doubles or wrench).

• Exponential Filter: Exponential filter for double data type.

Gravity compensation filter

This filter implements an algorithm compensating for the gravity forces acting at the center of gravity (CoG) of a known mass, computed at a sensor_frame and applied to a data_in wrench.

The filter relies on tf2, and might fail if transforms are missing.

Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given.

GC: Required parameters

• world_frame (ℛ_w): frame in which the CoG.force is represented.

• sensor_frame (ℛ_s): frame in which the CoG.pos is defined

• CoG.pos (p_s): position of the CoG of the mass the filter should compensate for

• CoG.force (g_w): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the world_frame

GC: Algorithm

Given

• above-required parameters, ℛ_w, ℛ_s, p_s, g_w

• data_in, a wrench ℱ_i = {f_i, τ_i} represented in the data_in frame ℛ_i

• access to tf2 homogeneous transforms:

  • T_si from ℛ_i to ℛ_s

  • T_sw from ℛ_w to ℛ_s

  • T_os from ℛ_s to ℛ_o

Compute data_out compensated wrench ℱc_o = {fc_o, τc_o} represented in the data_out frame ℛ_o if given, or the data_in frame ℛ_i otherwise, with equations:

ℱc_o = T_os.ℱc_s,

with ℱc_s = {fc_s, τc_s} the compensated wrench in sensor_frame (common frame for computation)

and,

fc_s = f_s - T_swg_w

its force and,

τc_s = τ_s - p_s x (T_swg_w)

its torque, and

ℱ_s = T_si.ℱ_i = {f_s, τ_s}

the full transform of the input wrench ℱ_i to sensor frame ℛ_s

Remarks :

• a full vector is used for gravity force, to not impose gravity to be only along z of world_frame.

• data_in frame is usually equal to sensor_frame, but could be different since measurement of wrench might occur in another frame. E.g.: measurements are at the FT sensor flange = data_in frame, but CoG is given in FT sensor base = sensor_frame (=frame to which it is mounted on the robot), introducing an offset (thickness of the sensor) to be accounted for.

• data_out frame is usually data_in frame, but for convenience, can be set to any other useful frame. E.g.: wrench expressed in a control_frame like the center of a gripper.

• T_sw will only rotate the g_w vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛ_w to ℛ_s).

Low Pass filter

This filter implements a low-pass filter in the form of an IIR filter, applied to a data_in (double or wrench). The feedforward and feedback coefficients of the IIR filter are computed from the low-pass filter parameters.

LPF: Required parameters

• sampling frequency as sf

• damping frequency as df

• damping intensity as di

LPF: Algorithm

Given

• above-required parameters, sf, df, di

• data_in, a double or wrench x

Compute data_out, the filtered output y(n) with equation:

y(n) = b x(n-1) + a y(n-1)

with

• a the feedbackward coefficient such that a = exp( -1/sf (2 pi df) / (10^(di/-10)) )

• b the feedforward coefficient such that b = 1 - a

Exponential filter

EF: Required parameters

• alpha: the exponential decay factor

EF: Algorithm

smoothed_value = alpha * current_value + (1 - alpha) * last_smoothed_value;