pid.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036   Author: Melonee Wise
00037   Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen
00038   Desc: Implements a standard proportional-integral-derivative controller
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"; // \todo better default prefix?
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   // Copy the realtime buffer to then new PID class
00060   gains_buffer_ = source.gains_buffer_;
00061 
00062   // Reset the state of this PID controller
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   // Create node handle for dynamic reconfigure
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, const bool quiet)
00088 {
00089   ros::NodeHandle nh(prefix);
00090   return init(nh, quiet);
00091 }
00092 
00093 bool Pid::init(const ros::NodeHandle &node, const bool quiet)
00094 {
00095   ros::NodeHandle nh(node);
00096 
00097   Gains gains;
00098 
00099   // Load PID gains from parameter server
00100   if (!nh.getParam("p", gains.p_gain_))
00101   {
00102     if (!quiet) {
00103       ROS_ERROR("No p gain specified for pid.  Namespace: %s", nh.getNamespace().c_str());
00104     }
00105     return false;
00106   }
00107   // Only the P gain is required, the I and D gains are optional and default to 0:
00108   nh.param("i", gains.i_gain_, 0.0);
00109   nh.param("d", gains.d_gain_, 0.0);
00110 
00111   // Load integral clamp from param server or default to 0
00112   double i_clamp;
00113   nh.param("i_clamp", i_clamp, 0.0);
00114   gains.i_max_ = std::abs(i_clamp);
00115   gains.i_min_ = -std::abs(i_clamp);
00116   if(nh.hasParam("i_clamp_min"))
00117   {
00118     nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
00119     gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
00120   }
00121   if(nh.hasParam("i_clamp_max"))
00122   {
00123     nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
00124     gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
00125   }
00126 
00127   setGains(gains);
00128 
00129   reset();
00130   initDynamicReconfig(nh);
00131 
00132   return true;
00133 }
00134 
00135 bool Pid::initXml(TiXmlElement *config)
00136 {
00137   // Create node handle for dynamic reconfigure
00138   ros::NodeHandle nh(DEFAULT_NAMESPACE);
00139 
00140   double i_clamp;
00141   i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
00142 
00143   setGains(
00144     config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
00145     config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
00146     config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
00147     std::abs(i_clamp),
00148     -std::abs(i_clamp)
00149   );
00150 
00151   reset();
00152   initDynamicReconfig(nh);
00153 
00154   return true;
00155 }
00156 
00157 void Pid::initDynamicReconfig(ros::NodeHandle &node)
00158 {
00159   ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
00160     << node.getNamespace());
00161 
00162   // Start dynamic reconfigure server
00163   param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
00164   dynamic_reconfig_initialized_ = true;
00165 
00166   // Set Dynamic Reconfigure's gains to Pid's values
00167   updateDynamicReconfig();
00168 
00169   // Set callback
00170   param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
00171   param_reconfig_server_->setCallback(param_reconfig_callback_);
00172 }
00173 
00174 void Pid::reset()
00175 {
00176   p_error_last_ = 0.0;
00177   p_error_ = 0.0;
00178   i_error_ = 0.0;
00179   d_error_ = 0.0;
00180   cmd_ = 0.0;
00181 }
00182 
00183 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00184 {
00185   Gains gains = *gains_buffer_.readFromRT();
00186 
00187   p     = gains.p_gain_;
00188   i     = gains.i_gain_;
00189   d     = gains.d_gain_;
00190   i_max = gains.i_max_;
00191   i_min = gains.i_min_;
00192 }
00193 
00194 Pid::Gains Pid::getGains()
00195 {
00196   return *gains_buffer_.readFromRT();
00197 }
00198 
00199 void Pid::setGains(double p, double i, double d, double i_max, double i_min)
00200 {
00201   Gains gains(p,i,d,i_max,i_min);
00202 
00203   setGains(gains);
00204 }
00205 
00206 void Pid::setGains(const Gains &gains)
00207 {
00208   gains_buffer_.writeFromNonRT(gains);
00209 
00210   // Update dynamic reconfigure with the new gains
00211   updateDynamicReconfig(gains);
00212 }
00213 
00214 void Pid::updateDynamicReconfig()
00215 {
00216   // Make sure dynamic reconfigure is initialized
00217   if(!dynamic_reconfig_initialized_)
00218     return;
00219 
00220   // Get starting values
00221   control_toolbox::ParametersConfig config;
00222 
00223   // Get starting values
00224   getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min);
00225 
00226   updateDynamicReconfig(config);
00227 }
00228 
00229 void Pid::updateDynamicReconfig(Gains gains_config)
00230 {
00231   // Make sure dynamic reconfigure is initialized
00232   if(!dynamic_reconfig_initialized_)
00233     return;
00234 
00235   control_toolbox::ParametersConfig config;
00236 
00237   // Convert to dynamic reconfigure format
00238   config.p = gains_config.p_gain_;
00239   config.i = gains_config.i_gain_;
00240   config.d = gains_config.d_gain_;
00241   config.i_clamp_max = gains_config.i_max_;
00242   config.i_clamp_min = gains_config.i_min_;
00243 
00244   updateDynamicReconfig(config);
00245 }
00246 
00247 void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
00248 {
00249   // Make sure dynamic reconfigure is initialized
00250   if(!dynamic_reconfig_initialized_)
00251     return;
00252 
00253   // Set starting values, using a shared mutex with dynamic reconfig
00254   param_reconfig_mutex_.lock();
00255   param_reconfig_server_->updateConfig(config);
00256   param_reconfig_mutex_.unlock();
00257 }
00258 
00259 void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/)
00260 {
00261   ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
00262 
00263   // Set the gains
00264   setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min);
00265 }
00266 
00267 double Pid::computeCommand(double error, ros::Duration dt)
00268 {
00269 
00270   if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
00271     return 0.0;
00272 
00273   double error_dot = d_error_;
00274 
00275   // Calculate the derivative error
00276   if (dt.toSec() > 0.0)
00277   {
00278     error_dot = (error - p_error_last_) / dt.toSec();
00279     p_error_last_ = error;
00280   }
00281 
00282   return computeCommand(error, error_dot, dt);
00283 }
00284 
00285 double Pid::updatePid(double error, ros::Duration dt)
00286 {
00287   return -computeCommand(error, dt);
00288 }
00289 
00290 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
00291 {
00292   // Get the gain parameters from the realtime buffer
00293   Gains gains = *gains_buffer_.readFromRT();
00294 
00295   double p_term, d_term, i_term;
00296   p_error_ = error; // this is error = target - state
00297   d_error_ = error_dot;
00298 
00299   if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
00300     return 0.0;
00301 
00302 
00303   // Calculate proportional contribution to command
00304   p_term = gains.p_gain_ * p_error_;
00305 
00306   // Calculate the integral of the position error
00307   i_error_ += dt.toSec() * p_error_;
00308 
00309   // Calculate integral contribution to command
00310   i_term = gains.i_gain_ * i_error_;
00311 
00312   // Limit i_term so that the limit is meaningful in the output
00313   i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );
00314 
00315   // Calculate derivative contribution to command
00316   d_term = gains.d_gain_ * d_error_;
00317 
00318   // Compute the command
00319   cmd_ = p_term + i_term + d_term;
00320 
00321   return cmd_;
00322 }
00323 
00324 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
00325 {
00326   return -computeCommand(error, error_dot, dt);
00327 }
00328 
00329 void Pid::setCurrentCmd(double cmd)
00330 {
00331   cmd_ = cmd;
00332 }
00333 
00334 double Pid::getCurrentCmd()
00335 {
00336   return cmd_;
00337 }
00338 
00339 void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
00340 {
00341   // Get the gain parameters from the realtime buffer
00342   Gains gains = *gains_buffer_.readFromRT();
00343 
00344   *pe = p_error_;
00345   *ie = i_error_;
00346   *de = d_error_;
00347 }
00348 
00349 void Pid::printValues()
00350 {
00351   Gains gains = getGains();
00352 
00353   ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
00354     << "  P Gain: " << gains.p_gain_ << "\n"
00355     << "  I Gain: " << gains.i_gain_ << "\n"
00356     << "  D Gain: " << gains.d_gain_ << "\n"
00357     << "  I_Max:  " << gains.i_max_  << "\n"
00358     << "  I_Min:  " << gains.i_min_  << "\n"
00359     << "  P_Error_Last: " << p_error_last_  << "\n"
00360     << "  P_Error:      " << p_error_  << "\n"
00361     << "  I_Error:       " << i_error_  << "\n"
00362     << "  D_Error:      " << d_error_  << "\n"
00363     << "  Command:      " << cmd_
00364   );
00365 
00366 }
00367 
00368 } // namespace


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Sat Jun 8 2019 20:43:37