Classes | Public Member Functions | Private Types | Private Attributes | List of all members
control_toolbox::Pid Class Reference

A basic pid class. More...

#include <pid.h>

Classes

struct  Gains
 Store gains in a struct to allow easier realtime buffer usage. More...
 

Public Member Functions

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 computed from the change in the error and the timestep dt. More...
 
double computeCommand (double error, double error_dot, ros::Duration dt)
 Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error. More...
 
void dynamicReconfigCallback (control_toolbox::ParametersConfig &config, uint32_t)
 Update the PID parameters from dynamics reconfigure. More...
 
double getCurrentCmd ()
 Return current command for this PID controller. More...
 
void getCurrentPIDErrors (double *pe, double *ie, double *de)
 Return PID error terms for the controller. More...
 
void getGains (double &p, double &i, double &d, double &i_max, double &i_min)
 Get PID gains for the controller. More...
 
void getGains (double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
 
Gains getGains ()
 Get PID gains for the controller. More...
 
bool init (const ros::NodeHandle &n, const bool quiet=false)
 Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID gains. More...
 
void initDynamicReconfig (ros::NodeHandle &node)
 Start the dynamic reconfigure node and load the default values. More...
 
bool initParam (const std::string &prefix, const bool quiet=false)
 Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains. More...
 
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 reconfigure for PID gains. More...
 
void initPid (double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &)
 Zeros out Pid values and initialize Pid-gains and integral term limits Initializes dynamic reconfigure for PID gains. More...
 
void initPid (double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &)
 
bool initXml (TiXmlElement *config)
 Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains. More...
 
Pidoperator= (const Pid &source)
 Custom assignment operator Does not initialize dynamic reconfigure for PID gains. More...
 
 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. Does not initialize dynamic reconfigure for PID gains. More...
 
 Pid (const Pid &source)
 Copy constructor required for preventing mutexes from being copied. More...
 
void printValues ()
 Print to console the current parameters. More...
 
void reset ()
 Reset the state of this PID controller. More...
 
void reset (double d_error, double i_error)
 Reset the state of this PID controller. Allows setting d_error & i_error initial value to support cases where PID is activated with some initial command. More...
 
void setCurrentCmd (double cmd)
 Set current command for this PID controller. More...
 
void setGains (double p, double i, double d, double i_max, double i_min, bool antiwindup=false)
 Set PID gains for the controller. More...
 
void setGains (const Gains &gains)
 Set PID gains for the controller. More...
 
void updateDynamicReconfig ()
 Set Dynamic Reconfigure's gains to Pid's values. More...
 
void updateDynamicReconfig (Gains gains_config)
 
void updateDynamicReconfig (control_toolbox::ParametersConfig config)
 
ROS_DEPRECATED double updatePid (double p_error, ros::Duration dt)
 Update the Pid loop with nonuniform time step size. More...
 
ROS_DEPRECATED double updatePid (double error, double error_dot, ros::Duration dt)
 Update the Pid loop with nonuniform time step size. This update call allows the user to pass in a precomputed derivative error. More...
 
 ~Pid ()
 Destructor of Pid class. More...
 

Private Types

typedef dynamic_reconfigure::Server< control_toolbox::ParametersConfig > DynamicReconfigServer
 

Private Attributes

double cmd_
 
double d_error_
 
bool dynamic_reconfig_initialized_
 
realtime_tools::RealtimeBuffer< Gainsgains_buffer_
 
double i_error_
 
double p_error_
 
double p_error_last_
 
DynamicReconfigServer::CallbackType param_reconfig_callback_
 
boost::recursive_mutex param_reconfig_mutex_
 
boost::shared_ptr< DynamicReconfigServerparam_reconfig_server_
 
bool publish_state_
 
boost::shared_ptr< realtime_tools::RealtimePublisher< control_msgs::PidState > > state_publisher_
 
bool valid_p_error_last_
 

Detailed Description

A basic pid class.

This class implements a generic structure that can be used to create a wide range of pid controllers. It can function independently or be subclassed to provide more specific controls based on a particular control loop.

In particular, this class implements the standard pid equation:

$command = -p_{term} - i_{term} - d_{term} $

where:

given:

ROS interface

Parameters
pProportional gain
dDerivative gain
iIntegral gain
i_clampMin/max bounds for the integral windup, the clamp is applied to the $i_{term}$
publish_stateEnable publishing internal controller state on the state topic. May break real-time guarantees due to clock_gettime system call.

Usage

To use the Pid class, you should first call some version of init() (in non-realtime) and then call updatePid() at every update step. For example:

control_toolbox::Pid pid;
pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
double position_desi_ = 0.5;
...
ros::Time last_time = ros::Time::now();
while (true) {
ros::Time time = ros::Time::now();
double effort = pid.updatePid(currentPosition() - position_desi_, time - last_time);
last_time = time;
}

Definition at line 117 of file pid.h.

Member Typedef Documentation

◆ DynamicReconfigServer

typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> control_toolbox::Pid::DynamicReconfigServer
private

Definition at line 412 of file pid.h.

Constructor & Destructor Documentation

◆ Pid() [1/2]

control_toolbox::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. Does not initialize dynamic reconfigure for PID gains.

Parameters
pThe proportional gain.
iThe integral gain.
dThe derivative gain.
i_maxThe max integral windup.
i_minThe min integral windup.

Definition at line 51 of file pid.cpp.

◆ Pid() [2/2]

control_toolbox::Pid::Pid ( const Pid source)

Copy constructor required for preventing mutexes from being copied.

Parameters
source- Pid to copy

Definition at line 59 of file pid.cpp.

◆ ~Pid()

control_toolbox::Pid::~Pid ( )

Destructor of Pid class.

Definition at line 69 of file pid.cpp.

Member Function Documentation

◆ computeCommand() [1/2]

double control_toolbox::Pid::computeCommand ( double  error,
ros::Duration  dt 
)

Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt.

Parameters
errorError since last call (error = target - state)
dtChange in time since last call
Returns
PID command

Definition at line 305 of file pid.cpp.

◆ computeCommand() [2/2]

double control_toolbox::Pid::computeCommand ( double  error,
double  error_dot,
ros::Duration  dt 
)

Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.

Parameters
errorError since last call (error = target - state)
error_dotd(Error)/dt since last call
dtChange in time since last call
Returns
PID command

Definition at line 331 of file pid.cpp.

◆ dynamicReconfigCallback()

void control_toolbox::Pid::dynamicReconfigCallback ( control_toolbox::ParametersConfig &  config,
uint32_t   
)

Update the PID parameters from dynamics reconfigure.

Definition at line 297 of file pid.cpp.

◆ getCurrentCmd()

double control_toolbox::Pid::getCurrentCmd ( )

Return current command for this PID controller.

Definition at line 406 of file pid.cpp.

◆ getCurrentPIDErrors()

void control_toolbox::Pid::getCurrentPIDErrors ( double *  pe,
double *  ie,
double *  de 
)

Return PID error terms for the controller.

Parameters
peThe proportional error.
ieThe integral error.
deThe derivative error.

Definition at line 411 of file pid.cpp.

◆ getGains() [1/3]

void control_toolbox::Pid::getGains ( double &  p,
double &  i,
double &  d,
double &  i_max,
double &  i_min 
)

Get PID gains for the controller.

Parameters
pThe proportional gain.
iThe integral gain.
dThe derivative gain.
i_maxThe max integral windup.
i_minThe min integral windup.

Definition at line 213 of file pid.cpp.

◆ getGains() [2/3]

void control_toolbox::Pid::getGains ( double &  p,
double &  i,
double &  d,
double &  i_max,
double &  i_min,
bool &  antiwindup 
)

Definition at line 219 of file pid.cpp.

◆ getGains() [3/3]

Pid::Gains control_toolbox::Pid::getGains ( )

Get PID gains for the controller.

Returns
gains A struct of the PID gain values

Definition at line 231 of file pid.cpp.

◆ init()

bool control_toolbox::Pid::init ( const ros::NodeHandle n,
const bool  quiet = false 
)

Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID gains.

Parameters
nThe NodeHandle which should be used to query parameters.
quietIf true, no error messages will be emitted on failure.

Definition at line 106 of file pid.cpp.

◆ initDynamicReconfig()

void control_toolbox::Pid::initDynamicReconfig ( ros::NodeHandle node)

Start the dynamic reconfigure node and load the default values.

Parameters
node- a node handle where dynamic reconfigure services will be published

Definition at line 180 of file pid.cpp.

◆ initParam()

bool control_toolbox::Pid::initParam ( const std::string &  prefix,
const bool  quiet = false 
)

Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains.

Parameters
prefixThe namespace prefix.
quietIf true, no error messages will be emitted on failure.

Definition at line 100 of file pid.cpp.

◆ initPid() [1/3]

void control_toolbox::Pid::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 reconfigure for PID gains.

Parameters
pThe proportional gain.
iThe integral gain.
dThe derivative gain.
i_maxThe max integral windup.
i_minThe min integral windup.

Definition at line 93 of file pid.cpp.

◆ initPid() [2/3]

void control_toolbox::Pid::initPid ( double  p,
double  i,
double  d,
double  i_max,
double  i_min,
const ros::NodeHandle  
)

Zeros out Pid values and initialize Pid-gains and integral term limits Initializes dynamic reconfigure for PID gains.

Parameters
pThe proportional gain.
iThe integral gain.
dThe derivative gain.
i_maxThe max integral windup.
i_minThe min integral windup.

Definition at line 73 of file pid.cpp.

◆ initPid() [3/3]

void control_toolbox::Pid::initPid ( double  p,
double  i,
double  d,
double  i_max,
double  i_min,
bool  antiwindup,
const ros::NodeHandle  
)

Definition at line 83 of file pid.cpp.

◆ initXml()

bool control_toolbox::Pid::initXml ( TiXmlElement *  config)

Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains.

Parameters
configthe XML element

Definition at line 157 of file pid.cpp.

◆ operator=()

Pid& control_toolbox::Pid::operator= ( const Pid source)
inline

Custom assignment operator Does not initialize dynamic reconfigure for PID gains.

Definition at line 380 of file pid.h.

◆ printValues()

void control_toolbox::Pid::printValues ( )

Print to console the current parameters.

Definition at line 421 of file pid.cpp.

◆ reset() [1/2]

void control_toolbox::Pid::reset ( )

Reset the state of this PID controller.

Definition at line 197 of file pid.cpp.

◆ reset() [2/2]

void control_toolbox::Pid::reset ( double  d_error,
double  i_error 
)

Reset the state of this PID controller. Allows setting d_error & i_error initial value to support cases where PID is activated with some initial command.

Definition at line 203 of file pid.cpp.

◆ setCurrentCmd()

void control_toolbox::Pid::setCurrentCmd ( double  cmd)

Set current command for this PID controller.

Definition at line 401 of file pid.cpp.

◆ setGains() [1/2]

void control_toolbox::Pid::setGains ( double  p,
double  i,
double  d,
double  i_max,
double  i_min,
bool  antiwindup = false 
)

Set PID gains for the controller.

Parameters
pThe proportional gain.
iThe integral gain.
dThe derivative gain.
i_maxThe max integral windup.
i_minThe min integral windup.

Definition at line 236 of file pid.cpp.

◆ setGains() [2/2]

void control_toolbox::Pid::setGains ( const Gains gains)

Set PID gains for the controller.

Parameters
gainsA struct of the PID gain values

Definition at line 243 of file pid.cpp.

◆ updateDynamicReconfig() [1/3]

void control_toolbox::Pid::updateDynamicReconfig ( )

Set Dynamic Reconfigure's gains to Pid's values.

Definition at line 251 of file pid.cpp.

◆ updateDynamicReconfig() [2/3]

void control_toolbox::Pid::updateDynamicReconfig ( Gains  gains_config)

Definition at line 266 of file pid.cpp.

◆ updateDynamicReconfig() [3/3]

void control_toolbox::Pid::updateDynamicReconfig ( control_toolbox::ParametersConfig  config)

Definition at line 285 of file pid.cpp.

◆ updatePid() [1/2]

double control_toolbox::Pid::updatePid ( double  p_error,
ros::Duration  dt 
)

Update the Pid loop with nonuniform time step size.

Deprecated:
in ROS Hydro. This function assumes p_error = (state - target) which is an unconventional definition of the error. Please use computeCommand instead, which assumes error = (target - state) . Note that calls to computeCommand should not be mixed with calls to updatePid.
Parameters
p_errorError since last call (p_state-p_target)
dtChange in time since last call

Definition at line 326 of file pid.cpp.

◆ updatePid() [2/2]

double control_toolbox::Pid::updatePid ( double  error,
double  error_dot,
ros::Duration  dt 
)

Update the Pid loop with nonuniform time step size. This update call allows the user to pass in a precomputed derivative error.

Deprecated:
in ROS Hydro. This function assumes p_error = (state - target) which is an unconventional definition of the error. Please use computeCommand instead, which assumes error = (target - state) . Note that calls to computeCommand should not be mixed with calls to updatePid.
Parameters
errorError since last call (p_state-p_target)
error_dotd(Error)/dt since last call
dtChange in time since last call

Definition at line 396 of file pid.cpp.

Member Data Documentation

◆ cmd_

double control_toolbox::Pid::cmd_
private

Command to send.

Definition at line 408 of file pid.h.

◆ d_error_

double control_toolbox::Pid::d_error_
private

Derivative of position error.

Definition at line 407 of file pid.h.

◆ dynamic_reconfig_initialized_

bool control_toolbox::Pid::dynamic_reconfig_initialized_
private

Definition at line 411 of file pid.h.

◆ gains_buffer_

realtime_tools::RealtimeBuffer<Gains> control_toolbox::Pid::gains_buffer_
private

Definition at line 398 of file pid.h.

◆ i_error_

double control_toolbox::Pid::i_error_
private

Integral of position error.

Definition at line 406 of file pid.h.

◆ p_error_

double control_toolbox::Pid::p_error_
private

Position error.

Definition at line 405 of file pid.h.

◆ p_error_last_

double control_toolbox::Pid::p_error_last_
private

_Save position state for derivative state calculation.

Definition at line 404 of file pid.h.

◆ param_reconfig_callback_

DynamicReconfigServer::CallbackType control_toolbox::Pid::param_reconfig_callback_
private

Definition at line 414 of file pid.h.

◆ param_reconfig_mutex_

boost::recursive_mutex control_toolbox::Pid::param_reconfig_mutex_
private

Definition at line 416 of file pid.h.

◆ param_reconfig_server_

boost::shared_ptr<DynamicReconfigServer> control_toolbox::Pid::param_reconfig_server_
private

Definition at line 413 of file pid.h.

◆ publish_state_

bool control_toolbox::Pid::publish_state_
private

Definition at line 401 of file pid.h.

◆ state_publisher_

boost::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::PidState> > control_toolbox::Pid::state_publisher_
private

Definition at line 400 of file pid.h.

◆ valid_p_error_last_

bool control_toolbox::Pid::valid_p_error_last_
private

Is saved position state valid for derivative state calculation

Definition at line 403 of file pid.h.


The documentation for this class was generated from the following files:


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