Remove deprecation of i_min/i_max PID parameters (#507)

Author: christophfroehlich

control_toolbox/include/control_toolbox/pid.hpp

@@ -82,8 +82,8 @@ struct AntiWindupStrategy
 
   AntiWindupStrategy()
   : type(UNDEFINED),
-    i_min(std::numeric_limits<double>::quiet_NaN()),
-    i_max(std::numeric_limits<double>::quiet_NaN()),
+    i_min(-std::numeric_limits<double>::infinity()),
+    i_max(std::numeric_limits<double>::infinity()),
     legacy_antiwindup(false),
     tracking_time_constant(0.0),
     error_deadband(std::numeric_limits<double>::epsilon())
@@ -135,20 +135,11 @@ struct AntiWindupStrategy
         "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant "
         "(tracking_time_constant)");
     }
-    if (type == LEGACY && ((i_min > i_max) || !std::isfinite(i_min) || !std::isfinite(i_max)))
+    if (i_min > i_max)
     {
       throw std::invalid_argument(
         fmt::format(
-          "AntiWindupStrategy 'legacy' requires i_min < i_max and to be finite (i_min: {}, i_max: "
-          "{})",
-          i_min, i_max));
-    }
-    if (type != LEGACY && (std::isfinite(i_min) || std::isfinite(i_max)))
-    {
-      std::cout << "Warning: The i_min and i_max are only valid for the deprecated LEGACY "
-                   "antiwindup strategy. Please use the AntiWindupStrategy::set_type() method to "
-                   "set the type of antiwindup strategy you want to use."
-                << std::endl;
+          "PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max));
     }
     if (
       type != NONE && type != UNDEFINED && type != LEGACY && type != BACK_CALCULATION &&
@@ -182,8 +173,8 @@ struct AntiWindupStrategy
   }
 
   Value type = UNDEFINED;
-  double i_min = std::numeric_limits<double>::quiet_NaN(); /**< Minimum allowable integral term. */
-  double i_max = std::numeric_limits<double>::quiet_NaN(); /**< Maximum allowable integral term. */
+  double i_min = -std::numeric_limits<double>::infinity(); /**< Minimum allowable integral term. */
+  double i_max = std::numeric_limits<double>::infinity();  /**< Maximum allowable integral term. */
 
   bool legacy_antiwindup = false; /**< Use legacy anti-windup strategy. */
 
@@ -430,8 +421,10 @@ class Pid
     double p_gain_ = 0.0; /**< Proportional gain. */
     double i_gain_ = 0.0; /**< Integral gain. */
     double d_gain_ = 0.0; /**< Derivative gain. */
-    double i_max_ = 0.0;  /**< Maximum allowable integral term. */
-    double i_min_ = 0.0;  /**< Minimum allowable integral term. */
+    double i_max_ =
+      std::numeric_limits<double>::infinity(); /**< Maximum allowable integral term. */
+    double i_min_ =
+      -std::numeric_limits<double>::infinity(); /**< Minimum allowable integral term. */
     double u_max_ = std::numeric_limits<double>::infinity();  /**< Maximum allowable output. */
     double u_min_ = -std::numeric_limits<double>::infinity(); /**< Minimum allowable output. */
     bool antiwindup_ = false;                                 /**< Anti-windup. */
@@ -456,8 +449,9 @@ class Pid
    */
   [[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);
+    double p = 0.0, double i = 0.0, double d = 0.0,
+    double i_max = std::numeric_limits<double>::infinity(),
+    double i_min = -std::numeric_limits<double>::infinity(), bool antiwindup = false);
 
   /*!
    * \brief Constructor, initialize Pid-gains and term limits.

control_toolbox/src/pid.cpp

@@ -431,6 +431,8 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
     }
   }
 
+  i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_);
+
   return cmd_;
 }
 

control_toolbox/src/pid_ros.cpp

@@ -40,7 +40,7 @@
 
 namespace control_toolbox
 {
-constexpr double UMAX_INFINITY = std::numeric_limits<double>::infinity();
+constexpr double MAX_INFINITY = std::numeric_limits<double>::infinity();
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
 PidROS::PidROS(
@@ -257,8 +257,8 @@ bool PidROS::initialize_from_ros_parameters()
   double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband;
   p = i = d = i_max = i_min = tracking_time_constant = std::numeric_limits<double>::quiet_NaN();
   error_deadband = std::numeric_limits<double>::epsilon();
-  u_max = UMAX_INFINITY;
-  u_min = -UMAX_INFINITY;
+  u_max = i_max = MAX_INFINITY;
+  u_min = i_min = -MAX_INFINITY;
   bool antiwindup = false;
   std::string antiwindup_strat_str = "legacy";
   bool all_params_available = true;
@@ -278,8 +278,8 @@ bool PidROS::initialize_from_ros_parameters()
   get_boolean_param(param_prefix_ + "saturation", saturation);
   if (!saturation)
   {
-    u_max = UMAX_INFINITY;
-    u_min = -UMAX_INFINITY;
+    u_max = MAX_INFINITY;
+    u_min = -MAX_INFINITY;
   }
   get_boolean_param(param_prefix_ + "antiwindup", antiwindup);
   get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str);
@@ -342,7 +342,7 @@ bool PidROS::initialize_from_args(
   antiwindup_strat.i_min = i_min;
   antiwindup_strat.legacy_antiwindup = antiwindup;
 
-  return initialize_from_args(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, false);
+  return initialize_from_args(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat, false);
 }
 #pragma GCC diagnostic pop
 
@@ -355,8 +355,7 @@ bool PidROS::initialize_from_args(
   antiwindup_strat.i_min = i_min;
   antiwindup_strat.legacy_antiwindup = antiwindup;
 
-  return initialize_from_args(
-    p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat, save_i_term);
+  return initialize_from_args(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat, save_i_term);
 }
 
 bool PidROS::initialize_from_args(
@@ -454,7 +453,7 @@ bool PidROS::set_gains(double p, double i, double d, double i_max, double i_min,
   antiwindup_strat.i_max = i_max;
   antiwindup_strat.i_min = i_min;
   antiwindup_strat.legacy_antiwindup = antiwindup;
-  return set_gains(p, i, d, UMAX_INFINITY, -UMAX_INFINITY, antiwindup_strat);
+  return set_gains(p, i, d, MAX_INFINITY, -MAX_INFINITY, antiwindup_strat);
 }
 
 bool PidROS::set_gains(
@@ -610,8 +609,8 @@ void PidROS::set_parameter_event_callback()
             RCLCPP_WARN(
               node_logging_->get_logger(),
               "Saturation is set to false, Changing the u_min and u_max to -inf and inf");
-            gains.u_min_ = -UMAX_INFINITY;
-            gains.u_max_ = UMAX_INFINITY;
+            gains.u_min_ = -MAX_INFINITY;
+            gains.u_max_ = MAX_INFINITY;
           }
           else
           {
@@ -659,15 +658,15 @@ void PidROS::set_parameter_event_callback()
         }
         else if (param_name == param_prefix_ + "u_clamp_max")
         {
-          gains.u_max_ = saturation ? parameter.get_value<double>() : UMAX_INFINITY;
+          gains.u_max_ = saturation ? parameter.get_value<double>() : MAX_INFINITY;
           RCLCPP_WARN_EXPRESSION(
             node_logging_->get_logger(), !saturation,
             "Saturation is set to false, Changing the u_clamp_max inf");
           changed = true;
         }
         else if (param_name == param_prefix_ + "u_clamp_min")
         {
-          gains.u_min_ = saturation ? parameter.get_value<double>() : -UMAX_INFINITY;
+          gains.u_min_ = saturation ? parameter.get_value<double>() : -MAX_INFINITY;
           RCLCPP_WARN_EXPRESSION(
             node_logging_->get_logger(), !saturation,
             "Saturation is set to false, Changing the u_clamp_min -inf");

control_toolbox/test/pid_ros_parameters_tests.cpp

@@ -178,8 +178,8 @@ TEST(PidParametersTest, InitPidTestBadParameter)
   ASSERT_EQ(gains.p_gain_, 0.0);
   ASSERT_EQ(gains.i_gain_, 0.0);
   ASSERT_EQ(gains.d_gain_, 0.0);
-  ASSERT_EQ(gains.i_max_, 0.0);
-  ASSERT_EQ(gains.i_min_, 0.0);
+  ASSERT_EQ(gains.i_max_, std::numeric_limits<double>::infinity());
+  ASSERT_EQ(gains.i_min_, -std::numeric_limits<double>::infinity());
   ASSERT_EQ(gains.u_max_, std::numeric_limits<double>::infinity());
   ASSERT_EQ(gains.u_min_, -std::numeric_limits<double>::infinity());
   ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0);
@@ -608,7 +608,7 @@ TEST(PidParametersTest, GetParametersFromParams)
   const bool ACTIVATE_STATE_PUBLISHER = false;
   TestablePidROS pid(node, "", "", ACTIVATE_STATE_PUBLISHER);
 
-  ASSERT_FALSE(pid.initialize_from_ros_parameters());
+  ASSERT_TRUE(pid.initialize_from_ros_parameters());
 
   rclcpp::Parameter param_p;
   ASSERT_TRUE(node->get_parameter("p", param_p));
@@ -624,11 +624,11 @@ TEST(PidParametersTest, GetParametersFromParams)
 
   rclcpp::Parameter param_i_clamp_max;
   ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max));
-  EXPECT_TRUE(std::isnan(param_i_clamp_max.get_value<double>()));
+  EXPECT_FALSE(std::isfinite(param_i_clamp_max.get_value<double>()));
 
   rclcpp::Parameter param_i_clamp_min;
   ASSERT_TRUE(node->get_parameter("i_clamp_min", param_i_clamp_min));
-  EXPECT_TRUE(std::isnan(param_i_clamp_min.get_value<double>()));
+  EXPECT_FALSE(std::isfinite(param_i_clamp_min.get_value<double>()));
 
   rclcpp::Parameter param_u_clamp_max;
   ASSERT_TRUE(node->get_parameter("u_clamp_max", param_u_clamp_max));

control_toolbox/test/pid_tests.cpp

@@ -587,7 +587,9 @@ TEST(CommandTest, proportionalOnlyTest)
     "only.");
 
   // Set only proportional gain
-  Pid pid(1.0, 0.0, 0.0, 0.0, 0.0);
+  Pid pid(
+    1.0, 0.0, 0.0, std::numeric_limits<double>::infinity(),
+    -std::numeric_limits<double>::infinity());
   double cmd = 0.0;
 
   // If initial error = 0, p-gain = 1, dt = 1
@@ -685,7 +687,9 @@ TEST(CommandTest, derivativeOnlyTest)
     "with own differentiation (ATTENTION: this test depends on the differentiation scheme).");
 
   // Set only derivative gain
-  Pid pid(0.0, 0.0, 1.0, 0.0, 0.0);
+  Pid pid(
+    0.0, 0.0, 1.0, std::numeric_limits<double>::infinity(),
+    -std::numeric_limits<double>::infinity());
   double cmd = 0.0;
 
   // If initial error = 0, d-gain = 1, dt = 1