acados · FreyJo · Aug 22, 2024 · Aug 21, 2024 · Aug 21, 2024 · Aug 21, 2024
diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c
@@ -129,7 +129,7 @@ void dense_qp_daqp_opts_set(void *config_, void *opts_, const char *field, void
     else if (!strcmp(field, "tol_comp"))
     {
         // Complementary slackness is implicitly
-        // handled by the worlking set
+        // handled by the working set
     }
     else if (!strcmp(field, "iter_max"))
     {

diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c
@@ -110,6 +110,8 @@ void dense_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *op
     d_dense_qp_ipm_arg_set_default(BALANCE, opts->hpipm_opts);
     dense_qp_hpipm_opts_overwrite_mode_opts(opts);
 
+    opts->print_level = 0;
+
     return;
 }
 
@@ -143,6 +145,11 @@ void dense_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void
         dense_qp_hpipm_opts_overwrite_mode_opts(opts);
 
     }
+    else if (!strcmp(field, "print_level"))
+    {
+        int* print_level = (int *) value;
+        opts->print_level = *print_level;
+    }
     else
     {
         d_dense_qp_ipm_arg_set((char *) field, value, opts->hpipm_opts);
@@ -282,6 +289,16 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void
     mem->time_qp_solver_call = info->solve_QP_time;
     mem->iter = mem->hpipm_workspace->iter;
 
+
+    // print HPIPM statistics:
+    if (opts->print_level > 0)
+    {
+        double *stat; d_dense_qp_ipm_get_stat(mem->hpipm_workspace, &stat);
+        int stat_m; d_dense_qp_ipm_get_stat_m(mem->hpipm_workspace, &stat_m);
+        printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+        d_print_exp_tran_mat(stat_m, mem->iter+1, stat, stat_m);
+    }
+
     // check exit conditions
     int acados_status = hpipm_status;
     if (hpipm_status == 0) acados_status = ACADOS_SUCCESS;

diff --git a/acados/dense_qp/dense_qp_hpipm.h b/acados/dense_qp/dense_qp_hpipm.h
@@ -49,6 +49,7 @@ extern "C" {
 typedef struct dense_qp_hpipm_opts_
 {
     struct d_dense_qp_ipm_arg *hpipm_opts;
+    int print_level;
 } dense_qp_hpipm_opts;
 
 

diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c
@@ -121,6 +121,7 @@ void ocp_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts
     d_ocp_qp_ipm_arg_set_default(BALANCE, opts->hpipm_opts);
 
     ocp_qp_hpipm_opts_overwrite_mode_opts(opts);
+    opts->print_level = 0;
 
     return;
 }
@@ -156,6 +157,11 @@ void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *
         ocp_qp_hpipm_opts_overwrite_mode_opts(opts);
 
     }
+    else if (!strcmp(field, "print_level"))
+    {
+        int* print_level = (int *) value;
+        opts->print_level = *print_level;
+    }
     else
     {
         d_ocp_qp_ipm_arg_set((char *) field, value, opts->hpipm_opts);
@@ -314,6 +320,16 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *
     mem->time_qp_solver_call = info->solve_QP_time;
     mem->iter = mem->hpipm_workspace->iter;
 
+    // print HPIPM statistics:
+    if (opts->print_level > 0)
+    {
+        double *stat; d_ocp_qp_ipm_get_stat(mem->hpipm_workspace, &stat);
+        int stat_m; d_ocp_qp_ipm_get_stat_m(mem->hpipm_workspace, &stat_m);
+        printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+        d_print_exp_tran_mat(stat_m, mem->iter+1, stat, stat_m);
+    }
+
+
     // check exit conditions
     int acados_status = mem->status;
     if (mem->status == 0) acados_status = ACADOS_SUCCESS;

diff --git a/acados/ocp_qp/ocp_qp_hpipm.h b/acados/ocp_qp/ocp_qp_hpipm.h
@@ -45,15 +45,14 @@ extern "C" {
 
 
 // struct of arguments to the solver
-// TODO(roversch): why not make this a typedef of the underlying struct?
 typedef struct ocp_qp_hpipm_opts_
 {
     struct d_ocp_qp_ipm_arg *hpipm_opts;
+    int print_level;
 } ocp_qp_hpipm_opts;
 
 
 
-// TODO(roversch): why not make this a typedef of the underlying struct?
 // struct of the solver memory
 typedef struct ocp_qp_hpipm_memory_
 {

diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py
@@ -1446,7 +1446,7 @@ def options_set(self, field_, value_):
         """
         Set options of the solver.
 
-            :param field: string, e.g. 'print_level', 'rti_phase', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'
+            :param field: string, e.g. 'print_level', 'rti_phase', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0', 'qp_print_level'
 
             :param value: of type int, float, string
 
@@ -1460,7 +1460,7 @@ def options_set(self, field_, value_):
             - rti_phase: 0: PREPARATION_AND_FEEDBACK, 1: PREPARATION, 2: FEEDBACK
         """
         int_fields = ['print_level', 'rti_phase', 'qp_warm_start',
-                      'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp', "as_rti_level", "max_iter"]
+                      'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp', "as_rti_level", "max_iter", "qp_print_level"]
         double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction',
                          'eps_sufficient_descent', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
         string_fields = ['globalization']

diff --git a/...ces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c b/...ces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
@@ -616,6 +616,17 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
         double qp_mu0 = (double) value[0];
         ocp_nlp_solver_opts_set(config, opts, "qp_mu0", &qp_mu0);
     }
+    else if (!strcmp(field, "qp_print_level"))
+    {
+        if (!(plan->ocp_qp_solver_plan.qp_solver == FULL_CONDENSING_HPIPM || plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM))
+        {
+            MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "qp_print_level", "HPIPM")
+        }
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int qp_print_level = (int) value[0];
+        ocp_nlp_solver_opts_set(config, opts, "qp_print_level", &qp_print_level);
+    }
     else if (!strcmp(field, "warm_start_first_qp"))
     {
         acados_size = 1;
@@ -637,7 +648,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
  constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z, \
  cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
  cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
- init_pi, nlp_solver_max_iter, qp_warm_start, qp_solver_mu0, warm_start_first_qp, print_level");
+ init_pi, nlp_solver_max_iter, qp_warm_start, qp_solver_mu0, qp_print_level, warm_start_first_qp, print_level");
     }
 
     return;