pid.hpp

// Copyright (c) 2008, Willow Garage, Inc.
//
// 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 HOLDER 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 > 0)
    {
      throw std::invalid_argument(
        fmt::format("PID requires i_min to be smaller or equal to 0 (i_min: {})", i_min));
    }
    if (i_max < 0)
    {
      throw std::invalid_argument(
        fmt::format("PID requires i_max to be greater or equal to 0 (i_max: {})", i_max));
    }
    if (
      type != NONE && type != UNDEFINED && type != BACK_CALCULATION &&
      type != CONDITIONAL_INTEGRATION)
    {
      throw std::invalid_argument("AntiWindupStrategy has an invalid type");
    }
  }
 
  void print() const { std::cout << string() << std::endl; }
 
  std::string string() const
  {
    return fmt::format(
      "antiwindup_strat: {}\ti_max: {}\ti_min: {}\ttracking_time_constant: {}\terror_deadband: {}",
      to_string(), i_max, i_min, tracking_time_constant, error_deadband);
  }
 
  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),
      u_max_(u_max),
      u_min_(u_min),
      antiwindup_strat_(antiwindup_strat)
    {
    }
 
    bool validate(std::string & error_msg) const
    {
      if (u_min_ >= u_max_)  // is false if any value is nan
      {
        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 << string() << std::endl; }
 
    std::string string() const
    {
      return fmt::format(
        "Gains(p: {}, i: {}, d: {}, u_max: {}, u_min: {}, antiwindup_strat: '{}')", p_gain_,
        i_gain_, d_gain_, u_max_, u_min_, antiwindup_strat_.string());
    }
 
    double p_gain_ = 0.0;                                     
    double i_gain_ = 0.0;                                     
    double d_gain_ = 0.0;                                     
    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_