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..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) @@ -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; @@ -359,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; } @@ -385,7 +411,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(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(), diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index e22f7857..a47d3632 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(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 - 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 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)