From 9502162d5ca110de9a2dc460156c446ec0d6c626 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Tue, 13 Apr 2021 17:24:52 -0600 Subject: [PATCH] Shrink trust region if convex solver fails --- trajopt_sco/include/trajopt_sco/optimizers.hpp | 2 ++ trajopt_sco/src/optimizers.cpp | 10 ++++++++++ 2 files changed, 12 insertions(+) diff --git a/trajopt_sco/include/trajopt_sco/optimizers.hpp b/trajopt_sco/include/trajopt_sco/optimizers.hpp index 48f2b974..b504b9ae 100644 --- a/trajopt_sco/include/trajopt_sco/optimizers.hpp +++ b/trajopt_sco/include/trajopt_sco/optimizers.hpp @@ -100,6 +100,8 @@ struct BasicTrustRegionSQPParameters // constraint violation is less than this, we're done /** @brief Max number of times that the constraints' cost will be increased */ double max_merit_coeff_increases; + /** @brief Max number of times the QP solver can fail before optimization is aborted*/ + int max_qp_solver_failures; double merit_coeff_increase_ratio; // ratio that we increate coeff each time /** @brief Max time in seconds that the optimizer will run*/ diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 0ae619c9..e11a30a0 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -174,6 +174,7 @@ BasicTrustRegionSQPParameters::BasicTrustRegionSQPParameters() trust_expand_ratio = 1.5; cnt_tolerance = 1e-4; max_merit_coeff_increases = 5; + max_qp_solver_failures = 3; merit_coeff_increase_ratio = 10; max_time = static_cast(INFINITY); initial_merit_error_coeff = 10; @@ -588,6 +589,7 @@ OptStatus BasicTrustRegionSQP::optimize() using Clock = std::chrono::high_resolution_clock; auto start_time = Clock::now(); + int qp_solver_failures = 0; for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases) { /* merit adjustment loop */ for (int iter = 1;; ++iter) @@ -666,6 +668,14 @@ OptStatus BasicTrustRegionSQP::optimize() "/tmp/fail.ilp"); model_->writeToFile("/tmp/fail.lp"); model_->writeToFile("/tmp/fail.ilp"); + if (qp_solver_failures < param_.max_qp_solver_failures) + { + adjustTrustRegion(param_.trust_shrink_ratio); + LOG_INFO("shrunk trust region. new box size: %.4f", param_.trust_box_size); + qp_solver_failures++; + break; + } + LOG_ERROR("The convex solver failed you one too many times."); retval = OPT_FAILED; goto cleanup; }