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)
61 {
62  // Copy the realtime buffer to then new PID class
64 
65  // Reset the state of this PID controller
66  reset();
67 }
68 
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
79  ros::NodeHandle nh(DEFAULT_NAMESPACE);
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
89  ros::NodeHandle nh(DEFAULT_NAMESPACE);
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
160  ros::NodeHandle nh(DEFAULT_NAMESPACE);
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_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
195 }
196 
198 {
199  p_error_last_ = 0.0;
200  p_error_ = 0.0;
201  i_error_ = 0.0;
202  d_error_ = 0.0;
203  cmd_ = 0.0;
204 }
205 
206 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
207 {
208  bool antiwindup;
209  getGains(p, i, d, i_max, i_min, antiwindup);
210 }
211 
212 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
213 {
214  Gains gains = *gains_buffer_.readFromRT();
215 
216  p = gains.p_gain_;
217  i = gains.i_gain_;
218  d = gains.d_gain_;
219  i_max = gains.i_max_;
220  i_min = gains.i_min_;
221  antiwindup = gains.antiwindup_;
222 }
223 
225 {
226  return *gains_buffer_.readFromRT();
227 }
228 
229 void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
230 {
231  Gains gains(p,i,d,i_max,i_min, antiwindup);
232 
233  setGains(gains);
234 }
235 
236 void Pid::setGains(const Gains &gains)
237 {
238  gains_buffer_.writeFromNonRT(gains);
239 
240  // Update dynamic reconfigure with the new gains
241  updateDynamicReconfig(gains);
242 }
243 
245 {
246  // Make sure dynamic reconfigure is initialized
248  return;
249 
250  // Get starting values
251  control_toolbox::ParametersConfig config;
252 
253  // Get starting values
254  getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);
255 
256  updateDynamicReconfig(config);
257 }
258 
260 {
261  // Make sure dynamic reconfigure is initialized
263  return;
264 
265  control_toolbox::ParametersConfig config;
266 
267  // Convert to dynamic reconfigure format
268  config.p = gains_config.p_gain_;
269  config.i = gains_config.i_gain_;
270  config.d = gains_config.d_gain_;
271  config.i_clamp_max = gains_config.i_max_;
272  config.i_clamp_min = gains_config.i_min_;
273  config.antiwindup = gains_config.antiwindup_;
274 
275  updateDynamicReconfig(config);
276 }
277 
278 void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
279 {
280  // Make sure dynamic reconfigure is initialized
282  return;
283 
284  // Set starting values, using a shared mutex with dynamic reconfig
285  param_reconfig_mutex_.lock();
286  param_reconfig_server_->updateConfig(config);
287  param_reconfig_mutex_.unlock();
288 }
289 
290 void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/)
291 {
292  ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
293 
294  // Set the gains
295  setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup);
296 }
297 
298 double Pid::computeCommand(double error, ros::Duration dt)
299 {
300 
301  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
302  return 0.0;
303 
304  double error_dot = d_error_;
305 
306  // Calculate the derivative error
307  if (dt.toSec() > 0.0)
308  {
309  error_dot = (error - p_error_last_) / dt.toSec();
310  p_error_last_ = error;
311  }
312 
313  return computeCommand(error, error_dot, dt);
314 }
315 
316 double Pid::updatePid(double error, ros::Duration dt)
317 {
318  return -computeCommand(error, dt);
319 }
320 
321 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
322 {
323  // Get the gain parameters from the realtime buffer
324  Gains gains = *gains_buffer_.readFromRT();
325 
326  double p_term, d_term, i_term;
327  p_error_ = error; // this is error = target - state
328  d_error_ = error_dot;
329 
330  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
331  return 0.0;
332 
333  // Calculate proportional contribution to command
334  p_term = gains.p_gain_ * p_error_;
335 
336  // Calculate the integral of the position error
337  i_error_ += dt.toSec() * p_error_;
338 
339  if(gains.antiwindup_ && gains.i_gain_!=0)
340  {
341  // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
342  boost::tuple<double, double> bounds = boost::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
343  i_error_ = boost::algorithm::clamp(i_error_, bounds.get<0>(), bounds.get<1>());
344  }
345 
346  // Calculate integral contribution to command
347  i_term = gains.i_gain_ * i_error_;
348 
349  if(!gains.antiwindup_)
350  {
351  // Limit i_term so that the limit is meaningful in the output
352  i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
353  }
354 
355  // Calculate derivative contribution to command
356  d_term = gains.d_gain_ * d_error_;
357 
358  // Compute the command
359  cmd_ = p_term + i_term + d_term;
360 
361  // Publish controller state if configured
363  {
364  if (state_publisher_->trylock())
365  {
366  state_publisher_->msg_.header.stamp = ros::Time::now();
367  state_publisher_->msg_.timestep = dt;
368  state_publisher_->msg_.error = error;
369  state_publisher_->msg_.error_dot = error_dot;
370  state_publisher_->msg_.p_error = p_error_;
371  state_publisher_->msg_.i_error = i_error_;
372  state_publisher_->msg_.d_error = d_error_;
373  state_publisher_->msg_.p_term = p_term;
374  state_publisher_->msg_.i_term = i_term;
375  state_publisher_->msg_.d_term = d_term;
376  state_publisher_->msg_.i_max = gains.i_max_;
377  state_publisher_->msg_.i_min = gains.i_min_;
378  state_publisher_->msg_.output = cmd_;
379  state_publisher_->unlockAndPublish();
380  }
381  }
382 
383  return cmd_;
384 }
385 
386 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
387 {
388  return -computeCommand(error, error_dot, dt);
389 }
390 
391 void Pid::setCurrentCmd(double cmd)
392 {
393  cmd_ = cmd;
394 }
395 
397 {
398  return cmd_;
399 }
400 
401 void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
402 {
403  // Get the gain parameters from the realtime buffer
404  Gains gains = *gains_buffer_.readFromRT();
405 
406  *pe = p_error_;
407  *ie = i_error_;
408  *de = d_error_;
409 }
410 
412 {
413  Gains gains = getGains();
414 
415  ROS_INFO_STREAM_NAMED("pid","Current Values of PID Class:\n"
416  << " P Gain: " << gains.p_gain_ << "\n"
417  << " I Gain: " << gains.i_gain_ << "\n"
418  << " D Gain: " << gains.d_gain_ << "\n"
419  << " I_Max: " << gains.i_max_ << "\n"
420  << " I_Min: " << gains.i_min_ << "\n"
421  << " Antiwindup: " << gains.antiwindup_ << "\n"
422  << " P_Error_Last: " << p_error_last_ << "\n"
423  << " P_Error: " << p_error_ << "\n"
424  << " I_Error: " << i_error_ << "\n"
425  << " D_Error: " << d_error_ << "\n"
426  << " Command: " << cmd_
427  );
428 
429 }
430 
431 } // namespace
realtime_tools::RealtimeBuffer< Gains > gains_buffer_
Definition: pid.h:393
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:100
double d_error_
Definition: pid.h:401
void getCurrentPIDErrors(double *pe, double *ie, double *de)
Return PID error terms for the controller.
Definition: pid.cpp:401
DynamicReconfigServer::CallbackType param_reconfig_callback_
Definition: pid.h:408
#define ROS_DEBUG_STREAM_NAMED(name, args)
void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t)
Update the PID parameters from dynamics reconfigure.
Definition: pid.cpp:290
ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt)
Update the Pid loop with nonuniform time step size.
Definition: pid.cpp:316
bool initXml(TiXmlElement *config)
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains...
Definition: pid.cpp:157
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:229
static const std::string DEFAULT_NAMESPACE
Definition: pid.cpp:49
boost::shared_ptr< realtime_tools::RealtimePublisher< control_msgs::PidState > > state_publisher_
Definition: pid.h:395
double i_error_
Definition: pid.h:400
boost::recursive_mutex param_reconfig_mutex_
Definition: pid.h:410
#define ROS_INFO_STREAM_NAMED(name, args)
double p_error_last_
Definition: pid.h:398
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:106
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:93
~Pid()
Destructor of Pid class.
Definition: pid.cpp:69
bool publish_state_
Definition: pid.h:396
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:298
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool dynamic_reconfig_initialized_
Definition: pid.h:405
const std::string & getNamespace() const
static const T & clamp(const T &a, const T &b, const T &c)
Definition: filters.h:45
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:51
Store gains in a struct to allow easier realtime buffer usage.
Definition: pid.h:124
dynamic_reconfigure::Server< control_toolbox::ParametersConfig > DynamicReconfigServer
Definition: pid.h:406
A basic pid class.
Definition: pid.h:117
void printValues()
Print to console the current parameters.
Definition: pid.cpp:411
bool getParam(const std::string &key, std::string &s) const
Gains getGains()
Get PID gains for the controller.
Definition: pid.cpp:224
static Time now()
void setCurrentCmd(double cmd)
Set current command for this PID controller.
Definition: pid.cpp:391
void updateDynamicReconfig()
Set Dynamic Reconfigure&#39;s gains to Pid&#39;s values.
Definition: pid.cpp:244
void initDynamicReconfig(ros::NodeHandle &node)
Start the dynamic reconfigure node and load the default values.
Definition: pid.cpp:180
bool hasParam(const std::string &key) const
boost::shared_ptr< DynamicReconfigServer > param_reconfig_server_
Definition: pid.h:407
double getCurrentCmd()
Return current command for this PID controller.
Definition: pid.cpp:396
#define ROS_ERROR(...)
double p_error_
Definition: pid.h:399
void reset()
Reset the state of this PID controller.
Definition: pid.cpp:197


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Mon Jun 10 2019 12:56:31