pid.hpp

// Copyright (c) 2008, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//  * Neither the name of the Willow Garage nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
 
#ifndef CONTROL_TOOLBOX__PID_HPP_
#define CONTROL_TOOLBOX__PID_HPP_
 
#include <chrono>
 
#include "rclcpp/duration.hpp"
#include "realtime_tools/realtime_buffer.hpp"
 
namespace control_toolbox
{
/***************************************************/
/***************************************************/
 
class Pid
{
public:
  struct Gains
  {
    Gains(double p, double i, double d, double i_max, double i_min)
    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(true)
    {
    }
 
    Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup)
    {
    }
 
    // Default constructor
    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false)
    {
    }
 
    double p_gain_;   
    double i_gain_;   
    double d_gain_;   
    double i_max_;    
    double i_min_;    
    bool antiwindup_; 
  };
 
  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);
 
  Pid(const Pid & source);
 
  ~Pid();
 
  void initialize(
    double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
 
  void reset();
 
  void reset(bool save_i_term);
 
  void clear_saved_iterm();
 
  void get_gains(double & p, double & i, double & d, double & i_max, double & i_min);
 
  void get_gains(
    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);
 
  Gains get_gains();
 
  void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
 
  void set_gains(const Gains & gains);
 
  [[nodiscard]] double compute_command(double error, const double & dt_s);
 
  [[nodiscard]] double compute_command(double error, const rcl_duration_value_t & dt_ns);
 
  [[nodiscard]] double compute_command(double error, const rclcpp::Duration & dt);
 
  [[nodiscard]] double compute_command(double error, const std::chrono::nanoseconds & dt_ns);
 
  [[nodiscard]] double compute_command(double error, double error_dot, const double & dt_s);
 
  [[nodiscard]] double compute_command(
    double error, double error_dot, const rcl_duration_value_t & dt_ns);
 
  [[nodiscard]] double compute_command(double error, double error_dot, const rclcpp::Duration & dt);
 
  [[nodiscard]] double compute_command(
      double error, double error_dot, const std::chrono::nanoseconds & dt_ns);
 
  void set_current_cmd(double cmd);
 
  double get_current_cmd();
 
  void get_current_pid_errors(double & pe, double & ie, double & de);
 
  Pid & operator=(const Pid & source)
  {
    if (this == &source) {
      return *this;
    }
 
    // Copy the realtime buffer to then new PID class
    gains_buffer_ = source.gains_buffer_;
 
    // Reset the state of this PID controller
    reset();
 
    return *this;
  }
 
protected:
  // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
  // blocking the realtime update loop
  realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
 
  double p_error_last_; 
  double p_error_;      
  double d_error_;      
  double i_term_;       
  double cmd_;          
};
 
}  // namespace control_toolbox
 
#endif  // CONTROL_TOOLBOX__PID_HPP_