Skip to content

Commit

Permalink
Handle solver failures like TrajOpt does (#369)
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen authored Dec 6, 2023
1 parent 89661ad commit 713cc9d
Show file tree
Hide file tree
Showing 5 changed files with 73 additions and 37 deletions.
2 changes: 1 addition & 1 deletion trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
2 changes: 2 additions & 0 deletions trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
39 changes: 33 additions & 6 deletions trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, std::milli>(Clock::now() - start_time).count() / 1000.0;
if (elapsed_time > params.max_time)
Expand Down Expand Up @@ -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)
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand All @@ -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(),
Expand Down
65 changes: 36 additions & 29 deletions trajopt_sco/src/optimizers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion trajopt_sco/src/osqp_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -343,8 +343,8 @@ CvxOptStatus OSQPModel::optimize()
{
Eigen::IOFormat format(5);
Eigen::Map<Eigen::VectorXd> solution_vec(solution_.data(), static_cast<Eigen::Index>(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)
Expand Down

0 comments on commit 713cc9d

Please sign in to comment.