00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <control_toolbox/pid.h>
00042 #include <tinyxml.h>
00043
00044 namespace control_toolbox {
00045
00046 static const std::string DEFAULT_NAMESPACE = "pid";
00047
00048 Pid::Pid(double p, double i, double d, double i_max, double i_min)
00049 : dynamic_reconfig_initialized_(false)
00050 {
00051 setGains(p,i,d,i_max,i_min);
00052
00053 reset();
00054 }
00055
00056 Pid::Pid(const Pid &source)
00057 : dynamic_reconfig_initialized_(false)
00058 {
00059
00060 gains_buffer_ = source.gains_buffer_;
00061
00062
00063 reset();
00064 }
00065
00066 Pid::~Pid()
00067 {
00068 }
00069
00070 void Pid::initPid(double p, double i, double d, double i_max, double i_min,
00071 const ros::NodeHandle &node)
00072 {
00073 initPid(p, i, d, i_max, i_min);
00074
00075
00076 ros::NodeHandle nh(DEFAULT_NAMESPACE);
00077 initDynamicReconfig(nh);
00078 }
00079
00080 void Pid::initPid(double p, double i, double d, double i_max, double i_min)
00081 {
00082 setGains(p,i,d,i_max,i_min);
00083
00084 reset();
00085 }
00086
00087 bool Pid::initParam(const std::string& prefix)
00088 {
00089 ros::NodeHandle nh(prefix);
00090 return init(nh);
00091 }
00092
00093 bool Pid::init(const ros::NodeHandle &node)
00094 {
00095 ros::NodeHandle nh(node);
00096
00097 Gains gains;
00098
00099
00100 if (!nh.getParam("p", gains.p_gain_))
00101 {
00102 ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
00103 return false;
00104 }
00105
00106 nh.param("i", gains.i_gain_, 0.0);
00107 nh.param("d", gains.d_gain_, 0.0);
00108
00109
00110 double i_clamp;
00111 nh.param("i_clamp", i_clamp, 0.0);
00112 gains.i_max_ = std::abs(i_clamp);
00113 gains.i_min_ = -std::abs(i_clamp);
00114 setGains(gains);
00115
00116 reset();
00117 initDynamicReconfig(nh);
00118
00119 return true;
00120 }
00121
00122 bool Pid::initXml(TiXmlElement *config)
00123 {
00124
00125 ros::NodeHandle nh(DEFAULT_NAMESPACE);
00126
00127 double i_clamp;
00128 i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
00129
00130 setGains(
00131 config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
00132 config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
00133 config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
00134 std::abs(i_clamp),
00135 -std::abs(i_clamp)
00136 );
00137
00138 reset();
00139 initDynamicReconfig(nh);
00140
00141 return true;
00142 }
00143
00144 void Pid::initDynamicReconfig(ros::NodeHandle &node)
00145 {
00146 ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
00147 << node.getNamespace());
00148
00149
00150 param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
00151 dynamic_reconfig_initialized_ = true;
00152
00153
00154 updateDynamicReconfig();
00155
00156
00157 param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
00158 param_reconfig_server_->setCallback(param_reconfig_callback_);
00159 }
00160
00161 void Pid::reset()
00162 {
00163 p_error_last_ = 0.0;
00164 p_error_ = 0.0;
00165 d_error_ = 0.0;
00166 i_term_ = 0.0;
00167 cmd_ = 0.0;
00168 }
00169
00170 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00171 {
00172 Gains gains = *gains_buffer_.readFromRT();
00173
00174 p = gains.p_gain_;
00175 i = gains.i_gain_;
00176 d = gains.d_gain_;
00177 i_max = gains.i_max_;
00178 i_min = gains.i_min_;
00179 }
00180
00181 Pid::Gains Pid::getGains()
00182 {
00183 return *gains_buffer_.readFromRT();
00184 }
00185
00186 void Pid::setGains(double p, double i, double d, double i_max, double i_min)
00187 {
00188 Gains gains(p,i,d,i_max,i_min);
00189
00190 setGains(gains);
00191 }
00192
00193 void Pid::setGains(const Gains &gains)
00194 {
00195 gains_buffer_.writeFromNonRT(gains);
00196
00197
00198 updateDynamicReconfig(gains);
00199 }
00200
00201 void Pid::updateDynamicReconfig()
00202 {
00203
00204 if(!dynamic_reconfig_initialized_)
00205 return;
00206
00207
00208 control_toolbox::ParametersConfig config;
00209
00210
00211 getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
00212
00213 updateDynamicReconfig(config);
00214 }
00215
00216 void Pid::updateDynamicReconfig(Gains gains_config)
00217 {
00218
00219 if(!dynamic_reconfig_initialized_)
00220 return;
00221
00222 control_toolbox::ParametersConfig config;
00223
00224
00225 config.p_gain = gains_config.p_gain_;
00226 config.i_gain = gains_config.i_gain_;
00227 config.d_gain = gains_config.d_gain_;
00228 config.i_clamp_max = gains_config.i_max_;
00229 config.i_clamp_min = gains_config.i_min_;
00230
00231 updateDynamicReconfig(config);
00232 }
00233
00234 void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
00235 {
00236
00237 if(!dynamic_reconfig_initialized_)
00238 return;
00239
00240
00241 param_reconfig_mutex_.lock();
00242 param_reconfig_server_->updateConfig(config);
00243 param_reconfig_mutex_.unlock();
00244 }
00245
00246 void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level)
00247 {
00248 ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
00249
00250
00251 setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
00252 }
00253
00254 double Pid::computeCommand(double error, ros::Duration dt)
00255 {
00256
00257 Gains gains = *gains_buffer_.readFromRT();
00258
00259 double p_term, d_term;
00260 p_error_ = error;
00261
00262 if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
00263 return 0.0;
00264
00265
00266 p_term = gains.p_gain_ * p_error_;
00267
00268
00269 i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
00270
00271
00272 i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
00273
00274
00275 if (dt.toSec() > 0.0)
00276 {
00277 d_error_ = (p_error_ - p_error_last_) / dt.toSec();
00278 p_error_last_ = p_error_;
00279 }
00280
00281 d_term = gains.d_gain_ * d_error_;
00282 cmd_ = p_term + i_term_ + d_term;
00283
00284 return cmd_;
00285 }
00286
00287 double Pid::updatePid(double error, ros::Duration dt)
00288 {
00289
00290 Gains gains = *gains_buffer_.readFromRT();
00291
00292 double p_term, d_term;
00293 p_error_ = error;
00294
00295 if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
00296 return 0.0;
00297
00298
00299 p_term = gains.p_gain_ * p_error_;
00300
00301
00302 i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
00303
00304
00305 i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
00306
00307
00308 if (dt.toSec() > 0.0)
00309 {
00310 d_error_ = (p_error_ - p_error_last_) / dt.toSec();
00311 p_error_last_ = p_error_;
00312 }
00313
00314 d_term = gains.d_gain_ * d_error_;
00315 cmd_ = - p_term - i_term_ - d_term;
00316
00317 return cmd_;
00318 }
00319
00320 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
00321 {
00322
00323 Gains gains = *gains_buffer_.readFromRT();
00324
00325 double p_term, d_term;
00326 p_error_ = error;
00327 d_error_ = error_dot;
00328
00329 if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
00330 return 0.0;
00331
00332
00333
00334 p_term = gains.p_gain_ * p_error_;
00335
00336
00337 i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
00338
00339
00340 i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
00341
00342
00343 d_term = gains.d_gain_ * d_error_;
00344 cmd_ = p_term + i_term_ + d_term;
00345
00346 return cmd_;
00347 }
00348
00349 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
00350 {
00351
00352 Gains gains = *gains_buffer_.readFromRT();
00353
00354 double p_term, d_term;
00355 p_error_ = error;
00356 d_error_ = error_dot;
00357
00358 if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
00359 return 0.0;
00360
00361
00362
00363 p_term = gains.p_gain_ * p_error_;
00364
00365
00366 i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;
00367
00368
00369 i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) );
00370
00371
00372 d_term = gains.d_gain_ * d_error_;
00373 cmd_ = - p_term - i_term_ - d_term;
00374
00375 return cmd_;
00376 }
00377
00378 void Pid::setCurrentCmd(double cmd)
00379 {
00380 cmd_ = cmd;
00381 }
00382
00383 double Pid::getCurrentCmd()
00384 {
00385 return cmd_;
00386 }
00387
00388 void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
00389 {
00390
00391 Gains gains = *gains_buffer_.readFromRT();
00392
00393 *pe = p_error_;
00394 *ie = gains.i_gain_ ? i_term_/gains.i_gain_ : 0.0;
00395 *de = d_error_;
00396 }
00397
00398 void Pid::printValues()
00399 {
00400 Gains gains = getGains();
00401
00402 ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
00403 << " P Gain: " << gains.p_gain_ << "\n"
00404 << " I Gain: " << gains.i_gain_ << "\n"
00405 << " D Gain: " << gains.d_gain_ << "\n"
00406 << " I_Max: " << gains.i_max_ << "\n"
00407 << " I_Min: " << gains.i_min_ << "\n"
00408 << " P_Error_Last: " << p_error_last_ << "\n"
00409 << " P_Error: " << p_error_ << "\n"
00410 << " D_Error: " << d_error_ << "\n"
00411 << " I_Term: " << i_term_ << "\n"
00412 << " Command: " << cmd_
00413 );
00414
00415 }
00416
00417 }