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 <cmath>
#include <iostream>
#include <limits>
#include <string>
 
#include "fmt/format.h"
#include "rclcpp/duration.hpp"
#include "realtime_tools/realtime_thread_safe_box.hpp"
 
namespace control_toolbox
{
struct AntiWindupStrategy
{
public:
  enum Value : int8_t
  {
    UNDEFINED = -1,
    NONE,
    BACK_CALCULATION,
    CONDITIONAL_INTEGRATION
  };
 
  AntiWindupStrategy()
  : type(NONE),
    i_max(std::numeric_limits<double>::infinity()),
    i_min(-std::numeric_limits<double>::infinity()),
    tracking_time_constant(0.0),
    error_deadband(std::numeric_limits<double>::epsilon())
  {
  }
 
  void set_type(const std::string & s)
  {
    if (s == "back_calculation")
    {
      type = BACK_CALCULATION;
    }
    else if (s == "conditional_integration")
    {
      type = CONDITIONAL_INTEGRATION;
    }
    else if (s == "none")
    {
      type = NONE;
    }
    else
    {
      type = UNDEFINED;
      throw std::invalid_argument(
        "AntiWindupStrategy: Unknown antiwindup strategy : '" + s +
        "'. Valid strategies are: 'back_calculation', 'conditional_integration', "
        "'none'.");
    }
  }
 
  void validate() const
  {
    if (type == UNDEFINED)
    {
      throw std::invalid_argument("AntiWindupStrategy is UNDEFINED. Please set a valid type");
    }
    if (
      type == BACK_CALCULATION &&
      (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant)))
    {
      throw std::invalid_argument(
        "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant "
        "(tracking_time_constant)");
    }
    if (i_min >= i_max)
    {
      throw std::invalid_argument(
        fmt::format(
          "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max));
    }
    if (
      type != NONE && type != UNDEFINED && type != BACK_CALCULATION &&
      type != CONDITIONAL_INTEGRATION)
    {
      throw std::invalid_argument("AntiWindupStrategy has an invalid type");
    }
  }
 
  operator std::string() const { return to_string(); }
 
  constexpr bool operator==(Value other) const { return type == other; }
  constexpr bool operator!=(Value other) const { return type != other; }
 
  std::string to_string() const
  {
    switch (type)
    {
      case BACK_CALCULATION:
        return "back_calculation";
      case CONDITIONAL_INTEGRATION:
        return "conditional_integration";
      case NONE:
        return "none";
      case UNDEFINED:
      default:
        return "UNDEFINED";
    }
  }
 
  Value type = UNDEFINED;
  double i_max = std::numeric_limits<double>::infinity();  
  double i_min = -std::numeric_limits<double>::infinity(); 
  // tracking_time_constant Specifies the tracking time constant for the 'back_calculation'
  // strategy. If set to 0.0 a recommended default value will be applied.
  double tracking_time_constant = 0.0; 
  double error_deadband =
    std::numeric_limits<double>::epsilon(); 
};
 
template <typename T>
inline bool is_zero(T value, T tolerance = std::numeric_limits<T>::epsilon())
{
  return std::abs(value) <= tolerance;
}
 
/***************************************************/
/***************************************************/
 
class Pid
{
public:
  struct Gains
  {
    Gains(
      double p, double i, double d, double u_max, double u_min,
      const AntiWindupStrategy & antiwindup_strat)
    : p_gain_(p),
      i_gain_(i),
      d_gain_(d),
      i_max_(antiwindup_strat.i_max),
      i_min_(antiwindup_strat.i_min),
      u_max_(u_max),
      u_min_(u_min),
      antiwindup_strat_(antiwindup_strat)
    {
      if (std::isnan(u_min) || std::isnan(u_max))
      {
        throw std::invalid_argument("Gains: u_min and u_max must not be NaN");
      }
      if (u_min > u_max)
      {
        std::cout << "Received invalid u_min and u_max values: " << "u_min: " << u_min
                  << ", u_max: " << u_max << ". Setting saturation to false." << std::endl;
        u_max_ = std::numeric_limits<double>::infinity();
        u_min_ = -std::numeric_limits<double>::infinity();
      }
    }
 
    bool validate(std::string & error_msg) const
    {
      if (i_min_ > i_max_)
      {
        error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_);
        return false;
      }
      else if (u_min_ >= u_max_)
      {
        error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_);
        return false;
      }
      else if (std::isnan(u_min_) || std::isnan(u_max_))
      {
        error_msg = "Gains: u_min and u_max must not be NaN";
        return false;
      }
      try
      {
        antiwindup_strat_.validate();
      }
      catch (const std::exception & e)
      {
        error_msg = e.what();
        return false;
      }
      return true;
    }
 
    void print() const
    {
      std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_
                << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_
                << ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string()
                << std::endl;
    }
 
    double p_gain_ = 0.0; 
    double i_gain_ = 0.0; 
    double d_gain_ = 0.0; 
    double i_max_ =
      std::numeric_limits<double>::infinity(); 
    double i_min_ =
      -std::numeric_limits<double>::infinity(); 
    double u_max_ = std::numeric_limits<double>::infinity();  
    double u_min_ = -std::numeric_limits<double>::infinity(); 
    AntiWindupStrategy antiwindup_strat_;                     
  };
 
  Pid(
    double p = 0.0, double i = 0.0, double d = 0.0,
    double u_max = std::numeric_limits<double>::infinity(),
    double u_min = -std::numeric_limits<double>::infinity(),
    const AntiWindupStrategy & antiwindup_strat = AntiWindupStrategy());
 
  Pid(const Pid & source);
 
  ~Pid();
 
  bool initialize(
    double p, double i, double d, double u_max, double u_min,
    const AntiWindupStrategy & antiwindup_strat);
 
  void reset();
 
  void reset(bool save_i_term);
 
  void clear_saved_iterm();
 
  void get_gains(
    double & p, double & i, double & d, double & u_max, double & u_min,
    AntiWindupStrategy & antiwindup_strat);
 
  Gains get_gains();
 
  Gains get_gains_rt() { return gains_; }
 
  bool set_gains(
    double p, double i, double d, double u_max, double u_min,
    const AntiWindupStrategy & antiwindup_strat);
 
  bool 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 box to then new PID class
    gains_box_ = source.gains_box_;
 
    // Reset the state of this PID controller
    reset();
 
    return *this;
  }
 
protected:
  // local copy of the gains for the RT loop
  Gains gains_{
    0.0,
    0.0,
    0.0,
    std::numeric_limits<double>::infinity(),
    -std::numeric_limits<double>::infinity(),
    AntiWindupStrategy()};
  // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without
  // blocking the realtime update loop
  realtime_tools::RealtimeThreadSafeBox<Gains> gains_box_{gains_};
 
  double p_error_last_ = 0; 
  double p_error_ = 0;      
  double d_error_ = 0;      
  double i_term_ = 0;       
  double cmd_ = 0;          
  double cmd_unsat_ = 0;    
};
 
}  // namespace control_toolbox
 
#endif  // CONTROL_TOOLBOX__PID_HPP_