Skip to content

[Pid] Save i_term instead of error integral #294

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 3 commits into from
Mar 12, 2025
Merged

[Pid] Save i_term instead of error integral #294

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4832a88
Save i_term instead of error
christophfroehlich Mar 7, 2025
8176afc
Fix docstring
christophfroehlich Mar 7, 2025
4591257
Merge branch 'ros2-master' into fix/i_term
christophfroehlich Mar 11, 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 include/control_toolbox/pid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -400,7 +400,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 @@ -431,8 +431,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. */
};

Expand Down
2 changes: 1 addition & 1 deletion include/control_toolbox/pid_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,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 src/pid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ void Pid::reset(bool save_i_term)

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

void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min)
Expand Down Expand Up @@ -182,7 +182,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 @@ -194,29 +194,21 @@ 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) {
// 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);
}

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

if (!gains.antiwindup_) {
// Limit i_term so that the limit is meaningful in the output
i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
if (gains.antiwindup_) {
// 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_);
} else {
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;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure why i_term is being clamped again here when it was clamped above when antiwindup is true. Shouldn't it only be clamped when antiwindup is true

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I left that to keep the current behavior, it was clamped even with antiwindup being true.
from the docstring:

antiwindup Antiwindup 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.

this was already questioned in #276 and I didn't want to change this behavior in this PR.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok. I understand and it shouldn't be changed in this PR. The only minor comment I would make is if antiwindup is false, the iterm is not clamped but the use of it is clamped when calculating output. That is slightly different behavior than before where the iterm was always clamped. Practically, I don't think it makes a difference.

I definitely think the antiwindup behavior needs to be fixed and I guess it will be addressed in #276. With this PR and the earlier behavior, the antiwindup flag, in practical terms, doesn't change anything. Antiwindup is always being implemented regardless of the flag.


return cmd_;
}
Expand All @@ -228,7 +220,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 src/pid_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -273,8 +273,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 @@ -284,7 +284,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;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you said you will change the message in a different PR since this is really weighted i-error.

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 @@ -314,8 +314,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 @@ -326,7 +326,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 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