- Fix termination condition for the trust region loop to match trajop…

…t_sco and the original paper.

- Add initial_merit_error_coeff to match trajopt_sco.

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h

@@ -66,17 +66,18 @@ struct SQPParameters
   double trust_shrink_ratio = 0.1;
   /** @brief Trust region is expanded by this when it is expanded */
   double trust_expand_ratio = 1.5;
-  /** @brief Max number of times the trust region will be expanded */
-  int max_trust_region_expansions = 10;
 
   /** @brief Any constraint under this value is not considered a violation */
   double cnt_tolerance = 1e-4;
   /** @brief Max number of times the constraints will be inflated */
   double max_merit_coeff_increases = 5;
   /** @brief Constraints are scaled by this amount when inflated */
   double merit_coeff_increase_ratio = 10;
-  /** @brief Unused */
+  /** @brief Max time in seconds that the optimizer will run */
   double max_time = static_cast<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;
   /** @brief If true, only the constraints that are violated will be inflated */
   bool inflate_constraints_individually = true;
   /** @brief Initial size of the trust region */
@@ -107,7 +108,7 @@ struct SQPResults
     best_var_vals = Eigen::VectorXd::Zero(num_vars);
     new_var_vals = Eigen::VectorXd::Zero(num_vars);
     box_size = Eigen::VectorXd::Ones(num_vars);
-    merit_error_coeffs = Eigen::VectorXd::Constant(num_cnts, 10);
+    merit_error_coeffs = Eigen::VectorXd::Ones(num_cnts);
   }
   SQPResults() = default;
   /** @brief The lowest cost ever achieved */

trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

@@ -46,6 +46,8 @@ bool TrustRegionSQPSolver::init(QPProblem::Ptr qp_prob)
   // Initialize optimization parameters
   results_ = SQPResults(qp_problem->getNumNLPVars(), qp_problem->getNumNLPConstraints(), qp_problem->getNumNLPCosts());
   results_.best_var_vals = qp_problem->getVariableValues();
+  results_.merit_error_coeffs =
+      Eigen::VectorXd::Constant(qp_problem->getNumNLPConstraints(), params.initial_merit_error_coeff);
 
   // Evaluate exact constraint violations (expensive)
   results_.best_costs = qp_problem->getExactCosts();
@@ -222,14 +224,14 @@ bool TrustRegionSQPSolver::stepSQPSolver()
 
 void TrustRegionSQPSolver::runTrustRegionLoop()
 {
-  for (int trust_region_iteration = 0; trust_region_iteration < params.max_trust_region_expansions;
-       trust_region_iteration++)
+  results_.trust_region_iteration = 0;
+  while (results_.box_size.maxCoeff() >= params.min_trust_box_size)
   {
     if (SUPER_DEBUG_MODE)
       qp_problem->print();
 
     results_.overall_iteration++;
-    results_.trust_region_iteration = trust_region_iteration + 1;
+    results_.trust_region_iteration++;
 
     // Solve the current QP problem
     status_ = solveQPProblem();