Replace all float INF values casted to double by proper double infini…

…ty values
tesseract-robotics · Dec 5, 2023 · 9f7812a · 9f7812a
1 parent 2ee35aa
commit 9f7812a
Show file tree

Hide file tree

Showing 8 changed files with 40 additions and 38 deletions.
diff --git a/.clang-format b/.clang-format
@@ -53,15 +53,16 @@ BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
 BraceWrapping: {
-    AfterClass: 'true'
-    AfterControlStatement: 'true'
-    AfterEnum : 'true'
-    AfterFunction : 'true'
-    AfterNamespace : 'true'
-    AfterStruct : 'true'
-    AfterUnion : 'true'
-    BeforeCatch : 'true'
-    BeforeElse : 'true'
-    IndentBraces : 'false'
+    AfterCaseLabel: 'true',
+    AfterClass: 'true',
+    AfterControlStatement: 'true',
+    AfterEnum : 'true',
+    AfterFunction : 'true',
+    AfterNamespace : 'true',
+    AfterStruct : 'true',
+    AfterUnion : 'true',
+    BeforeCatch : 'true',
+    BeforeElse : 'true',
+    IndentBraces : 'false',
 }
 ...
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h
@@ -59,7 +59,7 @@ struct SQPParameters
   /** @brief NLP converges if approx_merit_improves is smaller than this */
   double min_approx_improve = 1e-4;
   /** @brief NLP converges if approx_merit_improve / best_exact_merit < min_approx_improve_frac */
-  double min_approx_improve_frac = static_cast<double>(-INFINITY);
+  double min_approx_improve_frac = -std::numeric_limits<double>::infinity();
   /** @brief Max number of QP calls allowed */
   int max_iterations = 50;
 
@@ -77,7 +77,7 @@ struct SQPParameters
   /** @brief Constraints are scaled by this amount when inflated */
   double merit_coeff_increase_ratio = 10;
   /** @brief Max time in seconds that the optimizer will run */
-  double max_time = static_cast<double>(INFINITY);
+  double max_time = std::numeric_limits<double>::infinity();
   /** @brief Initial coefficient that is used to scale the constraints. The total constaint cost is constaint_value
    * coeff * merit_coeff */
   double initial_merit_error_coeff = 10;

diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp
@@ -124,8 +124,8 @@ void IfoptQPProblem::setup()
   }
 
   // Initialize the constraint bounds
-  bounds_lower_ = Eigen::VectorXd::Constant(num_qp_cnts_, -double(INFINITY));
-  bounds_upper_ = Eigen::VectorXd::Constant(num_qp_cnts_, double(INFINITY));
+  bounds_lower_ = Eigen::VectorXd::Constant(num_qp_cnts_, -std::numeric_limits<double>::infinity());
+  bounds_upper_ = Eigen::VectorXd::Constant(num_qp_cnts_, std::numeric_limits<double>::infinity());
 }
 
 void IfoptQPProblem::setVariables(const double* x) { nlp_->SetVariables(x); }
@@ -405,16 +405,16 @@ void IfoptQPProblem::updateSlackVariableBounds()
     if (constraint_types_[static_cast<std::size_t>(i)] == ConstraintType::EQ)
     {
       bounds_lower_[current_cnt_index] = 0;
-      bounds_upper_[current_cnt_index] = double(INFINITY);
+      bounds_upper_[current_cnt_index] = std::numeric_limits<double>::infinity();
       bounds_lower_[current_cnt_index + 1] = 0;
-      bounds_upper_[current_cnt_index + 1] = double(INFINITY);
+      bounds_upper_[current_cnt_index + 1] = std::numeric_limits<double>::infinity();
 
       current_cnt_index += 2;
     }
     else
     {
       bounds_lower_[current_cnt_index] = 0;
-      bounds_upper_[current_cnt_index] = double(INFINITY);
+      bounds_upper_[current_cnt_index] = std::numeric_limits<double>::infinity();
 
       current_cnt_index++;
     }

diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp
@@ -275,21 +275,21 @@ bool OSQPEigenSolver::updateGradient(const Eigen::Ref<const Eigen::VectorXd>& gr
 
 bool OSQPEigenSolver::updateLowerBound(const Eigen::Ref<const Eigen::VectorXd>& lowerBound)
 {
-  bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -INFINITY);
+  bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -std::numeric_limits<double>::infinity());
   return solver_.updateLowerBound(bounds_lower_);
 }
 
 bool OSQPEigenSolver::updateUpperBound(const Eigen::Ref<const Eigen::VectorXd>& upperBound)
 {
-  bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * INFINITY);
+  bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * std::numeric_limits<double>::infinity());
   return solver_.updateUpperBound(bounds_upper_);
 }
 
 bool OSQPEigenSolver::updateBounds(const Eigen::Ref<const Eigen::VectorXd>& lowerBound,
                                    const Eigen::Ref<const Eigen::VectorXd>& upperBound)
 {
-  bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -INFINITY);
-  bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * INFINITY);
+  bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -std::numeric_limits<double>::infinity());
+  bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * std::numeric_limits<double>::infinity());
 
   if (solver_.isInitialized())
     return solver_.updateBounds(bounds_lower_, bounds_upper_);

diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp
@@ -196,8 +196,8 @@ void TrajOptQPProblem::setup()
   }
 
   // Initialize the constraint bounds
-  bounds_lower_ = Eigen::VectorXd::Constant(num_qp_cnts_, -double(INFINITY));
-  bounds_upper_ = Eigen::VectorXd::Constant(num_qp_cnts_, double(INFINITY));
+  bounds_lower_ = Eigen::VectorXd::Constant(num_qp_cnts_, -std::numeric_limits<double>::infinity());
+  bounds_upper_ = Eigen::VectorXd::Constant(num_qp_cnts_, std::numeric_limits<double>::infinity());
 
   // Set initialized
   initialized_ = true;
@@ -593,30 +593,30 @@ void TrajOptQPProblem::updateSlackVariableBounds()
   for (Eigen::Index i = 0; i < hinge_costs_.GetRows(); i++)
   {
     bounds_lower_[current_cnt_index] = 0;
-    bounds_upper_[current_cnt_index++] = double(INFINITY);
+    bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
   }
 
   for (Eigen::Index i = 0; i < abs_costs_.GetRows(); i++)
   {
     bounds_lower_[current_cnt_index] = 0;
-    bounds_upper_[current_cnt_index++] = double(INFINITY);
+    bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
     bounds_lower_[current_cnt_index] = 0;
-    bounds_upper_[current_cnt_index++] = double(INFINITY);
+    bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
   }
 
   for (Eigen::Index i = 0; i < getNumNLPConstraints(); i++)
   {
     if (constraint_types_[static_cast<std::size_t>(i)] == ConstraintType::EQ)
     {
       bounds_lower_[current_cnt_index] = 0;
-      bounds_upper_[current_cnt_index++] = double(INFINITY);
+      bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
       bounds_lower_[current_cnt_index] = 0;
-      bounds_upper_[current_cnt_index++] = double(INFINITY);
+      bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
     }
     else
     {
       bounds_lower_[current_cnt_index] = 0;
-      bounds_upper_[current_cnt_index++] = double(INFINITY);
+      bounds_upper_[current_cnt_index++] = std::numeric_limits<double>::infinity();
     }
   }
 }

diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp
@@ -90,7 +90,7 @@ struct BasicTrustRegionSQPParameters
   /** @brief If model improves less than this, exit and report convergence */
   double min_approx_improve = 1e-4;
   /** @brief If model improves less than this, exit and report convergence */
-  double min_approx_improve_frac = static_cast<double>(-INFINITY);
+  double min_approx_improve_frac = -std::numeric_limits<double>::infinity();
   /** @brief The max number of iterations */
   double max_iter = 50;
   /** @brief If improvement is less than improve_ratio_threshold, shrink trust region by this ratio */
@@ -106,7 +106,7 @@ struct BasicTrustRegionSQPParameters
   /** @brief Ratio that we increase coeff each time */
   double merit_coeff_increase_ratio = 10;
   /** @brief Max time in seconds that the optimizer will run */
-  double max_time = static_cast<double>(INFINITY);
+  double max_time = std::numeric_limits<double>::infinity();
   /** @brief Initial coefficient that is used to scale the constraints. The total constaint cost is constaint_value *
    * coeff * merit_coeff */
   double initial_merit_error_coeff = 10;

diff --git a/trajopt_sco/src/modeling.cpp b/trajopt_sco/src/modeling.cpp
@@ -18,7 +18,7 @@ void ConvexObjective::addAffExpr(const AffExpr& affexpr) { exprInc(quad_, affexp
 void ConvexObjective::addQuadExpr(const QuadExpr& quadexpr) { exprInc(quad_, quadexpr); }
 void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff)
 {
-  Var hinge = model_->addVar("hinge", 0, static_cast<double>(INFINITY));
+  Var hinge = model_->addVar("hinge", 0, std::numeric_limits<double>::infinity());
   vars_.push_back(hinge);
   ineqs_.push_back(affexpr);
   exprDec(ineqs_.back(), hinge);
@@ -29,8 +29,8 @@ void ConvexObjective::addHinge(const AffExpr& affexpr, double coeff)
 void ConvexObjective::addAbs(const AffExpr& affexpr, double coeff)
 {
   // Add variables that will enforce ABS
-  Var neg = model_->addVar("neg", 0, static_cast<double>(INFINITY));
-  Var pos = model_->addVar("pos", 0, static_cast<double>(INFINITY));
+  Var neg = model_->addVar("neg", 0, std::numeric_limits<double>::infinity());
+  Var pos = model_->addVar("pos", 0, std::numeric_limits<double>::infinity());
   vars_.push_back(neg);
   vars_.push_back(pos);
   // Coeff will be applied whenever neg/pos are not 0
@@ -75,7 +75,7 @@ void ConvexObjective::addL2Norm(const AffExprVector& ev)
 
 void ConvexObjective::addMax(const AffExprVector& ev)
 {
-  Var m = model_->addVar("max", static_cast<double>(-INFINITY), static_cast<double>(INFINITY));
+  Var m = model_->addVar("max", -std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
   ineqs_.reserve(ineqs_.size() + ev.size());
   for (const auto& i : ev)
   {
@@ -174,8 +174,9 @@ OptProb::OptProb(ModelType convex_solver, const ModelConfig::ConstPtr& convex_so
 }
 VarVector OptProb::createVariables(const std::vector<std::string>& names)
 {
-  return createVariables(
-      names, DblVec(names.size(), static_cast<double>(-INFINITY)), DblVec(names.size(), static_cast<double>(INFINITY)));
+  return createVariables(names,
+                         DblVec(names.size(), -std::numeric_limits<double>::infinity()),
+                         DblVec(names.size(), std::numeric_limits<double>::infinity()));
 }
 
 VarVector OptProb::createVariables(const std::vector<std::string>& names, const DblVec& lb, const DblVec& ub)

diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp
@@ -279,7 +279,7 @@ struct MultiCritFilter {
    */
   vector<DblVec> errvecs;
   double improvement(const DblVec& errvec) {
-    double leastImprovement=INFINITY;
+    double leastImprovement=std::numeric_limits<double>::infinity();
     for (const DblVec& olderrvec : errvecs) {
       double improvement=0;
       for (int i=0; i < errvec.size(); ++i) improvement += pospart(olderrvec[i] - errvec[i]);