Skip to content

Commit

Permalink
Improvement in integral contribution implementation. Resolve ros-cont…
Browse files Browse the repository at this point in the history 
…rols#32.
  • Loading branch information
@carlosjoserg
carlosjoserg committed Jan 23, 2015
1 parent 4719510 commit bc45970
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 41 deletions.
4 changes: 2 additions & 2 deletions include/control_toolbox/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -370,8 +370,8 @@ class Pid

double p_error_last_; /**< _Save position state for derivative state calculation. */
double p_error_; /**< Position error. */
double d_error_; /**< Derivative error. */
double i_term_; /**< Integral term. */
double i_error_; /**< Integral of position error. */
double d_error_; /**< Derivative of position error. */
double cmd_; /**< Command to send. */

// Dynamics reconfigure
Expand Down
74 changes: 47 additions & 27 deletions src/pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,8 +175,8 @@ void Pid::reset()
{
p_error_last_ = 0.0;
p_error_ = 0.0;
i_error_ = 0.0;
d_error_ = 0.0;
i_term_ = 0.0;
cmd_ = 0.0;
}

Expand Down Expand Up @@ -269,7 +269,7 @@ double Pid::computeCommand(double error, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

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

if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
Expand All @@ -278,11 +278,14 @@ double Pid::computeCommand(double error, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

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

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate the derivative error
if (dt.toSec() > 0.0)
Expand All @@ -292,7 +295,9 @@ double Pid::computeCommand(double error, ros::Duration dt)
}
// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = p_term + i_term_ + d_term;

// Compute the command
cmd_ = p_term + i_term + d_term;

return cmd_;
}
Expand All @@ -302,7 +307,7 @@ double Pid::updatePid(double error, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget

if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
Expand All @@ -311,11 +316,14 @@ double Pid::updatePid(double error, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

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

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate the derivative error
if (dt.toSec() > 0.0)
Expand All @@ -325,7 +333,9 @@ double Pid::updatePid(double error, ros::Duration dt)
}
// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = - p_term - i_term_ - d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -335,7 +345,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

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

Expand All @@ -346,15 +356,20 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

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

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = p_term + i_term_ + d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -364,7 +379,7 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt)
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();

double p_term, d_term;
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget
d_error_ = error_dot;

Expand All @@ -375,15 +390,20 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt)
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;

//Calculate integral contribution to command
i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;

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

// Limit i_term_ so that the limit is meaningful in the output
i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
cmd_ = - p_term - i_term_ - d_term;

// Compute the command
cmd_ = - p_term - i_term - d_term;

return cmd_;
}
Expand All @@ -404,7 +424,7 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
Gains gains = *gains_buffer_.readFromRT();

*pe = p_error_;
*ie = gains.i_gain_ ? i_term_/gains.i_gain_ : 0.0;
*ie = i_error_;
*de = d_error_;
}

Expand All @@ -420,8 +440,8 @@ void Pid::printValues()
<< " I_Min: " << gains.i_min_ << "\n"
<< " P_Error_Last: " << p_error_last_ << "\n"
<< " P_Error: " << p_error_ << "\n"
<< " I_Error: " << i_error_ << "\n"
<< " D_Error: " << d_error_ << "\n"
<< " I_Term: " << i_term_ << "\n"
<< " Command: " << cmd_
);

Expand Down
19 changes: 7 additions & 12 deletions test/pid_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,27 +32,23 @@ TEST(ParameterTest, zeroITermBadIBoundsTest)

TEST(ParameterTest, integrationWindupTest)
{
RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is non-zero.");
RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is non-zero.");

Pid pid(0.0, 1.0, 0.0, 1.0, -1.0);
Pid pid(0.0, 2.0, 0.0, 1.0, -1.0);

double cmd = 0.0;
double pe,ie,de;

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_EQ(-1.0, ie);
EXPECT_EQ(-1.0, cmd);

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_EQ(-1.0, ie);
EXPECT_EQ(-1.0, cmd);
}

TEST(ParameterTest, integrationWindupZeroGainTest)
{
RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is zero. If the integral error is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled.");
RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is zero. If the integral contribution is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled.");

double i_gain = 0.0;
double i_min = -1.0;
Expand All @@ -64,14 +60,13 @@ TEST(ParameterTest, integrationWindupZeroGainTest)

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_LE(i_min, ie);
EXPECT_LE(ie, i_max);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);

cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
pid.getCurrentPIDErrors(&pe,&ie,&de);
EXPECT_LE(i_min, ie);
EXPECT_LE(ie, i_max);
EXPECT_LE(i_min, cmd);
EXPECT_LE(cmd, i_max);
EXPECT_EQ(0.0, cmd);
}

Expand Down

0 comments on commit bc45970

Please sign in to comment.