Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add three new anti-windup techniques and a Saturation feature #298

Open
wants to merge 8 commits into
base: ros2-master
Choose a base branch
from
Open

Add three new anti-windup techniques and a Saturation feature #298

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
6fe1daa
Add three new anti-windup techniques
ViktorCVS Mar 11, 2025
8b03fdb
Add unit tests
ViktorCVS Mar 13, 2025
274eed8
Merge upstream and add local modifications
ViktorCVS Mar 13, 2025
083c11f
Add unit tests
ViktorCVS Mar 13, 2025
a3db0aa
Adjust unit tests for new parameters
ViktorCVS Mar 13, 2025
6a61e19
Removed access to deleted PidState fields in pub
ViktorCVS Mar 17, 2025
ba7fddb
Modify cmd eq. to keep the anti-windup behavior
ViktorCVS Mar 18, 2025
9021434
Removed access to deleted PidState fields in pub
ViktorCVS Mar 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
209 changes: 191 additions & 18 deletions include/control_toolbox/pid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#define CONTROL_TOOLBOX__PID_HPP_

#include <chrono>
#include <string>

#include "rclcpp/duration.hpp"
#include "realtime_tools/realtime_buffer.hpp"
Expand Down Expand Up @@ -82,7 +83,12 @@ namespace control_toolbox

\param i Integral gain

\param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$
\param i_clamp Minimum and maximum bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$

\param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$.

\param trk_tc Tracking time constant for the 'back_calculation' and 'conditioning_technique' strategies.


\section Usage

Expand Down Expand Up @@ -115,7 +121,7 @@ class Pid
struct Gains
{
/*!
* \brief Optional constructor for passing in values without antiwindup
* \brief Optional constructor for passing in values without antiwindup and saturation
*
* \param p The proportional gain.
* \param i The integral gain.
Expand All @@ -125,40 +131,82 @@ class Pid
*
*/
Gains(double p, double i, double d, double i_max, double i_min)
: p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true)
: p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0),
trk_tc_(0.0), saturation_(false), antiwindup_(true), antiwindup_strat_("none")
{
}

/*!
* \brief Optional constructor for passing in values
* \brief Optional constructor for passing in values without saturation
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param 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.
*
*/
Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
: p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup)
: p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(0.0), u_min_(0.0),
trk_tc_(0.0), saturation_(false), antiwindup_(antiwindup), antiwindup_strat_("none")
{
}

/*!
* \brief Optional constructor for passing in values
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation'
and 'conditioning_technique' strategies. If set to 0.0 when one of these
strategies is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param 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.
* \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation',
'conditioning_technique', 'conditional_integration', or 'none'. Note that
the 'back_calculation' and 'conditioning_technique' strategies 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.
*
*/
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, std::string antiwindup_strat)
: p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), u_max_(u_max),
u_min_(u_min), trk_tc_(trk_tc), saturation_(saturation), antiwindup_(antiwindup),
antiwindup_strat_(antiwindup_strat)
{
}

// Default constructor
Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false)
Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), u_max_(0.0),
u_min_(0.0), trk_tc_(0.0), saturation_(false), antiwindup_(false), antiwindup_strat_("none")
{
}

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. */
bool antiwindup_; /**< Antiwindup. */
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. */
std::string antiwindup_strat_; /**< Anti-windup strategy. */
};

/*!
Expand All @@ -171,7 +219,7 @@ class Pid
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param 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.
Expand All @@ -182,6 +230,39 @@ class 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);

/*!
* \brief Constructor, zeros out Pid values when created and
* initialize Pid-gains and integral term limits.
* Does not initialize dynamic reconfigure for PID gains
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation'
and 'conditioning_technique' strategies. If set to 0.0 when one of these
strategies is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param 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.
* \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation',
'conditioning_technique', 'conditional_integration', or 'none'. Note that
the 'back_calculation' and 'conditioning_technique' strategies 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 i_max, double i_min,
double u_max, double u_min, double trk_tc, bool saturation,
bool antiwindup, std::string antiwindup_strat);

/**
* \brief Copy constructor required for preventing mutexes from being copied
* \param source - Pid to copy
Expand All @@ -202,7 +283,7 @@ class Pid
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param 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.
Expand All @@ -212,6 +293,37 @@ class Pid
void initialize(
double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

/*!
* \brief Optional constructor for passing in values
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation'
and 'conditioning_technique' strategies. If set to 0.0 when one of these
strategies is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param 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.
* \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation',
'conditioning_technique', 'conditional_integration', or 'none'. Note that
the 'back_calculation' and 'conditioning_technique' strategies 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.
*
* \note New gains are not applied if i_min_ > i_max_ or u_min > u_max
*/
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, std::string antiwindup_strat);

/*!
* \brief Reset the state of this PID controller
* @note The integral term is not retained and it is reset to zero
Expand Down Expand Up @@ -247,14 +359,43 @@ class Pid
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param 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, bool & antiwindup);

/*!
* \brief Get PID gains for the controller.
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation'
and 'conditioning_technique' strategies. If set to 0.0 when one of these
strategies is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param 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.
* \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation',
'conditioning_technique', 'conditional_integration', or 'none'. Note that
the 'back_calculation' and 'conditioning_technique' strategies 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 & i_max, double & i_min, double & u_max,
double & u_min, double & trk_tc, bool & saturation, bool & antiwindup,
std::string & antiwindup_strat);

/*!
* \brief Get PID gains for the controller.
* \return gains A struct of the PID gain values
Expand All @@ -268,7 +409,7 @@ class Pid
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param antiwindup Antiwindup functionality. When set to true, limits
* \param 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.
Expand All @@ -277,6 +418,37 @@ class Pid
*/
void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

/*!
* \brief Optional constructor for passing in values
*
* \param p The proportional gain.
* \param i The integral gain.
* \param d The derivative gain.
* \param i_max Upper integral clamp.
* \param i_min Lower integral clamp.
* \param u_max Upper output clamp.
* \param u_min Lower output clamp.
* \param trk_tc Specifies the tracking time constant for the 'back_calculation'
and 'conditioning_technique' strategies. If set to 0.0 when one of these
strategies is selected, a recommended default value will be applied.
* \param saturation Enables output saturation. When true, the controller output is
clamped between u_max and u_min.
* \param 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.
* \param antiwindup_strat Choose the anti-windup strategy. Options: 'back_calculation',
'conditioning_technique', 'conditional_integration', or 'none'. Note that
the 'back_calculation' and 'conditioning_technique' strategies 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.
*
* \note New gains are not applied if i_min_ > i_max_ or u_min > u_max
*/
void set_gains(double p, double i, double d, double i_max, double i_min, double u_max,
double u_min, double trk_tc, bool saturation = false, bool antiwindup = false,
std::string antiwindup_strat = "none");

/*!
* \brief Set PID gains for the controller.
* \param gains A struct of the PID gain values
Expand Down Expand Up @@ -432,8 +604,9 @@ class Pid
double p_error_last_; /** Save state for derivative state calculation. */
double p_error_; /** Error. */
double d_error_; /** Derivative of error. */
double i_term_; /** Integrator state. */
double i_term_{0}; /** Integrator state. */
double cmd_; /** Command to send. */
double cmd_unsat_; /** command without saturation. */
};

} // namespace control_toolbox
Expand Down
Loading