Class Pid

Defined in File pid.hpp

Nested Relationships

Nested Types

Struct Pid::Gains

Class Documentation

class Pid

A basic pid class.

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.

This class also allows for retention of integral term on reset. This is useful for control loops that are enabled/disabled with a constant steady-state external disturbance. Once the integrator cancels out the external disturbance, disabling/resetting/ re-enabling closed-loop control does not require the integrator to wind up again.

In particular, this class implements the standard pid equation:

\(command = p_{term} + i_{term} + d_{term} \)

where:

\( p_{term} = p_{gain} * p_{error} \)
\( i_{term} = i_{term} + \int{i_{gain} * p_{error} * dt} \)
\( d_{term} = d_{gain} * d_{error} \)
\( d_{error} = (p_{error} - p_{error last}) / dt \)

given:

\( p_{error} = p_{desired} - p_{state} \).

Usage

To use the Pid class, you should first call some version of init() (in non-realtime) and then call updatePid() at every update step. For example:

control_toolbox::Pid pid;
pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3);
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;
}

Param p:: Proportional gain
Param d:: Derivative gain
Param i:: Integral gain
Param i_clamp:: Minimum and maximum bounds for the integral windup, the clamp is applied to the \(i_{term}\)
Param u_clamp:: Minimum and maximum bounds for the controller output. The clamp is applied to the \(command\).
Param trk_tc:: Tracking time constant for the ‘back_calculation’ strategy.

Public Functions

Pid(double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0, bool antiwindup = false)

Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.

Throws:

An – std::invalid_argument exception is thrown if i_min > i_max

Pid(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat)

Constructor, initialize Pid-gains and term limits.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than ‘none’ is selected, it will override the controller’s default anti-windup behavior.

Throws:

An – std::invalid_argument exception is thrown if i_min > i_max or u_min > u_max

Pid(double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, AntiwindupStrategy antiwindup_strat)

Constructor, initialize Pid-gains and term limits.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior.

Throws:

An – std::invalid_argument exception is thrown if u_min > u_max

Pid(const Pid &source)

Copy constructor required for preventing mutexes from being copied.

Parameters:: source – - Pid to copy

~Pid(): Destructor of Pid class.

void initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup = false)

Zeros out Pid values and initialize Pid-gains and term limits.

Note

New gains are not applied if i_min_ > i_max_

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.

void initialize(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat)

Initialize Pid-gains and term limits.

Note

New gains are not applied if i_min_ > i_max_ or u_min > u_max

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than ‘none’ is selected, it will override the controller’s default anti-windup behavior.

void initialize(double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, AntiwindupStrategy antiwindup_strat)

Initialize Pid-gains and term limits.

Note

New gains are not applied if i_min_ > i_max_ or u_min > u_max

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior.

void reset(): Reset the state of this PID controller.

Note

The integral term is not retained and it is reset to zero

void reset(bool save_i_term)

Reset the state of this PID controller.

Parameters:: save_i_term – boolean indicating if integral term is retained on reset()

void clear_saved_iterm(): Clear the saved integrator output of this controller.

void get_gains(double &p, double &i, double &d, double &i_max, double &i_min)

Get PID gains for the controller.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.

void get_gains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)

Get PID gains for the controller.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.

void get_gains(double &p, double &i, double &d, double &i_max, double &i_min, double &u_max, double &u_min, double &trk_tc, bool &saturation, bool &antiwindup, AntiwindupStrategy &antiwindup_strat)

Get PID gains for the controller.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than ‘none’ is selected, it will override the controller’s default anti-windup behavior.

void get_gains(double &p, double &i, double &d, double &u_max, double &u_min, double &trk_tc, bool &saturation, AntiwindupStrategy &antiwindup_strat)

Get PID gains for the controller (preferred).

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior.

Gains get_gains()

Get PID gains for the controller.

Returns:: gains A struct of the PID gain values

void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false)

Set PID gains for the controller.

Note

New gains are not applied if i_min > i_max

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.

void set_gains(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc = 0.0, bool saturation = false, bool antiwindup = false, AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE)

Set PID gains for the controller.

Note

New gains are not applied if i_min_ > i_max_ or u_min > u_max

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than ‘none’ is selected, it will override the controller’s default anti-windup behavior.

void set_gains(double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, AntiwindupStrategy antiwindup_strat)

Set PID gains for the controller.

Note

New gains are not applied if i_min_ > i_max_ or u_min > u_max

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior.

void set_gains(const Gains &gains)

Set PID gains for the controller.

Note

New gains are not applied if gains.i_min_ > gains.i_max_

Parameters:: gains – A struct of the PID gain values

double compute_command(double error, const double &dt_s)

Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt_s.

Parameters:

error – Error since last call (error = target - state)
dt_s – Change in time since last call in seconds

Returns:

PID command

double compute_command(double error, const rcl_duration_value_t &dt_ns)

Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt_ns.

Parameters:

error – Error since last call (error = target - state)
dt_ns – Change in time since last call, measured in nanoseconds.

Returns:

PID command

double compute_command(double error, const rclcpp::Duration &dt)

Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt.

Parameters:

error – Error since last call (error = target - state)
dt – Change in time since last call

Returns:

PID command

double compute_command(double error, const std::chrono::nanoseconds &dt_ns)

Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt_ns.

Parameters:

error – Error since last call (error = target - state)
dt_ns – Change in time since last call

Returns:

PID command

double compute_command(double error, double error_dot, const double &dt_s)

Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.

Parameters:

error – Error since last call (error = target - state)
error_dot – d(Error)/dt_s since last call
dt_s – Change in time since last call in seconds

Returns:

PID command

double compute_command(double error, double error_dot, const rcl_duration_value_t &dt_ns)

Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.

Parameters:

error – Error since last call (error = target - state)
error_dot – d(Error)/dt_ns since last call
dt_ns – Change in time since last call, measured in nanoseconds.

Returns:

PID command

double compute_command(double error, double error_dot, const rclcpp::Duration &dt)

Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.

Parameters:

error – Error since last call (error = target - state)
error_dot – d(Error)/dt since last call
dt – Change in time since last call

Returns:

PID command

double compute_command(double error, double error_dot, const std::chrono::nanoseconds &dt_ns)

Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.

Parameters:

error – Error since last call (error = target - state)
error_dot – d(Error)/(dt_ns/1e9) since last call
dt_ns – Change in time since last call, measured in nanoseconds.

Returns:

PID command

void set_current_cmd(double cmd): Set current command for this PID controller.

double get_current_cmd(): Return current command for this PID controller.

void get_current_pid_errors(double &pe, double &ie, double &de)

Return PID error terms for the controller.

Parameters:

pe – The proportional error.
ie – The weighted integral error.
de – The derivative error.

inline Pid &operator=(const Pid &source): Custom assignment operator Does not initialize dynamic reconfigure for PID gains.

Protected Attributes

realtime_tools::RealtimeBuffer<Gains> gains_buffer_

double p_error_last_ = 0

double p_error_ = 0: Save state for derivative state calculation.

double d_error_ = 0: Error.

double i_term_ = 0: Derivative of error.

double cmd_ = 0: Integrator state.

double cmd_unsat_ = 0: Command to send.

struct Gains

Store gains in a struct to allow easier realtime buffer usage.

Public Functions

inline Gains(double p, double i, double d, double i_max, double i_min)

Optional constructor for passing in values without antiwindup and saturation.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.

inline Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)

Optional constructor for passing in values without saturation.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.

inline Gains(double p, double i, double d, double i_max, double i_min, double u_max, double u_min, double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat)

Optional constructor for passing in values.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
i_max – Upper integral clamp.
i_min – Lower integral clamp.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup – Anti-windup functionality. When set to true, limits the integral error to prevent windup; otherwise, constrains the integral contribution to the control output. i_max and i_min are applied in both scenarios.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior. When a strategy other than ‘none’ is selected, it will override the controller’s default anti-windup behavior.

inline Gains(double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation, AntiwindupStrategy antiwindup_strat)

Constructor for passing in values.

Parameters:

p – The proportional gain.
i – The integral gain.
d – The derivative gain.
u_max – Upper output clamp.
u_min – Lower output clamp.
trk_tc – 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.
saturation – Enables output saturation. When true, the controller output is clamped between u_max and u_min.
antiwindup_strat – Specifies the anti-windup strategy. Options: ‘back_calculation’, ‘conditional_integration’, or ‘none’. Note that the ‘back_calculation’ strategy use the tracking_time_constant parameter to tune the anti-windup behavior.

inline Gains()

Public Members

double p_gain_: Proportional gain.

double i_gain_: Integral gain.

double d_gain_: Derivative gain.

double i_max_: Maximum allowable integral term.

double i_min_: Minimum allowable integral term.

double u_max_: Maximum allowable output.

double u_min_: Minimum allowable output.

double trk_tc_: Tracking time constant.

bool saturation_: Saturation.

bool antiwindup_: Anti-windup.

AntiwindupStrategy antiwindup_strat_: Anti-windup strategy.