- Added max_qp_solver_failures parameter and the logic of TrajOpt to …

…handle solver failures

- Added updateBounds() call after each box size modification
- Removed setVariables() call after a solver failure
- Renamed SQPStatus::TIME_LIMIT to SQPStatus::OPT_TIME_LIMIT because of a name collision with OSQP (in tesseract_planning)

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h

@@ -56,7 +56,7 @@ class QPProblem
   /** @brief Set the current Optimization variables */
   virtual void setVariables(const double* x) = 0;
 
-  /** @brief Set the current Optimization variable values */
+  /** @brief Get the current Optimization variable values */
   virtual Eigen::VectorXd getVariableValues() const = 0;
 
   /** @brief Run the full convexification routine */

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h

@@ -72,6 +72,8 @@ struct SQPParameters
   double cnt_tolerance = 1e-4;
   /** @brief Max number of times the constraints will be inflated */
   double max_merit_coeff_increases = 5;
+  /** @brief Max number of times the QP solver can fail before optimization is aborted */
+  int max_qp_solver_failures = 3;
   /** @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 */
@@ -215,7 +217,7 @@ enum class SQPStatus
   NLP_CONVERGED,           /**< NLP Successfully converged */
   ITERATION_LIMIT,         /**< SQP Optimization reached iteration limit */
   PENALTY_ITERATION_LIMIT, /**< SQP Optimization reached penalty iteration limit */
-  TIME_LIMIT,              /**< SQP Optimization reached reached limit */
+  OPT_TIME_LIMIT,          /**< SQP Optimization reached reached limit */
   QP_SOLVER_ERROR,         /**< QP Solver failed */
   CALLBACK_STOPPED         /**< Optimization stopped because callback returned false */
 };

trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

@@ -104,7 +104,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
       if (elapsed_time > params.max_time)
       {
         CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time);
-        status_ = SQPStatus::TIME_LIMIT;
+        status_ = SQPStatus::OPT_TIME_LIMIT;
         break;
       }
 
@@ -127,7 +127,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
     }
 
     // If status is iteration limit or time limit we need to exit penalty iteration loop
-    if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::TIME_LIMIT)
+    if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::OPT_TIME_LIMIT)
       break;
 
     // Set status to running
@@ -230,6 +230,7 @@ bool TrustRegionSQPSolver::stepSQPSolver()
 void TrustRegionSQPSolver::runTrustRegionLoop()
 {
   results_.trust_region_iteration = 0;
+  int qp_solver_failures = 0;
   while (results_.box_size.maxCoeff() >= params.min_trust_box_size)
   {
     if (SUPER_DEBUG_MODE)
@@ -240,12 +241,34 @@ void TrustRegionSQPSolver::runTrustRegionLoop()
 
     // Solve the current QP problem
     status_ = solveQPProblem();
+
     if (status_ != SQPStatus::RUNNING)
     {
-      qp_problem->setVariables(results_.best_var_vals.data());
-      qp_problem->scaleBoxSize(params.trust_shrink_ratio);
-      qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
-      results_.box_size = qp_problem->getBoxSize();
+      qp_solver_failures++;
+      CONSOLE_BRIDGE_logWarn("Convex solver failed (%d/%d)!", qp_solver_failures, params.max_qp_solver_failures);
+
+      if (qp_solver_failures < params.max_qp_solver_failures)
+      {
+        qp_problem->scaleBoxSize(params.trust_shrink_ratio);
+        qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
+        results_.box_size = qp_problem->getBoxSize();
+
+        CONSOLE_BRIDGE_logInform("Shrunk trust region. New box size: %.4f", results_.box_size[0]);
+        continue;
+      }
+
+      if (qp_solver_failures == params.max_qp_solver_failures)
+      {
+        // Convex solver failed and this is the last attempt so setting the trust region to the minimum
+        qp_problem->setBoxSize(Eigen::VectorXd::Constant(qp_problem->getNumNLPVars(), params.min_trust_box_size));
+        qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
+        results_.box_size = qp_problem->getBoxSize();
+
+        CONSOLE_BRIDGE_logInform("Shrunk trust region to minimum. New box size: %.4f", results_.box_size[0]);
+        continue;
+      }
+
+      CONSOLE_BRIDGE_logError("The convex solver failed you one too many times.");
       return;
     }
 
@@ -303,6 +326,7 @@ void TrustRegionSQPSolver::runTrustRegionLoop()
       qp_problem->setVariables(results_.best_var_vals.data());
 
       qp_problem->scaleBoxSize(params.trust_expand_ratio);
+      qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
       results_.box_size = qp_problem->getBoxSize();
       CONSOLE_BRIDGE_logInform("Expanded trust region. new box size: %.4f", results_.box_size[0]);
       return;
@@ -385,7 +409,8 @@ void TrustRegionSQPSolver::printStepInfo() const
   // Print Header
   std::printf("\n| %s |\n", std::string(88, '=').c_str());
   std::printf("| %s %s %s |\n", std::string(36, ' ').c_str(), "ROS Industrial", std::string(36, ' ').c_str());
-  std::printf("| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str());
+  std::printf(
+      "| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Ifopt Motion Planning", std::string(31, ' ').c_str());
   std::printf("| %s |\n", std::string(88, '=').c_str());
   std::printf("| %s %s (Box Size: %-3.9f) %s |\n",
               std::string(26, ' ').c_str(),