Program Listing for File pid.hpp

↰ Return to documentation for file (include/control_toolbox/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 <limits>
#include <string>

#include "rclcpp/duration.hpp"
#include "realtime_tools/realtime_buffer.hpp"

namespace control_toolbox
{
class AntiwindupStrategy
{
public:
  enum Value : int8_t
  {
    NONE = 0,
    BACK_CALCULATION,
    CONDITIONAL_INTEGRATION
  };

  constexpr AntiwindupStrategy() : value_(NONE) {}
  constexpr AntiwindupStrategy(Value v) : value_(v) {}  // NOLINT(runtime/explicit)

  explicit AntiwindupStrategy(const std::string & s)
  {
    if (s == "back_calculation")
    {
      value_ = BACK_CALCULATION;
    }
    else if (s == "conditional_integration")
    {
      value_ = CONDITIONAL_INTEGRATION;
    }
    else
    {
      value_ = NONE;
    }
  }

  operator std::string() const { return to_string(); }

  constexpr bool operator==(AntiwindupStrategy other) const { return value_ == other.value_; }
  constexpr bool operator!=(AntiwindupStrategy other) const { return value_ != other.value_; }

  constexpr bool operator==(Value other) const { return value_ == other; }
  constexpr bool operator!=(Value other) const { return value_ != other; }

  std::string to_string() const
  {
    switch (value_)
    {
      case BACK_CALCULATION:
        return "back_calculation";
      case CONDITIONAL_INTEGRATION:
        return "conditional_integration";
      case NONE:
      default:
        return "none";
    }
  }

private:
  Value value_;
};

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
  {
    [[deprecated("Use constructor with AntiwindupStrategy instead.")]]
    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),
      u_max_(0.0),
      u_min_(0.0),
      trk_tc_(0.0),
      saturation_(false),
      antiwindup_(true),
      antiwindup_strat_(AntiwindupStrategy::NONE)
    {
    }

    [[deprecated("Use constructor with AntiwindupStrategy instead.")]]
    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),
      u_max_(0.0),
      u_min_(0.0),
      trk_tc_(0.0),
      saturation_(false),
      antiwindup_(antiwindup),
      antiwindup_strat_(AntiwindupStrategy::NONE)
    {
    }

    [[deprecated("Use constructor with AntiwindupStrategy only.")]]
    Gains(
      double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
      double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat)
    : p_gain_(p),
      i_gain_(i),
      d_gain_(d),
      i_max_(i_max),
      i_min_(i_min),
      u_max_(u_max),
      u_min_(u_min),
      trk_tc_(trk_tc),
      saturation_(saturation),
      antiwindup_(antiwindup),
      antiwindup_strat_(antiwindup_strat)
    {
    }

    Gains(
      double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
      AntiwindupStrategy antiwindup_strat)
    : p_gain_(p),
      i_gain_(i),
      d_gain_(d),
      i_max_(0.0),
      i_min_(0.0),
      u_max_(u_max),
      u_min_(u_min),
      trk_tc_(trk_tc),
      saturation_(saturation),
      antiwindup_(false),
      antiwindup_strat_(antiwindup_strat)
    {
    }

    // Default constructor
    Gains()
    : p_gain_(0.0),
      i_gain_(0.0),
      d_gain_(0.0),
      i_max_(0.0),
      i_min_(0.0),
      u_max_(0.0),
      u_min_(0.0),
      trk_tc_(0.0),
      saturation_(false),
      antiwindup_(false),
      antiwindup_strat_(AntiwindupStrategy::NONE)
    {
    }

    double p_gain_;
    double i_gain_;
    double d_gain_;
    double i_max_;
    double i_min_;
    double u_max_;
    double u_min_;
    double trk_tc_;
    bool saturation_;
    bool antiwindup_;
    AntiwindupStrategy antiwindup_strat_;
  };

  [[deprecated("Use constructor with AntiwindupStrategy only.")]]
  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);

  [[deprecated("Use constructor with AntiwindupStrategy only.")]]
  Pid(
    double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
    double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat);

  Pid(
    double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
    AntiwindupStrategy antiwindup_strat);

  Pid(const Pid & source);

  ~Pid();

  [[deprecated("Use initialize with AntiwindupStrategy instead.")]]
  void initialize(
    double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  [[deprecated("Use initialize() instead")]] void initPid(
    double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  [[deprecated("Use initialize with AntiwindupStrategy only.")]]
  void initialize(
    double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
    double trk_tc, bool saturation, bool antiwindup, AntiwindupStrategy antiwindup_strat);

  void initialize(
    double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
    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 & i_max, double & i_min);

  [[deprecated("Use get_gains() instead")]] void getGains(
    double & p, double & i, double & d, double & i_max, double & i_min);

  [[deprecated("Use get_gains overload without bool antiwindup.")]]
  void get_gains(
    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);

  [[deprecated("Use get_gains() instead")]] void getGains(
    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);

  [[deprecated("Use get_gains overload with AntiwindupStrategy only.")]]
  void get_gains(
    double & p, double & i, double & d, double & i_max, double & i_min, double & u_max,
    double & u_min, double & trk_tc, bool & saturation, bool & antiwindup,
    AntiwindupStrategy & antiwindup_strat);

  void get_gains(
    double & p, double & i, double & d, double & u_max, double & u_min, double & trk_tc,
    bool & saturation, AntiwindupStrategy & antiwindup_strat);

  Gains get_gains();

  [[deprecated("Use get_gains() instead")]] Gains getGains();

  [[deprecated("Use set_gains with AntiwindupStrategy instead.")]]
  void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  [[deprecated("Use set_gains() instead")]] void setGains(
    double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

  [[deprecated("Use set_gains with AntiwindupStrategy only.")]]
  void set_gains(
    double p, double i, double d, double i_max, double i_min, double u_max, double u_min,
    double trk_tc = 0.0, bool saturation = false, bool antiwindup = false,
    AntiwindupStrategy antiwindup_strat = AntiwindupStrategy::NONE);

  void set_gains(
    double p, double i, double d, double u_max, double u_min, double trk_tc, bool saturation,
    AntiwindupStrategy antiwindup_strat);

  void set_gains(const Gains & gains);

  [[deprecated("Use set_gains() instead")]] void setGains(const Gains & gains);

  [[nodiscard]] double compute_command(double error, const double & dt_s);

  [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
    double error, uint64_t dt);

  [[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);

  [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
    double error, double error_dot, uint64_t dt);

  [[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);

  [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd);

  double get_current_cmd();

  [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd();

  [[deprecated("Use get_current_pid_errors() instead")]] double getDerivativeError();

  void get_current_pid_errors(double & pe, double & ie, double & de);

  [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors(
    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_ = 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_