Skip to content

Commit

Permalink
C: move print_level to nlp_opts, improve printing
Browse files Browse the repository at this point in the history
  • Loading branch information
@FreyJo
FreyJo committed Jan 16, 2022
1 parent ccdebb9 commit abb1389
Show file tree
Hide file tree
Showing 5 changed files with 44 additions and 35 deletions.
28 changes: 24 additions & 4 deletions acados/ocp_nlp/ocp_nlp_common.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1075,6 +1075,7 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_)
// printf("\nocp_nlp: openmp threads = %d\n", opts->num_threads);

opts->globalization = FIXED_STEP;
opts->print_level = 0;
opts->step_length = 1.0;
opts->levenberg_marquardt = 0.0;

Expand Down Expand Up @@ -1291,6 +1292,16 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value
config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii],
"compute_hess", value);
}
else if (!strcmp(field, "print_level"))
{
int* print_level = (int *) value;
if (*print_level < 0)
{
printf("\nerror: ocp_nlp_opts_set: invalid value for print_level field, need int >=0, got %d.", *print_level);
exit(1);
}
opts->print_level = *print_level;
}
else
{
printf("\nerror: ocp_nlp_opts_set: wrong field: %s\n", field);
Expand Down Expand Up @@ -2392,7 +2403,8 @@ double ocp_nlp_compute_merit_gradient(ocp_nlp_config *config, ocp_nlp_dims *dims
// print_ocp_qp_in(mem->qp_in);

merit_grad = merit_grad_cost + merit_grad_dyn + merit_grad_ineq;
printf("merit_grad = %e, merit_grad_cost = %e, merit_grad_dyn = %e, merit_grad_ineq = %e\n", merit_grad, merit_grad_cost, merit_grad_dyn, merit_grad_ineq);
if (opts->print_level > 1)
printf("merit_grad = %e, merit_grad_cost = %e, merit_grad_dyn = %e, merit_grad_ineq = %e\n", merit_grad, merit_grad_cost, merit_grad_dyn, merit_grad_ineq);

return merit_grad;
}
Expand Down Expand Up @@ -2640,7 +2652,10 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
merit_fun1 = ocp_nlp_evaluate_merit_fun(config, dims, in, out, opts, mem, work);

double violation_step = ocp_nlp_get_violation(config, dims, in, out, opts, mem, work);
printf("preliminary line_search: merit0 %e, merit1 %e; viol_current %e, viol_step %e\n", merit_fun0, merit_fun1, violation_current, violation_step);
if (opts->print_level > 0)
{
printf("\npreliminary line_search: merit0 %e, merit1 %e; viol_current %e, viol_step %e\n", merit_fun0, merit_fun1, violation_current, violation_step);
}

if (merit_fun1 > merit_fun0 || violation_step > violation_current)
{
Expand All @@ -2660,8 +2675,10 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
dmerit_dy = ocp_nlp_compute_merit_gradient(config, dims, in, out, opts, mem, work);
if (dmerit_dy > 0.0)
{
if (dmerit_dy > 1e-6)
if (dmerit_dy > 1e-6 && opts->print_level > 0)
{
printf("\nacados line search: found dmerit_dy = %e > 0. Setting it to 0.0 instead", dmerit_dy);
}
dmerit_dy = 0.0;
}
}
Expand All @@ -2688,7 +2705,10 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
blasfeo_daxpy(nv[i], alpha, qp_out->ux+i, 0, out->ux+i, 0, work->tmp_nlp_out->ux+i, 0);

merit_fun1 = ocp_nlp_evaluate_merit_fun(config, dims, in, out, opts, mem, work);
// printf("\nbacktracking %d, merit_fun1 = %e, merit_fun0 %e", j, merit_fun1, merit_fun0);
if (opts->print_level > 1)
{
printf("\nbacktracking %d, merit_fun1 = %e, merit_fun0 %e", j, merit_fun1, merit_fun0);
}

// if (merit_fun1 < merit_fun0 && merit_fun1 > max_next_merit_fun_val)
// {
Expand Down
1 change: 1 addition & 0 deletions acados/ocp_nlp/ocp_nlp_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,6 +270,7 @@ typedef struct ocp_nlp_opts
double levenberg_marquardt; // LM factor to be added to the hessian before regularization
int reuse_workspace;
int num_threads;
int print_level;

// TODO: move to separate struct?
ocp_nlp_globalization_t globalization;
Expand Down
37 changes: 13 additions & 24 deletions acados/ocp_nlp/ocp_nlp_sqp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,6 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_
opts->qp_warm_start = 0;
opts->warm_start_first_qp = false;
opts->rti_phase = 0;
opts->print_level = 0;
opts->initialize_t_slacks = 0;

// overwrite default submodules opts
Expand Down Expand Up @@ -245,16 +244,6 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v
exit(1);
} else opts->rti_phase = *rti_phase;
}
else if (!strcmp(field, "print_level"))
{
int* print_level = (int *) value;
if (*print_level < 0)
{
printf("\nerror: ocp_nlp_sqp_opts_set: invalid value for print_level field, need int >=0, got %d.", *print_level);
exit(1);
}
opts->print_level = *print_level;
}
else if (!strcmp(field, "initialize_t_slacks"))
{
int* initialize_t_slacks = (int *) value;
Expand Down Expand Up @@ -555,7 +544,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_res, nlp_mem);
ocp_nlp_res_get_inf_norm(nlp_res, &nlp_out->inf_norm_res);

if (opts->print_level > sqp_iter + 1)
if (nlp_opts->print_level > sqp_iter + 1)
{
printf("\n\nSQP: ocp_qp_in at iteration %d\n", sqp_iter);
print_ocp_qp_in(qp_in);
Expand Down Expand Up @@ -584,7 +573,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
mem->sqp_iter = sqp_iter;
mem->time_tot = acados_toc(&timer0);

if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
{
printf("%i\t%e\t%e\t%e\t%e.\n", sqp_iter, nlp_res->inf_norm_res_stat,
nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq,
Expand Down Expand Up @@ -641,7 +630,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
"warm_start", &opts->qp_warm_start);
}

if (opts->print_level > sqp_iter + 1)
if (nlp_opts->print_level > sqp_iter + 1)
{
printf("\n\nSQP: ocp_qp_out at iteration %d\n", sqp_iter);
print_ocp_qp_out(qp_out);
Expand Down Expand Up @@ -679,7 +668,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
// exit conditions on QP status
if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
{
if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
{
printf("%i\t%e\t%e\t%e\t%e.\n", sqp_iter, nlp_res->inf_norm_res_stat,
nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq,
Expand All @@ -698,10 +687,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
omp_set_num_threads(num_threads_bkp);
#endif

if (opts->print_level > 1)
if (nlp_opts->print_level > 1)
{
printf("\n Failed to solve the following QP:\n");
if (opts->print_level)
if (nlp_opts->print_level)
print_ocp_qp_in(qp_in);
}

Expand Down Expand Up @@ -733,7 +722,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
// Paragraph: APPROACH III: S l1 QP (SEQUENTIAL l1 QUADRATIC PROGRAMMING),
// Section 18.8 TRUST-REGION SQP METHODS
// - just no trust region radius here.
if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
printf("ocp_nlp_sqp: performing SOC, since alpha %e in prelim. line search\n\n", alpha);
int *nb = qp_in->dim->nb;
int *ng = qp_in->dim->ng;
Expand Down Expand Up @@ -836,7 +825,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
// blasfeo_print_exp_dvec(2*nb[ii]+2*ng[ii], qp_in->d+ii, 0);
}

if (opts->print_level > sqp_iter + 1)
if (nlp_opts->print_level > sqp_iter + 1)
{
printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", sqp_iter);
print_ocp_qp_in(qp_in);
Expand Down Expand Up @@ -887,7 +876,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+7));
}

if (opts->print_level > sqp_iter + 1)
if (nlp_opts->print_level > sqp_iter + 1)
{
printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", sqp_iter);
print_ocp_qp_out(qp_out);
Expand All @@ -913,10 +902,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
omp_set_num_threads(num_threads_bkp);
#endif

if (opts->print_level > 1)
if (nlp_opts->print_level > 1)
{
printf("\n Failed to solve the following QP:\n");
if (opts->print_level > sqp_iter + 1)
if (nlp_opts->print_level > sqp_iter + 1)
print_ocp_qp_in(qp_in);
}

Expand All @@ -943,7 +932,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
// update variables
ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha);

if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
{
if (sqp_iter%10 == 0)
{
Expand All @@ -954,7 +943,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
}
} // end SQP loop

if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
printf("\n\n");

// ocp_nlp_out_print(dims, nlp_out);
Expand Down
5 changes: 2 additions & 3 deletions acados/ocp_nlp/ocp_nlp_sqp_rti.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,6 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_,
opts->ext_qp_res = 0;
opts->warm_start_first_qp = false;
opts->rti_phase = 0;
opts->print_level = 0;

// overwrite default submodules opts

Expand Down Expand Up @@ -591,7 +590,7 @@ void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_,
dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem);
mem->time_reg += acados_toc(&timer1);

if (opts->print_level > 0) {
if (nlp_opts->print_level > 0) {
printf("\n------- qp_in --------\n");
print_ocp_qp_in(nlp_mem->qp_in);
}
Expand Down Expand Up @@ -655,7 +654,7 @@ void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_,
printf("\nSQP_RTI: QP solver returned error status %d QP iteration %d.\n",
qp_status, qp_iter);
#endif
if (opts->print_level > 0)
if (nlp_opts->print_level > 0)
{
printf("\n Failed to solve the following QP:\n");
print_ocp_qp_in(nlp_mem->qp_in);
Expand Down
8 changes: 4 additions & 4 deletions acados/utils/print.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);

// ocp qp
// TODO: move printing routines below that print qp structures to HPIPM!
void print_ocp_qp_dims(ocp_qp_dims *dims);

// void print_dense_qp_dims(dense_qp_dims *dims);
Expand All @@ -81,20 +82,19 @@ void print_ocp_qp_out(ocp_qp_out *qp_out);

void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);

void print_ocp_qp_res(ocp_qp_res *qp_res);

void print_dense_qp_in(dense_qp_in *qp_in);
// void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);

// void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);

void print_ocp_qp_res(ocp_qp_res *qp_res);

// void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);

// void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);

// void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);

void print_dense_qp_in(dense_qp_in *qp_in);

void print_qp_info(qp_info *info);

// void acados_warning(char warning_string[]);
Expand Down

0 comments on commit abb1389

Please sign in to comment.