pid.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /*
36  Author: Melonee Wise
37  Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen
38  Desc: Implements a standard proportional-integral-derivative controller
39 */
40 
41 #include <control_toolbox/pid.h>
42 #include <tinyxml.h>
43 
44 #include <boost/algorithm/clamp.hpp>
45 #include <boost/algorithm/minmax.hpp>
46 
47 namespace control_toolbox {
48 
49 static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix?
50 
51 Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
52  : dynamic_reconfig_initialized_(false)
53 {
54  setGains(p,i,d,i_max,i_min,antiwindup);
55 
56  reset();
57 }
58 
59 Pid::Pid(const Pid &source)
60  : dynamic_reconfig_initialized_(false)
61 {
62  // Copy the realtime buffer to then new PID class
63  gains_buffer_ = source.gains_buffer_;
64 
65  // Reset the state of this PID controller
66  reset();
67 }
68 
69 Pid::~Pid()
70 {
71 }
72 
73 void Pid::initPid(double p, double i, double d, double i_max, double i_min,
74  const ros::NodeHandle& /*node*/)
75 {
76  initPid(p, i, d, i_max, i_min);
77 
78  // Create node handle for dynamic reconfigure
81 }
82 
83 void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
84  const ros::NodeHandle& /*node*/)
85 {
86  initPid(p, i, d, i_max, i_min, antiwindup);
87 
88  // Create node handle for dynamic reconfigure
91 }
92 
93 void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
94 {
95  setGains(p,i,d,i_max,i_min, antiwindup);
96 
97  reset();
98 }
99 
100 bool Pid::initParam(const std::string& prefix, const bool quiet)
101 {
102  ros::NodeHandle nh(prefix);
103  return init(nh, quiet);
104 }
105 
106 bool Pid::init(const ros::NodeHandle &node, const bool quiet)
107 {
108  ros::NodeHandle nh(node);
109 
110  Gains gains;
111 
112  // Load PID gains from parameter server
113  if (!nh.getParam("p", gains.p_gain_))
114  {
115  if (!quiet) {
116  ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
117  }
118  return false;
119  }
120  // Only the P gain is required, the I and D gains are optional and default to 0:
121  nh.param("i", gains.i_gain_, 0.0);
122  nh.param("d", gains.d_gain_, 0.0);
123 
124  // Load integral clamp from param server or default to 0
125  double i_clamp;
126  nh.param("i_clamp", i_clamp, 0.0);
127  gains.i_max_ = std::abs(i_clamp);
128  gains.i_min_ = -std::abs(i_clamp);
129  if(nh.hasParam("i_clamp_min"))
130  {
131  nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
132  gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
133  }
134  if(nh.hasParam("i_clamp_max"))
135  {
136  nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
137  gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
138  }
139  nh.param("antiwindup", gains.antiwindup_, false);
140 
141  nh.param("publish_state", publish_state_, false);
142 
143  if(publish_state_)
144  {
146  state_publisher_->init(nh, "state", 1);
147  }
148 
149  setGains(gains);
150 
151  reset();
153 
154  return true;
155 }
156 
157 bool Pid::initXml(TiXmlElement *config)
158 {
159  // Create node handle for dynamic reconfigure
161 
162  double i_clamp;
163  i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
164 
165  setGains(
166  config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
167  config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
168  config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
169  std::abs(i_clamp),
170  -std::abs(i_clamp),
171  config->Attribute("antiwindup") ? atof(config->Attribute("antiwindup")) : false
172  );
173 
174  reset();
176 
177  return true;
178 }
179 
181 {
182  ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
183  << node.getNamespace());
184 
185  // Start dynamic reconfigure server
188 
189  // Set Dynamic Reconfigure's gains to Pid's values
191 
192  // Set callback
193  param_reconfig_callback_ = std::bind(&Pid::dynamicReconfigCallback, this, std::placeholders::_1, std::placeholders::_2);
195 }
196 
197 void Pid::reset()
198 {
199  reset(0.0, 0.0);
200  valid_p_error_last_ = true;
201 }
202 
203 void Pid::reset(double d_error, double i_error)
204 {
205  p_error_last_ = 0.0;
206  p_error_ = 0.0;
207  i_error_ = i_error;
208  d_error_ = d_error;
209  cmd_ = 0.0;
210  valid_p_error_last_ = false;
211 }
212 
213 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
214 {
215  bool antiwindup;
216  getGains(p, i, d, i_max, i_min, antiwindup);
217 }
218 
219 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
220 {
221  Gains gains = *gains_buffer_.readFromRT();
222 
223  p = gains.p_gain_;
224  i = gains.i_gain_;
225  d = gains.d_gain_;
226  i_max = gains.i_max_;
227  i_min = gains.i_min_;
228  antiwindup = gains.antiwindup_;
229 }
230 
232 {
233  return *gains_buffer_.readFromRT();
234 }
235 
236 void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
237 {
238  Gains gains(p,i,d,i_max,i_min, antiwindup);
239 
240  setGains(gains);
241 }
242 
243 void Pid::setGains(const Gains &gains)
244 {
245  gains_buffer_.writeFromNonRT(gains);
246 
247  // Update dynamic reconfigure with the new gains
248  updateDynamicReconfig(gains);
249 }
250 
252 {
253  // Make sure dynamic reconfigure is initialized
255  return;
256 
257  // Get starting values
258  control_toolbox::ParametersConfig config;
259 
260  // Get starting values
261  getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);
262 
264 }
265 
266 void Pid::updateDynamicReconfig(Gains gains_config)
267 {
268  // Make sure dynamic reconfigure is initialized
270  return;
271 
272  control_toolbox::ParametersConfig config;
273 
274  // Convert to dynamic reconfigure format
275  config.p = gains_config.p_gain_;
276  config.i = gains_config.i_gain_;
277  config.d = gains_config.d_gain_;
278  config.i_clamp_max = gains_config.i_max_;
279  config.i_clamp_min = gains_config.i_min_;
280  config.antiwindup = gains_config.antiwindup_;
281 
282  updateDynamicReconfig(config);
283 }
284 
285 void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
286 {
287  // Make sure dynamic reconfigure is initialized
289  return;
290 
291  // Set starting values, using a shared mutex with dynamic reconfig
292  param_reconfig_mutex_.lock();
293  param_reconfig_server_->updateConfig(config);
294  param_reconfig_mutex_.unlock();
295 }
296 
297 void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/)
298 {
299  ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
300 
301  // Set the gains
302  setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);
303 }
304 
305 double Pid::computeCommand(double error, ros::Duration dt)
306 {
307 
308  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
309  return 0.0;
310 
311  double error_dot = d_error_;
312 
313  // Calculate the derivative error
314  if (dt.toSec() > 0.0)
315  {
316  if (valid_p_error_last_) {
317  error_dot = (error - p_error_last_) / dt.toSec();
318  }
319  p_error_last_ = error;
320  valid_p_error_last_ = true;
321  }
322 
323  return computeCommand(error, error_dot, dt);
324 }
325 
326 double Pid::updatePid(double error, ros::Duration dt)
327 {
328  return -computeCommand(error, dt);
329 }
330 
331 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
332 {
333  // Get the gain parameters from the realtime buffer
334  Gains gains = *gains_buffer_.readFromRT();
335 
336  double p_term, d_term, i_term;
337  p_error_ = error; // this is error = target - state
338  d_error_ = error_dot;
339 
340  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
341  return 0.0;
342 
343  // Calculate proportional contribution to command
344  p_term = gains.p_gain_ * p_error_;
345 
346  // Calculate the integral of the position error
347  i_error_ += dt.toSec() * p_error_;
348 
349  if(gains.antiwindup_ && gains.i_gain_!=0)
350  {
351  // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
352  boost::tuple<double, double> bounds = boost::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
353  i_error_ = boost::algorithm::clamp(i_error_, bounds.get<0>(), bounds.get<1>());
354  }
355 
356  // Calculate integral contribution to command
357  i_term = gains.i_gain_ * i_error_;
358 
359  if(!gains.antiwindup_)
360  {
361  // Limit i_term so that the limit is meaningful in the output
362  i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
363  }
364 
365  // Calculate derivative contribution to command
366  d_term = gains.d_gain_ * d_error_;
367 
368  // Compute the command
369  cmd_ = p_term + i_term + d_term;
370 
371  // Publish controller state if configured
373  {
374  if (state_publisher_->trylock())
375  {
376  state_publisher_->msg_.header.stamp = ros::Time::now();
377  state_publisher_->msg_.timestep = dt;
378  state_publisher_->msg_.error = error;
379  state_publisher_->msg_.error_dot = error_dot;
380  state_publisher_->msg_.p_error = p_error_;
381  state_publisher_->msg_.i_error = i_error_;
382  state_publisher_->msg_.d_error = d_error_;
383  state_publisher_->msg_.p_term = p_term;
384  state_publisher_->msg_.i_term = i_term;
385  state_publisher_->msg_.d_term = d_term;
386  state_publisher_->msg_.i_max = gains.i_max_;
387  state_publisher_->msg_.i_min = gains.i_min_;
388  state_publisher_->msg_.output = cmd_;
389  state_publisher_->unlockAndPublish();
390  }
391  }
392 
393  return cmd_;
394 }
395 
396 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
397 {
398  return -computeCommand(error, error_dot, dt);
399 }
400 
401 void Pid::setCurrentCmd(double cmd)
402 {
403  cmd_ = cmd;
404 }
405 
406 double Pid::getCurrentCmd()
407 {
408  return cmd_;
409 }
410 
411 void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
412 {
413  // Get the gain parameters from the realtime buffer
414  Gains gains = *gains_buffer_.readFromRT();
415 
416  *pe = p_error_;
417  *ie = i_error_;
418  *de = d_error_;
419 }
420 
421 void Pid::printValues()
422 {
423  Gains gains = getGains();
424 
425  ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
426  << " P Gain: " << gains.p_gain_ << "\n"
427  << " I Gain: " << gains.i_gain_ << "\n"
428  << " D Gain: " << gains.d_gain_ << "\n"
429  << " I_Max: " << gains.i_max_ << "\n"
430  << " I_Min: " << gains.i_min_ << "\n"
431  << " Antiwindup: " << gains.antiwindup_ << "\n"
432  << " P_Error_Last: " << p_error_last_ << "\n"
433  << " P_Error: " << p_error_ << "\n"
434  << " I_Error: " << i_error_ << "\n"
435  << " D_Error: " << d_error_ << "\n"
436  << " Command: " << cmd_
437  );
438 
439 }
440 
441 } // namespace
control_toolbox::Pid::Gains::d_gain_
double d_gain_
Definition: pid.h:155
control_toolbox::Pid::d_error_
double d_error_
Definition: pid.h:407
control_toolbox::Pid::getCurrentPIDErrors
void getCurrentPIDErrors(double *pe, double *ie, double *de)
Return PID error terms for the controller.
Definition: pid.cpp:443
control_toolbox::Pid::param_reconfig_callback_
DynamicReconfigServer::CallbackType param_reconfig_callback_
Definition: pid.h:414
control_toolbox::Pid::dynamicReconfigCallback
void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t)
Update the PID parameters from dynamics reconfigure.
Definition: pid.cpp:329
control_toolbox::Pid::Gains::i_min_
double i_min_
Definition: pid.h:157
control_toolbox::Pid::setGains
void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Set PID gains for the controller.
Definition: pid.cpp:268
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
control_toolbox::Pid::state_publisher_
boost::shared_ptr< realtime_tools::RealtimePublisher< control_msgs::PidState > > state_publisher_
Definition: pid.h:400
control_toolbox::Pid::updatePid
ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt)
Update the Pid loop with nonuniform time step size.
Definition: pid.cpp:358
control_toolbox::Pid::i_error_
double i_error_
Definition: pid.h:406
control_toolbox::Pid::param_reconfig_mutex_
boost::recursive_mutex param_reconfig_mutex_
Definition: pid.h:416
control_toolbox::Pid::initXml
bool initXml(TiXmlElement *config)
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains.
Definition: pid.cpp:189
control_toolbox::Pid::p_error_last_
double p_error_last_
Definition: pid.h:404
control_toolbox::Pid::Gains::p_gain_
double p_gain_
Definition: pid.h:153
control_toolbox::Pid::initPid
void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
Zeros out Pid values and initialize Pid-gains and integral term limits Does not initialize dynamic re...
Definition: pid.cpp:125
realtime_tools::RealtimePublisher
control_toolbox::Pid::valid_p_error_last_
bool valid_p_error_last_
Definition: pid.h:403
control_toolbox::Pid::~Pid
~Pid()
Destructor of Pid class.
Definition: pid.cpp:101
control_toolbox::Pid::getGains
void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
Definition: pid.cpp:251
control_toolbox::Pid::publish_state_
bool publish_state_
Definition: pid.h:401
control_toolbox::Pid::computeCommand
double computeCommand(double error, ros::Duration dt)
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is...
Definition: pid.cpp:337
control_toolbox::Pid::init
bool init(const ros::NodeHandle &n, const bool quiet=false)
Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID ...
Definition: pid.cpp:138
filters::clamp
static const T & clamp(const T &a, const T &b, const T &c)
Definition: filters.h:77
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
d
d
control_toolbox::Pid::Pid
Pid(double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=-0.0, bool antiwindup=false)
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits....
Definition: pid.cpp:83
control_toolbox::Pid::Gains::i_max_
double i_max_
Definition: pid.h:156
control_toolbox::Pid::Gains::i_gain_
double i_gain_
Definition: pid.h:154
control_toolbox::Pid::DynamicReconfigServer
dynamic_reconfigure::Server< control_toolbox::ParametersConfig > DynamicReconfigServer
Definition: pid.h:412
control_toolbox::Pid::dynamic_reconfig_initialized_
bool dynamic_reconfig_initialized_
Definition: pid.h:411
control_toolbox::Pid::printValues
void printValues()
Print to console the current parameters.
Definition: pid.cpp:453
control_toolbox::Pid::setCurrentCmd
void setCurrentCmd(double cmd)
Set current command for this PID controller.
Definition: pid.cpp:433
pid.h
ROS_ERROR
#define ROS_ERROR(...)
control_toolbox::Pid::cmd_
double cmd_
Definition: pid.h:408
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
control_toolbox::Pid::initDynamicReconfig
void initDynamicReconfig(ros::NodeHandle &node)
Start the dynamic reconfigure node and load the default values.
Definition: pid.cpp:212
control_toolbox::Pid::Gains
Store gains in a struct to allow easier realtime buffer usage.
Definition: pid.h:124
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
control_toolbox::Pid::getGains
Gains getGains()
Get PID gains for the controller.
Definition: pid.cpp:263
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
control_toolbox::DEFAULT_NAMESPACE
static const std::string DEFAULT_NAMESPACE
Definition: pid.cpp:81
DurationBase< Duration >::toSec
double toSec() const
control_toolbox::Pid::getCurrentCmd
double getCurrentCmd()
Return current command for this PID controller.
Definition: pid.cpp:438
control_toolbox::Pid::p_error_
double p_error_
Definition: pid.h:405
cmd
string cmd
control_toolbox::Pid::updateDynamicReconfig
void updateDynamicReconfig()
Set Dynamic Reconfigure's gains to Pid's values.
Definition: pid.cpp:283
control_toolbox
Definition: dither.h:46
control_toolbox::Pid::reset
void reset()
Reset the state of this PID controller.
Definition: pid.cpp:229
ros::Duration
control_toolbox::Pid::param_reconfig_server_
boost::shared_ptr< DynamicReconfigServer > param_reconfig_server_
Definition: pid.h:413
control_toolbox::Pid::Gains::antiwindup_
bool antiwindup_
Definition: pid.h:158
control_toolbox::LimitedProxy::reset
void reset(double pos_act, double vel_act)
Definition: limited_proxy.cpp:170
ros::NodeHandle
control_toolbox::Pid::gains_buffer_
realtime_tools::RealtimeBuffer< Gains > gains_buffer_
Definition: pid.h:398
ros::Time::now
static Time now()
control_toolbox::Pid::initParam
bool initParam(const std::string &prefix, const bool quiet=false)
Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains.
Definition: pid.cpp:132


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Wed May 11 2022 02:09:47