Fix TrustRegionSQPSolver solve method to correctly set status (#341)

tesseract-robotics · Jul 31, 2023 · c458814 · c458814
1 parent bfafddb
commit c458814
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 7 deletions.
diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h
@@ -209,11 +209,13 @@ struct SQPResults
  */
 enum class SQPStatus
 {
-  RUNNING,         /**< Optimization is currently running */
-  NLP_CONVERGED,   /**< NLP Successfully converged */
-  ITERATION_LIMIT, /**< SQP Optimization reached iteration limit */
-  QP_SOLVER_ERROR, /**< QP Solver failed */
-  CALLBACK_STOPPED /**< Optimization stopped because callback returned false */
+  RUNNING,                 /**< Optimization is currently running */
+  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 */
+  QP_SOLVER_ERROR,         /**< QP Solver failed */
+  CALLBACK_STOPPED         /**< Optimization stopped because callback returned false */
 };
 
 }  // namespace trajopt_sqp

diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
@@ -29,8 +29,9 @@
  * limitations under the License.
  */
 #include <trajopt_sqp/trust_region_sqp_solver.h>
-#include <iostream>
 #include <console_bridge/console.h>
+#include <iostream>
+#include <chrono>
 
 namespace trajopt_sqp
 {
@@ -76,6 +77,10 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
 {
   status_ = SQPStatus::RUNNING;
 
+  // Start time
+  using Clock = std::chrono::steady_clock;
+  auto start_time = Clock::now();
+
   // Initialize solver
   init(qp_problem);
 
@@ -88,28 +93,53 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
     // Convexification loop
     for (int convex_iteration = 0; convex_iteration < 100; convex_iteration++)
     {
-      if (stepSQPSolver())
+      double elapsed_time = std::chrono::duration<double, std::milli>(Clock::now() - start_time).count() / 1000.0;
+      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;
         break;
+      }
 
       if (results_.overall_iteration >= params.max_iterations)
       {
         CONSOLE_BRIDGE_logInform("Iteration limit");
         status_ = SQPStatus::ITERATION_LIMIT;
         break;
       }
+
+      if (stepSQPSolver())
+        break;
     }
 
     // Check if constraints are satisfied
     if (verifySQPSolverConvergence())
+    {
+      status_ = SQPStatus::NLP_CONVERGED;
+      break;
+    }
+
+    // If status is iteration limit or time limit we need to exit penalty iteration loop
+    if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::TIME_LIMIT)
       break;
 
+    // Set status to running
+    status_ = SQPStatus::RUNNING;
+
     // ---------------------------
     // Constraints are not satisfied!
     // Penalty Adjustment
     // ---------------------------
     adjustPenalty();
   }  // Penalty adjustment loop
 
+  // If status is still set to running the penalty iteration limit was reached
+  if (status_ == SQPStatus::RUNNING)
+  {
+    status_ = SQPStatus::PENALTY_ITERATION_LIMIT;
+    CONSOLE_BRIDGE_logInform("Penalty iteration limit, optimization couldn't satisfy all constraints");
+  }
+
   // Final Cleanup
   if (SUPER_DEBUG_MODE)
     results_.print();