Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Handle solver failures like TrajOpt does #369

Merged
merged 8 commits into from
Dec 6, 2023
Merged

Handle solver failures like TrajOpt does #369

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3e0fe76
- Added max_qp_solver_failures parameter and the logic of TrajOpt to …
rjoomen Nov 28, 2023
6881f5b
Re-add setVariables
rjoomen Nov 28, 2023
78d4c57
Update Trajopt debug output to match TrajoptIfopt (for comparison)
rjoomen Dec 4, 2023
4a16157
Change boundaries from OSQP_INFTY to INFINITY to match Trajopt
rjoomen Dec 5, 2023
d81bb48
Output fixup
rjoomen Dec 5, 2023
7f40295
Output fixup
rjoomen Dec 5, 2023
2ee35aa
Match output of Trajopt
rjoomen Dec 5, 2023
128eb37
Revert "Change boundaries from OSQP_INFTY to INFINITY to match Trajopt"
rjoomen Dec 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This just needs to be added back.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why here exactly? Wouldn't adding it to solveQPProblem() upon failure (in the else block) be more logical?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think that would work since it would need to be set before convexify is called in stepSQPSolver. I think we might be able to move into that method but I would want to do a lot of testing to make sure everything aligns with the legacy version before making a change like this.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this equivalent? In the previous version solveQPProblem() is called, and if qp_solver->solve() failed and hence solveQPProblem() failed setVariables is called from withing runTrustRegionLoop(). Now I moved this line to solveQPProblem(), still to be called if qp_solver->solve() failed. It's just clearer now, as solveQPProblem() either sets the variables to new_var_vals and back to best_var_vals if succeeded and immediately to best_var_vals if failed. See line 386 in my PR.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I know the numerical ik has difference on Jammy because on one interaction it returns iteration limit instead of approximate solution but focal produces identical results. I am currently running focal so let me know if you want me to compare since I think you are running jammy.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Aha, I'm running in that issue exactly! But I've also access to focal (I'm using WSL), so I'll run it there tomorrow.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From what I could tell both hit the iteration limit but because of slight numerical differences focal picks approximate solution and jammy pick iteration limit.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Mmm, I'm seeing other differences on Focal. And Focal and Jammy do not match. Would you mind taking a look?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will take a look

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
Open in desktop
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
Open in desktop
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
Loading