Skip to content

[Pid] Save i_term instead of error integral (backport #294) #389

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

Merged
merged 2 commits into from
Jun 7, 2025
Merged

[Pid] Save i_term instead of error integral (backport #294) #389

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aac226e
[Pid] Save `i_term` instead of error integral (#294)
christophfroehlich Mar 12, 2025
c953258
Fix merge conflicts
christophfroehlich Jun 7, 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
4 changes: 2 additions & 2 deletions control_toolbox/include/control_toolbox/pid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -518,7 +518,7 @@ class Pid
/*!
* \brief Return PID error terms for the controller.
* \param pe The proportional error.
* \param ie The integral error.
* \param ie The weighted integral error.
* \param de The derivative error.
*/
void get_current_pid_errors(double & pe, double & ie, double & de);
Expand Down Expand Up @@ -559,8 +559,8 @@ class Pid

double p_error_last_; /** Save state for derivative state calculation. */
double p_error_; /** Error. */
double i_error_; /** Integral of error. */
double d_error_; /** Derivative of error. */
double i_term_; /** Integrator state. */
double cmd_; /** Command to send. */
// TODO(christophfroehlich) remove this -> breaks ABI
[[deprecated("Use d_error_")]] double error_dot_; /** Derivative error */
Expand Down
2 changes: 1 addition & 1 deletion control_toolbox/include/control_toolbox/pid_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -327,7 +327,7 @@ class PidROS
/*!
* \brief Return PID error terms for the controller.
* \param pe[out] The proportional error.
* \param ie[out] The integral error.
* \param ie[out] The weighted integral error.
* \param de[out] The derivative error.
*/
void get_current_pid_errors(double & pe, double & ie, double & de);
Expand Down
30 changes: 11 additions & 19 deletions control_toolbox/src/pid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ void Pid::reset(bool save_i_term)
}
}

void Pid::clear_saved_iterm() { i_error_ = 0.0; }
void Pid::clear_saved_iterm() { i_term_ = 0.0; }

void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min)
{
Expand Down Expand Up @@ -202,7 +202,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term, i_term;
double p_term, d_term;
p_error_ = error; // this is error = target - state
d_error_ = error_dot;

Expand All @@ -219,31 +219,23 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

// Calculate the integral of the position error
i_error_ += dt_s * p_error_;

if (gains.antiwindup_ && gains.i_gain_ != 0)
// Calculate integral contribution to command
if (gains.antiwindup_)
{
// Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
std::pair<double, double> bounds =
std::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
i_error_ = std::clamp(i_error_, bounds.first, bounds.second);
// Prevent i_term_ from climbing higher than permitted by i_max_/i_min_
i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_);
}

// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;

if (!gains.antiwindup_)
else
{
// Limit i_term so that the limit is meaningful in the output
i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
i_term_ += gains.i_gain_ * dt_s * p_error_;
}

// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;

// Compute the command
cmd_ = p_term + i_term + d_term;
// Limit i_term so that the limit is meaningful in the output
cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term;

return cmd_;
}
Expand All @@ -255,7 +247,7 @@ double Pid::get_current_cmd() { return cmd_; }
void Pid::get_current_pid_errors(double & pe, double & ie, double & de)
{
pe = p_error_;
ie = i_error_;
ie = i_term_;
de = d_error_;
}

Expand Down
12 changes: 6 additions & 6 deletions control_toolbox/src/pid_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -297,8 +297,8 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
{
Pid::Gains gains = pid_.get_gains();

double p_error, i_error, d_error;
get_current_pid_errors(p_error, i_error, d_error);
double p_error, i_term, d_error;
get_current_pid_errors(p_error, i_term, d_error);

// Publish controller state if configured
if (rt_state_pub_)
Expand All @@ -310,7 +310,7 @@ void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
rt_state_pub_->msg_.error = error;
rt_state_pub_->msg_.error_dot = d_error;
rt_state_pub_->msg_.p_error = p_error;
rt_state_pub_->msg_.i_error = i_error;
rt_state_pub_->msg_.i_error = i_term;
rt_state_pub_->msg_.d_error = d_error;
rt_state_pub_->msg_.p_term = gains.p_gain_;
rt_state_pub_->msg_.i_term = gains.i_gain_;
Expand Down Expand Up @@ -340,8 +340,8 @@ void PidROS::print_values()
{
Pid::Gains gains = pid_.get_gains();

double p_error, i_error, d_error;
get_current_pid_errors(p_error, i_error, d_error);
double p_error, i_term, d_error;
get_current_pid_errors(p_error, i_term, d_error);

RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n"
<< " P Gain: " << gains.p_gain_ << "\n"
Expand All @@ -352,7 +352,7 @@ void PidROS::print_values()
<< " Antiwindup: " << gains.antiwindup_
<< "\n"
<< " P_Error: " << p_error << "\n"
<< " I_Error: " << i_error << "\n"
<< " i_term: " << i_term << "\n"
<< " D_Error: " << d_error << "\n"
<< " Command: " << get_current_cmd(););
}
Expand Down
17 changes: 9 additions & 8 deletions control_toolbox/test/pid_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,7 @@ TEST(ParameterTest, integrationClampZeroGainTest)
EXPECT_EQ(0.0, cmd);
}

constexpr double EPS = 1e-9;
TEST(ParameterTest, integrationAntiwindupTest)
{
RecordProperty(
Expand All @@ -131,16 +132,16 @@ TEST(ParameterTest, integrationAntiwindupTest)
double cmd = 0.0;

cmd = pid.compute_command(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);
EXPECT_NEAR(-1.0, cmd, EPS);

cmd = pid.compute_command(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);
EXPECT_NEAR(-1.0, cmd, EPS);

cmd = pid.compute_command(0.5, 1.0);
EXPECT_EQ(0.0, cmd);
EXPECT_NEAR(0.0, cmd, EPS);

cmd = pid.compute_command(-1.0, 1.0);
EXPECT_EQ(-1.0, cmd);
EXPECT_NEAR(-1.0, cmd, EPS);
}

TEST(ParameterTest, negativeIntegrationAntiwindupTest)
Expand All @@ -157,16 +158,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest)
double cmd = 0.0;

cmd = pid.compute_command(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);
EXPECT_NEAR(-0.2, cmd, EPS);

cmd = pid.compute_command(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);
EXPECT_NEAR(-0.2, cmd, EPS);

cmd = pid.compute_command(-0.05, 1.0);
EXPECT_EQ(-0.075, cmd);
EXPECT_NEAR(-0.075, cmd, EPS);

cmd = pid.compute_command(0.1, 1.0);
EXPECT_EQ(-0.2, cmd);
EXPECT_NEAR(-0.2, cmd, EPS);
}

TEST(ParameterTest, gainSettingCopyPIDTest)
Expand Down
Loading