diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1a15fd4ee6..6663aecc8e 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -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; @@ -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); @@ -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; } @@ -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) { @@ -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; } } @@ -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) // { diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index a8ccfcb95c..4a7f19a07e 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -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; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 3f9c48f69d..d1db09503a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -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 @@ -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; @@ -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); @@ -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, @@ -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); @@ -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, @@ -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); } @@ -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; @@ -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); @@ -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); @@ -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); } @@ -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) { @@ -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); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index c5e2a65b05..b95eb170ca 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -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 @@ -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); } @@ -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); diff --git a/acados/utils/print.h b/acados/utils/print.h index 8579580af4..f8568afb26 100644 --- a/acados/utils/print.h +++ b/acados/utils/print.h @@ -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); @@ -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[]);