8000 C: add QP solve function to simplify solver codes by FreyJo · Pull Request #1263 · acados/acados · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

C: add QP solve function to simplify solver codes #1263

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

Merged
merged 2 commits into from
Sep 26, 2024
Merged

C: add QP solve function to simplify solver codes #1263

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3e097eb
ocp_nlp_solve_qp_and_correct_dual
FreyJo Sep 26, 2024
c855fb7
comment cleanup
FreyJo Sep 26, 2024
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
43 changes: 43 additions & 0 deletions acados/ocp_nlp/ocp_nlp_common.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3027,6 +3027,49 @@ void ocp_nlp_common_eval_lagr_grad_p(ocp_nlp_config *config, ocp_nlp_dims *dims,
}


int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts,
ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, bool precondensed_lhs)
{
acados_timer timer;
ocp_qp_xcond_solver_config *qp_solver = config->qp_solver;

ocp_qp_in *qp_in = nlp_mem->qp_in;
ocp_qp_out *qp_out = nlp_mem->qp_out;
ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings;

double tmp_time;
int qp_status;

// solve qp
acados_tic(&timer);
if (precondensed_lhs)
{
qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);
}
else
{
qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out,
nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work);
}
// add qp timings
nlp_timings->time_qp_sol += acados_toc(&timer);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
nlp_timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
nlp_timings->time_qp_xcond += tmp_time;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer);
config->regularize->correct_dual_sol(config->regularize, dims->regularize,
nlp_opts->regularize, nlp_mem->regularize_mem);
nlp_timings->time_reg += acados_toc(&timer);

return qp_status;
}


void ocp_nlp_dump_qp_in_to_file(ocp_qp_in *qp_in, int sqp_iter, int soc)
{
Expand Down
5 changes: 4 additions & 1 deletion acados/ocp_nlp/ocp_nlp_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -485,7 +485,7 @@ void ocp_nlp_level_c_update(ocp_nlp_config *config,
ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_update_variables_sqp(void *config_, void *dims_,
void *in_, void *out_, void *opts_, void *mem_, void *work_,
void *in_, void *out_, void *opts_, void *mem_, void *work_,
void *out_destination_, void *solver_mem, double alpha, bool full_step_dual);
//
int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
Expand Down Expand Up @@ -519,6 +519,9 @@ void ocp_nlp_add_levenberg_marquardt_term(ocp_nlp_config *config, ocp_nlp_dims *
//
double ocp_nlp_get_l1_infeasibility(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_memory *nlp_mem);

int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts,
ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, bool precondensed_lhs);

// print / debug functionality
void ocp_nlp_dump_qp_out_to_file(ocp_qp_out *qp_out, int sqp_iter, int soc);
void ocp_nlp_dump_qp_in_to_file(ocp_qp_in *qp_in, int sqp_iter, int soc);
Expand Down
26 changes: 3 additions & 23 deletions acados/ocp_nlp/ocp_nlp_ddp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -648,7 +648,6 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
ocp_qp_out *qp_out = nlp_mem->qp_out;

// zero timers
double tmp_time;
ocp_nlp_timings_reset(nlp_timings);

int qp_status = 0;
Expand Down Expand Up @@ -767,42 +766,23 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
if (ddp_iter == 0 && !opts->warm_start_first_qp)
{
int tmp_int = 0;
config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts,
"warm_start", &tmp_int);
qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int);
}
// Show input to QP
// if (nlp_opts->print_level > ddp_iter + 1)
if (nlp_opts->print_level > 1)
{
printf("\n\nDDP: ocp_qp_in at iteration %d\n", ddp_iter + 1);
print_ocp_qp_in(qp_in);
}

// solve qp
acados_tic(&timer1);
qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out,
nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work);
nlp_timings->time_qp_sol += acados_toc(&timer1);

qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
nlp_timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
nlp_timings->time_qp_xcond += tmp_time;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize, dims->regularize,
nlp_opts->regularize, nlp_mem->regularize_mem);
nlp_timings->time_reg += acados_toc(&timer1);
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false);

// restore default warm start
if (ddp_iter==0)
{
config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts,
"warm_start", &opts->qp_warm_start);
qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &opts->qp_warm_start);
}

// if (nlp_opts->print_level > ddp_iter + 1)
if (nlp_opts->print_level > 1)
{
printf("\n\nDDP: ocp_qp_out at iteration %d\n", ddp_iter + 1);
Expand Down
24 changes: 3 additions & 21 deletions acados/ocp_nlp/ocp_nlp_sqp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -559,7 +559,6 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
ocp_qp_out *qp_out = nlp_mem->qp_out;

// zero timers
double tmp_time;
ocp_nlp_timings_reset(nlp_timings);

int qp_status = 0;
Expand Down Expand Up @@ -662,8 +661,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
if (sqp_iter == 0 && !opts->warm_start_first_qp)
{
int tmp_int = 0;
config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts,
"warm_start", &tmp_int);
qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int);
}
// Show input to QP
if (nlp_opts->print_level > 3)
Expand All @@ -675,28 +673,12 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
#if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE)
ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 0);
#endif
// solve qp
acados_tic(&timer1);
qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out,
nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work);
nlp_timings->time_qp_sol += acados_toc(&timer1);

qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
nlp_timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
nlp_timings->time_qp_xcond += tmp_time;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize, dims->regularize,
nlp_opts->regularize, nlp_mem->regularize_mem);
nlp_timings->time_reg += acados_toc(&timer1);
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false);

// restore default warm start
if (sqp_iter==0)
{
config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts,
"warm_start", &opts->qp_warm_start);
qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &opts->qp_warm_start);
}

if (nlp_opts->print_level > 3)
Expand Down
100 changes: 11 additions & 89 deletions acados/ocp_nlp/ocp_nlp_sqp_rti.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,7 +558,6 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *

int qp_iter = 0;
int qp_status;
double tmp_time;
int globalization_status = 1;

// update QP rhs for SQP (step prim var, abs dual var)
Expand Down Expand Up @@ -607,32 +606,12 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *
}

// solve QP
acados_tic(&timer1);
if (opts->rti_phase == FEEDBACK)
{
qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);
}
else if (opts->rti_phase == PREPARATION_AND_FEEDBACK)
bool precondensed_lhs = true;
if (opts->rti_phase == PREPARATION_AND_FEEDBACK)
{
qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);
precondensed_lhs = false;
}
// add qp timings
timings->time_qp_sol += acados_toc(&timer1);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
timings->time_qp_xcond += tmp_time;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize,
dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, precondensed_lhs);

qp_info *qp_info_;
ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_);
Expand Down Expand Up @@ -896,29 +875,14 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem);

// solve QP
acados_tic(&timer1);
qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);
timings->time_qp_sol += acados_toc(&timer1);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
timings->time_qp_xcond += tmp_time;
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true);

// save statistics
ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_);
qp_iter = qp_info_->num_iter;
mem->stat[mem->stat_n * nlp_mem->iter+0] = qp_status;
mem->stat[mem->stat_n * nlp_mem->iter+1] = qp_iter;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize,
dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);

if (nlp_opts->print_level > 0)
{
printf("\n------- qp_in AS-RTI-A preparation --------\n");
Expand Down Expand Up @@ -961,19 +925,9 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
config->regularize->regularize_rhs(config->regularize,
dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);

// QP solve
acados_tic(&timer1);
qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);

// add qp timings
timings->time_qp_sol += acados_toc(&timer1);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
timings->time_qp_xcond += tmp_time;
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true);

ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_);
qp_iter = qp_info_->num_iter;
Expand Down Expand Up @@ -1035,38 +989,21 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
rti_store_residuals_in_stats(opts, mem);
}


// rhs regularization
acados_tic(&timer1);
config->regularize->regularize_rhs(config->regularize,
dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);

// QP solve
acados_tic(&timer1);
qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);

// add qp timings
timings->time_qp_sol += acados_toc(&timer1);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
timings->time_qp_xcond += tmp_time;
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true);

ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_);
qp_iter = qp_info_->num_iter;

// save statistics
mem->stat[mem->stat_n * nlp_mem->iter+0] = qp_status;
mem->stat[mem->stat_n * nlp_mem->iter+1] = qp_iter;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize,
dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);
if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
{
#ifndef ACADOS_SILENT
Expand Down Expand Up @@ -1128,19 +1065,9 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
config->regularize->regularize(config->regularize,
dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);

// QP solve
acados_tic(&timer1);
qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver,
nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts,
nlp_mem->qp_solver_mem, nlp_work->qp_work);

// add qp timings
timings->time_qp_sol += acados_toc(&timer1);
// NOTE: timings within qp solver are added internally (lhs+rhs)
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time);
timings->time_qp_solver_call += tmp_time;
qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time);
timings->time_qp_xcond += tmp_time;
qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false);

ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_);
qp_iter = qp_info_->num_iter;
Expand All @@ -1149,11 +1076,6 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
mem->stat[mem->stat_n * nlp_mem->iter+0] = qp_status;
mem->stat[mem->stat_n * nlp_mem->iter+1] = qp_iter;

// compute correct dual solution in case of Hessian regularization
acados_tic(&timer1);
config->regularize->correct_dual_sol(config->regularize,
dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem);
timings->time_reg += acados_toc(&timer1);
if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
{
#ifndef ACADOS_SILENT
Expand Down
Loading
0