From 3e0fe765a0fec0046c89d9c6968681072fe16eb0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 15:49:59 +0100 Subject: [PATCH 1/8] - 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) --- .../include/trajopt_sqp/qp_problem.h | 2 +- .../trajopt_sqp/include/trajopt_sqp/types.h | 2 ++ .../src/trust_region_sqp_solver.cpp | 35 ++++++++++++++++--- 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h index ca567269..ae964a81 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h +++ b/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 */ diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h index 37a705de..071fd18c 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h +++ b/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 */ diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index 630c823d..95681423 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -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(), From 6881f5b1cd3d33d1deca488c71aad93fe6454f4d Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 19:34:02 +0100 Subject: [PATCH 2/8] Re-add setVariables --- trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index 95681423..da65165e 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -383,6 +383,8 @@ SQPStatus TrustRegionSQPSolver::solveQPProblem() } else { + qp_problem->setVariables(results_.best_var_vals.data()); + CONSOLE_BRIDGE_logError("Solver Failure"); return SQPStatus::QP_SOLVER_ERROR; } From 78d4c575a1f16071924b9b110e312b31a33a7bf0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 4 Dec 2023 16:56:39 +0100 Subject: [PATCH 3/8] Update Trajopt debug output to match TrajoptIfopt (for comparison) --- trajopt_sco/src/optimizers.cpp | 65 +++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 29 deletions(-) diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index e22f7857..1eeb517d 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -360,99 +360,106 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results, void BasicTrustRegionSQPResults::print() const { // Print Header - std::printf("\n| %s |\n", std::string(75, '=').c_str()); - std::printf("| %s %s %s |\n", std::string(29, ' ').c_str(), "ROS Industrial", std::string(30, ' ').c_str()); - std::printf("| %s %s %s |\n", std::string(25, ' ').c_str(), "TrajOpt Motion Planning", std::string(25, ' ').c_str()); - std::printf("| %s |\n", std::string(75, '=').c_str()); + 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(31, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str()); + std::printf("| %s |\n", std::string(88, '=').c_str()); // Print Cost and Constraint Data - std::printf("| %10s | %10s | %10s | %10s | %10s | %10s | -%15s \n", + std::printf("| %10s | %10s | %10s | %10s | %10s | %10s | %10s |\n", "merit", "oldexact", "new_exact", + "new_approx", "dapprox", "dexact", - "ratio", - ""); - std::printf("| %s | COSTS\n", std::string(75, '-').c_str()); + "ratio"); + std::printf("| %s | COSTS\n", std::string(88, '-').c_str()); for (size_t i = 0; i < old_cost_vals.size(); ++i) { double approx_improve = old_cost_vals[i] - model_cost_vals[i]; double exact_improve = old_cost_vals[i] - new_cost_vals[i]; if (fabs(approx_improve) > 1e-8) - std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", + std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", "----------", old_cost_vals[i], new_cost_vals[i], + model_cost_vals[i], approx_improve, exact_improve, exact_improve / approx_improve, cost_names[i].c_str()); else - std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n", + std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n", "----------", old_cost_vals[i], new_cost_vals[i], + model_cost_vals[i], approx_improve, exact_improve, - " ------ ", + "----------", cost_names[i].c_str()); } - std::printf("| %s |\n", std::string(75, '=').c_str()); - std::printf("| %10s | %10.3e | %10.3e | %10s | %10s | %10s | SUM COSTS \n", + std::printf("| %s |\n", std::string(88, '=').c_str()); + std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM COSTS\n", "----------", vecSum(old_cost_vals), vecSum(new_cost_vals), - " ------ ", - " ------ ", - " ------ "); - std::printf("| %s |\n", std::string(75, '=').c_str()); + vecSum(model_cost_vals), + "----------", + "----------", + "----------"); + std::printf("| %s |\n", std::string(88, '=').c_str()); if (!cnt_names.empty()) { - std::printf("| %s | CONSTRAINTS\n", std::string(75, '-').c_str()); + std::printf("| %s | CONSTRAINTS\n", std::string(88, '-').c_str()); for (size_t i = 0; i < old_cnt_viols.size(); ++i) { double approx_improve = old_cnt_viols[i] - model_cnt_viols[i]; double exact_improve = old_cnt_viols[i] - new_cnt_viols[i]; if (fabs(approx_improve) > 1e-8) - std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", + std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n", merit_error_coeffs[i], merit_error_coeffs[i] * old_cnt_viols[i], merit_error_coeffs[i] * new_cnt_viols[i], + merit_error_coeffs[i] * model_cnt_viols[i], merit_error_coeffs[i] * approx_improve, merit_error_coeffs[i] * exact_improve, exact_improve / approx_improve, cnt_names[i].c_str()); else - std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n", + std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n", merit_error_coeffs[i], merit_error_coeffs[i] * old_cnt_viols[i], merit_error_coeffs[i] * new_cnt_viols[i], + merit_error_coeffs[i] * model_cnt_viols[i], merit_error_coeffs[i] * approx_improve, merit_error_coeffs[i] * exact_improve, - " ------ ", + "----------", cnt_names[i].c_str()); } } - std::printf("| %s |\n", std::string(75, '=').c_str()); - std::printf("| %10s | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT) \n", + std::printf("| %s |\n", std::string(88, '=').c_str()); + std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT) \n", "----------", vecSum(old_cnt_viols), vecSum(new_cnt_viols), - " ------ ", - " ------ ", - " ------ "); - std::printf("| %s |\n", std::string(75, '=').c_str()); - std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH " + vecSum(model_cnt_viols), + "----------", + "----------", + "----------"); + std::printf("| %s |\n", std::string(88, '=').c_str()); + std::printf("| %10s | %10.3e | %10.3e | %10s | %10.3e | %10.3e | %10.3e | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH " "MERIT)\n", "----------", old_merit, new_merit, + "----------", approx_merit_improve, exact_merit_improve, merit_improve_ratio); - std::printf("| %s |\n", std::string(75, '=').c_str()); + std::printf("| %s |\n", std::string(88, '=').c_str()); } void BasicTrustRegionSQPResults::writeSolver(std::FILE* stream, bool header) const From 4a16157a3e96cd04eb067734514ab81bd20a20b0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 5 Dec 2023 09:06:51 +0100 Subject: [PATCH 4/8] Change boundaries from OSQP_INFTY to INFINITY to match Trajopt --- trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index bc08e5af..6c22dccc 100644 --- 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& gr bool OSQPEigenSolver::updateLowerBound(const Eigen::Ref& lowerBound) { - bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -OSQP_INFTY); + bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -INFINITY); return solver_.updateLowerBound(bounds_lower_); } bool OSQPEigenSolver::updateUpperBound(const Eigen::Ref& upperBound) { - bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); + bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * INFINITY); return solver_.updateUpperBound(bounds_upper_); } bool OSQPEigenSolver::updateBounds(const Eigen::Ref& lowerBound, const Eigen::Ref& upperBound) { - bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -OSQP_INFTY); - bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); + bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -INFINITY); + bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * INFINITY); if (solver_.isInitialized()) return solver_.updateBounds(bounds_lower_, bounds_upper_); From d81bb48b0e083a191b9a8d7952b32b70048e82b2 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 5 Dec 2023 09:07:14 +0100 Subject: [PATCH 5/8] Output fixup --- trajopt_sco/src/optimizers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 1eeb517d..a47d3632 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -362,7 +362,7 @@ void BasicTrustRegionSQPResults::print() 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(31, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str()); + std::printf("| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str()); std::printf("| %s |\n", std::string(88, '=').c_str()); // Print Cost and Constraint Data From 7f40295dac811238cb5ed9ff981c5094e4a8d903 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 5 Dec 2023 09:09:38 +0100 Subject: [PATCH 6/8] Output fixup --- .../trajopt_sqp/src/trust_region_sqp_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index da65165e..34ea048e 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -98,7 +98,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) results_.convexify_iteration = 0; // Convexification loop - for (int convex_iteration = 0; convex_iteration < 100; convex_iteration++) + for (int convex_iteration = 1; convex_iteration < 100; convex_iteration++) { double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; if (elapsed_time > params.max_time) @@ -412,7 +412,7 @@ void TrustRegionSQPSolver::printStepInfo() const 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 Ifopt Motion Planning", std::string(31, ' ').c_str()); + "| %s %s %s |\n", std::string(28, ' ').c_str(), "TrajOpt Ifopt Motion Planning", std::string(29, ' ').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(), From 2ee35aaa66ee0703c64ea0490fc0d219d196388f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 5 Dec 2023 09:38:25 +0100 Subject: [PATCH 7/8] Match output of Trajopt --- trajopt_sco/src/osqp_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 04253864..72941ab1 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -343,8 +343,8 @@ CvxOptStatus OSQPModel::optimize() { Eigen::IOFormat format(5); Eigen::Map solution_vec(solution_.data(), static_cast(solution_.size())); - std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl; std::cout << "OSQP Status Value: " << status << std::endl; + std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl; } if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE) From 128eb371d366e00a17be21164626b75ea94a6ca4 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 5 Dec 2023 16:30:08 +0100 Subject: [PATCH 8/8] Revert "Change boundaries from OSQP_INFTY to INFINITY to match Trajopt" This reverts commit 4a16157a3e96cd04eb067734514ab81bd20a20b0. --- trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index 6c22dccc..bc08e5af 100644 --- 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& gr bool OSQPEigenSolver::updateLowerBound(const Eigen::Ref& lowerBound) { - bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -INFINITY); + bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -OSQP_INFTY); return solver_.updateLowerBound(bounds_lower_); } bool OSQPEigenSolver::updateUpperBound(const Eigen::Ref& upperBound) { - bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * INFINITY); + bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); return solver_.updateUpperBound(bounds_upper_); } bool OSQPEigenSolver::updateBounds(const Eigen::Ref& lowerBound, const Eigen::Ref& 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_) * -OSQP_INFTY); + bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); if (solver_.isInitialized()) return solver_.updateBounds(bounds_lower_, bounds_upper_);