Skip to content

Commit 713cc9d

Browse files
rjoomenrjoomen
authored
Handle solver failures like TrajOpt does (#369)
1 parent 89661ad commit 713cc9d

File tree

5 files changed

+73
-37
lines changed

5 files changed

+73
-37
lines changed

‎trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ class QPProblem
5656
/** @brief Set the current Optimization variables */
5757
virtual void setVariables(const double* x) = 0;
5858

59-
/** @brief Set the current Optimization variable values */
59+
/** @brief Get the current Optimization variable values */
6060
virtual Eigen::VectorXd getVariableValues() const = 0;
6161

6262
/** @brief Run the full convexification routine */

‎trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,8 @@ struct SQPParameters
7272
double cnt_tolerance = 1e-4;
7373
/** @brief Max number of times the constraints will be inflated */
7474
double max_merit_coeff_increases = 5;
75+
/** @brief Max number of times the QP solver can fail before optimization is aborted */
76+
int max_qp_solver_failures = 3;
7577
/** @brief Constraints are scaled by this amount when inflated */
7678
double merit_coeff_increase_ratio = 10;
7779
/** @brief Max time in seconds that the optimizer will run */

‎trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp‎

Lines changed: 33 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
9898
results_.convexify_iteration = 0;
9999

100100
// Convexification loop
101-
for (int convex_iteration = 0; convex_iteration < 100; convex_iteration++)
101+
for (int convex_iteration = 1; convex_iteration < 100; convex_iteration++)
102102
{
103103
double elapsed_time = std::chrono::duration<double, std::milli>(Clock::now() - start_time).count() / 1000.0;
104104
if (elapsed_time > params.max_time)
@@ -230,6 +230,7 @@ bool TrustRegionSQPSolver::stepSQPSolver()
230230
void TrustRegionSQPSolver::runTrustRegionLoop()
231231
{
232232
results_.trust_region_iteration = 0;
233+
int qp_solver_failures = 0;
233234
while (results_.box_size.maxCoeff() >= params.min_trust_box_size)
234235
{
235236
if (SUPER_DEBUG_MODE)
@@ -240,12 +241,34 @@ void TrustRegionSQPSolver::runTrustRegionLoop()
240241

241242
// Solve the current QP problem
242243
status_ = solveQPProblem();
244+
243245
if (status_ != SQPStatus::RUNNING)
244246
{
245-
qp_problem->setVariables(results_.best_var_vals.data());
246-
qp_problem->scaleBoxSize(params.trust_shrink_ratio);
247-
qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
248-
results_.box_size = qp_problem->getBoxSize();
247+
qp_solver_failures++;
248+
CONSOLE_BRIDGE_logWarn("Convex solver failed (%d/%d)!", qp_solver_failures, params.max_qp_solver_failures);
249+
250+
if (qp_solver_failures < params.max_qp_solver_failures)
251+
{
252+
qp_problem->scaleBoxSize(params.trust_shrink_ratio);
253+
qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
254+
results_.box_size = qp_problem->getBoxSize();
255+
256+
CONSOLE_BRIDGE_logInform("Shrunk trust region. New box size: %.4f", results_.box_size[0]);
257+
continue;
258+
}
259+
260+
if (qp_solver_failures == params.max_qp_solver_failures)
261+
{
262+
// Convex solver failed and this is the last attempt so setting the trust region to the minimum
263+
qp_problem->setBoxSize(Eigen::VectorXd::Constant(qp_problem->getNumNLPVars(), params.min_trust_box_size));
264+
qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
265+
results_.box_size = qp_problem->getBoxSize();
266+
267+
CONSOLE_BRIDGE_logInform("Shrunk trust region to minimum. New box size: %.4f", results_.box_size[0]);
268+
continue;
269+
}
270+
271+
CONSOLE_BRIDGE_logError("The convex solver failed you one too many times.");
249272
return;
250273
}
251274

@@ -303,6 +326,7 @@ void TrustRegionSQPSolver::runTrustRegionLoop()
303326
qp_problem->setVariables(results_.best_var_vals.data());
304327

305328
qp_problem->scaleBoxSize(params.trust_expand_ratio);
329+
qp_solver->updateBounds(qp_problem->getBoundsLower(), qp_problem->getBoundsUpper());
306330
results_.box_size = qp_problem->getBoxSize();
307331
CONSOLE_BRIDGE_logInform("Expanded trust region. new box size: %.4f", results_.box_size[0]);
308332
return;
@@ -359,6 +383,8 @@ SQPStatus TrustRegionSQPSolver::solveQPProblem()
359383
}
360384
else
361385
{
386+
qp_problem->setVariables(results_.best_var_vals.data());
387+
362388
CONSOLE_BRIDGE_logError("Solver Failure");
363389
return SQPStatus::QP_SOLVER_ERROR;
364390
}
@@ -385,7 +411,8 @@ void TrustRegionSQPSolver::printStepInfo() const
385411
// Print Header
386412
std::printf("\n| %s |\n", std::string(88, '=').c_str());
387413
std::printf("| %s %s %s |\n", std::string(36, ' ').c_str(), "ROS Industrial", std::string(36, ' ').c_str());
388-
std::printf("| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str());
414+
std::printf(
415+
"| %s %s %s |\n", std::string(28, ' ').c_str(), "TrajOpt Ifopt Motion Planning", std::string(29, ' ').c_str());
389416
std::printf("| %s |\n", std::string(88, '=').c_str());
390417
std::printf("| %s %s (Box Size: %-3.9f) %s |\n",
391418
std::string(26, ' ').c_str(),

‎trajopt_sco/src/optimizers.cpp‎

Lines changed: 36 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -360,99 +360,106 @@ void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
360360
void BasicTrustRegionSQPResults::print() const
361361
{
362362
// Print Header
363-
std::printf("\n| %s |\n", std::string(75, '=').c_str());
364-
std::printf("| %s %s %s |\n", std::string(29, ' ').c_str(), "ROS Industrial", std::string(30, ' ').c_str());
365-
std::printf("| %s %s %s |\n", std::string(25, ' ').c_str(), "TrajOpt Motion Planning", std::string(25, ' ').c_str());
366-
std::printf("| %s |\n", std::string(75, '=').c_str());
363+
std::printf("\n| %s |\n", std::string(88, '=').c_str());
364+
std::printf("| %s %s %s |\n", std::string(36, ' ').c_str(), "ROS Industrial", std::string(36, ' ').c_str());
365+
std::printf("| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str());
366+
std::printf("| %s |\n", std::string(88, '=').c_str());
367367

368368
// Print Cost and Constraint Data
369-
std::printf("| %10s | %10s | %10s | %10s | %10s | %10s | -%15s \n",
369+
std::printf("| %10s | %10s | %10s | %10s | %10s | %10s | %10s |\n",
370370
"merit",
371371
"oldexact",
372372
"new_exact",
373+
"new_approx",
373374
"dapprox",
374375
"dexact",
375-
"ratio",
376-
"");
377-
std::printf("| %s | COSTS\n", std::string(75, '-').c_str());
376+
"ratio");
377+
std::printf("| %s | COSTS\n", std::string(88, '-').c_str());
378378
for (size_t i = 0; i < old_cost_vals.size(); ++i)
379379
{
380380
double approx_improve = old_cost_vals[i] - model_cost_vals[i];
381381
double exact_improve = old_cost_vals[i] - new_cost_vals[i];
382382
if (fabs(approx_improve) > 1e-8)
383-
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
383+
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
384384
"----------",
385385
old_cost_vals[i],
386386
new_cost_vals[i],
387+
model_cost_vals[i],
387388
approx_improve,
388389
exact_improve,
389390
exact_improve / approx_improve,
390391
cost_names[i].c_str());
391392
else
392-
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
393+
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
393394
"----------",
394395
old_cost_vals[i],
395396
new_cost_vals[i],
397+
model_cost_vals[i],
396398
approx_improve,
397399
exact_improve,
398-
" ------ ",
400+
"----------",
399401
cost_names[i].c_str());
400402
}
401-
std::printf("| %s |\n", std::string(75, '=').c_str());
402-
std::printf("| %10s | %10.3e | %10.3e | %10s | %10s | %10s | SUM COSTS \n",
403+
std::printf("| %s |\n", std::string(88, '=').c_str());
404+
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM COSTS\n",
403405
"----------",
404406
vecSum(old_cost_vals),
405407
vecSum(new_cost_vals),
406-
" ------ ",
407-
" ------ ",
408-
" ------ ");
409-
std::printf("| %s |\n", std::string(75, '=').c_str());
408+
vecSum(model_cost_vals),
409+
"----------",
410+
"----------",
411+
"----------");
412+
std::printf("| %s |\n", std::string(88, '=').c_str());
410413

411414
if (!cnt_names.empty())
412415
{
413-
std::printf("| %s | CONSTRAINTS\n", std::string(75, '-').c_str());
416+
std::printf("| %s | CONSTRAINTS\n", std::string(88, '-').c_str());
414417
for (size_t i = 0; i < old_cnt_viols.size(); ++i)
415418
{
416419
double approx_improve = old_cnt_viols[i] - model_cnt_viols[i];
417420
double exact_improve = old_cnt_viols[i] - new_cnt_viols[i];
418421
if (fabs(approx_improve) > 1e-8)
419-
std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
422+
std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
420423
merit_error_coeffs[i],
421424
merit_error_coeffs[i] * old_cnt_viols[i],
422425
merit_error_coeffs[i] * new_cnt_viols[i],
426+
merit_error_coeffs[i] * model_cnt_viols[i],
423427
merit_error_coeffs[i] * approx_improve,
424428
merit_error_coeffs[i] * exact_improve,
425429
exact_improve / approx_improve,
426430
cnt_names[i].c_str());
427431
else
428-
std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
432+
std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
429433
merit_error_coeffs[i],
430434
merit_error_coeffs[i] * old_cnt_viols[i],
431435
merit_error_coeffs[i] * new_cnt_viols[i],
436+
merit_error_coeffs[i] * model_cnt_viols[i],
432437
merit_error_coeffs[i] * approx_improve,
433438
merit_error_coeffs[i] * exact_improve,
434-
" ------ ",
439+
"----------",
435440
cnt_names[i].c_str());
436441
}
437442
}
438-
std::printf("| %s |\n", std::string(75, '=').c_str());
439-
std::printf("| %10s | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT) \n",
443+
std::printf("| %s |\n", std::string(88, '=').c_str());
444+
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT) \n",
440445
"----------",
441446
vecSum(old_cnt_viols),
442447
vecSum(new_cnt_viols),
443-
" ------ ",
444-
" ------ ",
445-
" ------ ");
446-
std::printf("| %s |\n", std::string(75, '=').c_str());
447-
std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH "
448+
vecSum(model_cnt_viols),
449+
"----------",
450+
"----------",
451+
"----------");
452+
std::printf("| %s |\n", std::string(88, '=').c_str());
453+
std::printf("| %10s | %10.3e | %10.3e | %10s | %10.3e | %10.3e | %10.3e | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH "
448454
"MERIT)\n",
449455
"----------",
450456
old_merit,
451457
new_merit,
458+
"----------",
452459
approx_merit_improve,
453460
exact_merit_improve,
454461
merit_improve_ratio);
455-
std::printf("| %s |\n", std::string(75, '=').c_str());
462+
std::printf("| %s |\n", std::string(88, '=').c_str());
456463
}
457464

458465
void BasicTrustRegionSQPResults::writeSolver(std::FILE* stream, bool header) const

‎trajopt_sco/src/osqp_interface.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -343,8 +343,8 @@ CvxOptStatus OSQPModel::optimize()
343343
{
344344
Eigen::IOFormat format(5);
345345
Eigen::Map<Eigen::VectorXd> solution_vec(solution_.data(), static_cast<Eigen::Index>(solution_.size()));
346-
std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl;
347346
std::cout << "OSQP Status Value: " << status << std::endl;
347+
std::cout << "OSQP Solution: " << solution_vec.transpose().format(format) << std::endl;
348348
}
349349

350350
if (status == OSQP_SOLVED || status == OSQP_SOLVED_INACCURATE)

0 commit comments

Comments
 (0)