acados · FreyJo · Feb 11, 2025 · Nov 26, 2024 · Nov 26, 2024 · Nov 26, 2024
diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c
@@ -1163,6 +1163,7 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_)
     opts->with_adaptive_levenberg_marquardt = false;
 
     opts->ext_qp_res = 0;
+    opts->qp_warm_start = 0;
     opts->store_iterates = false;
 
     return;
@@ -1223,6 +1224,11 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value
     {
         config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts,
                                     field+module_length+1, value);
+        if (!strcmp(field, "qp_iter_max"))
+        {
+            int* qp_iter_max = (int *) value;
+            opts->qp_iter_max = *qp_iter_max;
+        }
     }
     else if ( ptr_module!=NULL && (!strcmp(ptr_module, "reg")) )
     {
@@ -1290,6 +1296,11 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value
             double* adaptive_levenberg_marquardt_mu0 = (double *) value;
             opts->adaptive_levenberg_marquardt_mu0 = *adaptive_levenberg_marquardt_mu0;
         }
+        else if (!strcmp(field, "solution_sens_qp_t_lam_min"))
+        {
+            double* solution_sens_qp_t_lam_min = (double *) value;
+            opts->solution_sens_qp_t_lam_min = *solution_sens_qp_t_lam_min;
+        }
         else if (!strcmp(field, "exact_hess"))
         {
             int N = config->N;
@@ -3229,6 +3240,76 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in
 
 
 
+int ocp_nlp_common_setup_qp_matrices_and_factorize(ocp_nlp_config *config, ocp_nlp_dims *dims_, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out,
+                ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work)
+{
+    acados_timer timer0, timer1;
+    acados_tic(&timer0);
+
+    ocp_nlp_dims *dims = dims_;
+    ocp_qp_xcond_solver_config *qp_solver = config->qp_solver;
+    ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings;
+    ocp_nlp_timings_reset(nlp_timings);
+
+    ocp_qp_in *qp_in = nlp_mem->qp_in;
+    ocp_qp_out *qp_out = nlp_mem->qp_out;
+
+    ocp_nlp_initialize_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work);
+
+    int qp_status, tmp_int;
+
+    /* Prepare the QP data */
+    // linearize NLP and update QP matrices
+    acados_tic(&timer1);
+    ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work);
+    // update QP rhs for SQP (step prim var, abs dual var)
+    ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work);
+    nlp_timings->time_lin = acados_toc(&timer1);
+
+    /* solve QP */
+    // warm start QP
+    ocp_nlp_initialize_qp_from_nlp(config, dims, qp_in, nlp_out, qp_out);
+    int tmp_bool = true;
+    qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "initialize_next_xcond_qp_from_qp_out", &tmp_bool);
+    // HPIPM hot start
+    tmp_int = 3;
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int);
+    // HPIPM: iter_max 0
+    tmp_int = 0;
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "iter_max", &tmp_int);
+    // require new factorization at exit
+    tmp_int = 1;
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "update_fact_exit", &tmp_int);
+    // HPIPM: set t_min, lam_min to avoid ill-conditioning
+    double tmp_double = nlp_opts->solution_sens_qp_t_lam_min;
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "t_min", &tmp_double);
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "lam_min", &tmp_double);
+
+    // QP solve
+    qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL);
+
+    // reset QP solver settings
+    qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start);
+    qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "iter_max", &nlp_opts->qp_iter_max);
+    tmp_double = 1e-16; // TODO: fix this if t_min, lam_min defaults change in HPIPM, or if they are exposed in acados high level interfaces.
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "t_min", &tmp_double);
+    config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "lam_min", &tmp_double);
+
+    if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER))
+    {
+        nlp_mem->status = ACADOS_QP_FAILURE;
+    }
+    else
+    {
+        nlp_mem->status = ACADOS_SUCCESS;
+    }
+
+    nlp_timings->time_tot = acados_toc(&timer0);
+
+    return nlp_mem->status;
+}
+
+
 void ocp_nlp_params_jac_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work)
 {
     // This function sets up: jac_lag_stat_p_global, jac_ineq_p_global, jac_dyn_p_global
@@ -3714,6 +3795,11 @@ void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const c
         config->qp_solver->memory_get(config->qp_solver,
             nlp_mem->qp_solver_mem, "status", return_value_);
     }
+    else if (!strcmp("qp_tau_iter", field))
+    {
+        config->qp_solver->memory_get(config->qp_solver,
+            nlp_mem->qp_solver_mem, "tau_iter", return_value_);
+    }
     else if (!strcmp("res_stat", field))
     {
         double *value = return_value_;

diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h
@@ -87,6 +87,8 @@ typedef struct ocp_nlp_config
     void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
     // evaluate solver // TODO rename into solve
     int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    int (*setup_qp_matrices_and_factorize)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+
     void (*eval_kkt_residual)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
     void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
                             char *field, int stage, int index, void *sens_nlp_out);
@@ -293,6 +295,7 @@ typedef struct ocp_nlp_opts
     int fixed_hess;
     int log_primal_step_norm; // compute and log the max norm of the primal steps
     int max_iter; // maximum number of (SQP/DDP) iterations
+    int qp_iter_max; // maximum iter of QP solver, stored to remember.
 
     // Flag for usage of adaptive levenberg marquardt strategy
     bool with_adaptive_levenberg_marquardt;
@@ -302,8 +305,10 @@ typedef struct ocp_nlp_opts
 
     int with_solution_sens_wrt_params;
     int with_value_sens_wrt_params;
+    double solution_sens_qp_t_lam_min;
 
     int ext_qp_res;
+    int qp_warm_start;
 
     bool store_iterates; // flag indicating whether intermediate iterates should be stored
 
@@ -536,7 +541,10 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in
 //
 void ocp_nlp_get_cost_value_from_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
-
+//
+int ocp_nlp_common_setup_qp_matrices_and_factorize(ocp_nlp_config *config, ocp_nlp_dims *dims_, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out,
+                ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work);
+//
 void ocp_nlp_params_jac_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
 
 void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims,

diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c
@@ -119,7 +119,6 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_
 
     opts->ext_qp_res = 0;
 
-    opts->qp_warm_start = 0;
     opts->warm_start_first_qp = false;
     opts->warm_start_first_qp_from_nlp = false;
     opts->eval_residual_at_max_iter = false;
@@ -165,12 +164,6 @@ void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* v
     if ( ptr_module!=NULL && (!strcmp(ptr_module, "qp")) )
     {
         ocp_nlp_opts_set(config, nlp_opts, field, value);
-
-        if (!strcmp(field, "qp_warm_start"))
-        {
-            int* i_ptr = (int *) value;
-            opts->qp_warm_start = *i_ptr;
-        }
     }
     else // nlp opts
     {
@@ -607,6 +600,17 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me
  * functions
  ************************************************/
 
+ int ocp_nlp_ddp_setup_qp_matrices_and_factorize(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_)
+{
+    ocp_nlp_ddp_opts *opts = opts_;
+    ocp_nlp_ddp_memory *mem = mem_;
+    ocp_nlp_ddp_workspace *work = work_;
+
+    return ocp_nlp_common_setup_qp_matrices_and_factorize(config_, dims_, nlp_in_, nlp_out_, opts->nlp_opts, mem->nlp_mem, work->nlp_work);
+}
+
+
 // MAIN OPTIMIZATION ROUTINE
 int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
                 void *opts_, void *mem_, void *work_)
@@ -764,6 +768,8 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
             }
             else if (opts->warm_start_first_qp_from_nlp)
             {
+                int tmp_bool = true;
+                qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "initialize_next_xcond_qp_from_qp_out", &tmp_bool);
                 ocp_nlp_initialize_qp_from_nlp(config, dims, qp_in, nlp_out, qp_out);
             }
         }
@@ -779,7 +785,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
         // restore default warm start
         if (ddp_iter==0)
         {
-            qp_solver->opts_set(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", &nlp_opts->qp_warm_start);
         }
 
         if (nlp_opts->print_level > 1)
@@ -1148,6 +1154,7 @@ void ocp_nlp_ddp_config_initialize_default(void *config_)
     config->memory_assign = &ocp_nlp_ddp_memory_assign;
     config->workspace_calculate_size = &ocp_nlp_ddp_workspace_calculate_size;
     config->evaluate = &ocp_nlp_ddp;
+    config->setup_qp_matrices_and_factorize = &ocp_nlp_ddp_setup_qp_matrices_and_factorize;
     config->memory_reset_qp_solver = &ocp_nlp_ddp_memory_reset_qp_solver;
     config->eval_param_sens = &ocp_nlp_ddp_eval_param_sens;
     config->eval_lagr_grad_p = &ocp_nlp_ddp_eval_lagr_grad_p;

diff --git a/acados/ocp_nlp/ocp_nlp_ddp.h b/acados/ocp_nlp/ocp_nlp_ddp.h
@@ -62,7 +62,6 @@ typedef struct
     double tol_comp;     // exit tolerance on complementarity condition
     double tol_zero_res; // exit tolerance if objective function is 0 for least-squares problem
     int ext_qp_res;      // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
-    int qp_warm_start;   // qp_warm_start in all but the first ddp iterations
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     bool warm_start_first_qp_from_nlp;
     bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached

diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c
@@ -121,7 +121,6 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_
     opts->tol_unbounded = -1e10;
     opts->tol_min_step_norm = 1e-12;
 
-    opts->qp_warm_start = 0;
     opts->warm_start_first_qp = false;
     opts->eval_residual_at_max_iter = false;
 
@@ -169,12 +168,6 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v
     if ( ptr_module!=NULL && (!strcmp(ptr_module, "qp")) )
     {
         ocp_nlp_opts_set(config, nlp_opts, field, value);
-
-        if (!strcmp(field, "qp_warm_start"))
-        {
-            int* i_ptr = (int *) value;
-            opts->qp_warm_start = *i_ptr;
-        }
     }
     else // nlp opts
     {
@@ -740,6 +733,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
             }
             else if (opts->warm_start_first_qp_from_nlp)
             {
+                int tmp_bool = true;
+                qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "initialize_next_xcond_qp_from_qp_out", &tmp_bool);
                 ocp_nlp_initialize_qp_from_nlp(config, dims, qp_in, nlp_out, qp_out);
             }
         }
@@ -758,7 +753,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
         // restore default warm start
         if (sqp_iter==0)
         {
-            qp_solver->opts_set(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", &nlp_opts->qp_warm_start);
         }
 
         if (nlp_opts->print_level > 3)
@@ -877,6 +872,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
 }
 
 
+int ocp_nlp_sqp_setup_qp_matrices_and_factorize(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_)
+{
+    ocp_nlp_sqp_opts *opts = opts_;
+    ocp_nlp_sqp_memory *mem = mem_;
+    ocp_nlp_sqp_workspace *work = work_;
+
+    return ocp_nlp_common_setup_qp_matrices_and_factorize(config_, dims_, nlp_in_, nlp_out_, opts->nlp_opts, mem->nlp_mem, work->nlp_work);
+}
+
+
 void ocp_nlp_sqp_eval_kkt_residual(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
                 void *opts_, void *mem_, void *work_)
 {
@@ -1100,6 +1106,7 @@ void ocp_nlp_sqp_config_initialize_default(void *config_)
     config->memory_assign = &ocp_nlp_sqp_memory_assign;
     config->workspace_calculate_size = &ocp_nlp_sqp_workspace_calculate_size;
     config->evaluate = &ocp_nlp_sqp;
+    config->setup_qp_matrices_and_factorize = &ocp_nlp_sqp_setup_qp_matrices_and_factorize;
     config->memory_reset_qp_solver = &ocp_nlp_sqp_memory_reset_qp_solver;
     config->eval_param_sens = &ocp_nlp_sqp_eval_param_sens;
     config->eval_lagr_grad_p = &ocp_nlp_sqp_eval_lagr_grad_p;

diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h
@@ -64,7 +64,6 @@ typedef struct
     double tol_min_step_norm; // exit tolerance for small step
     int ext_qp_res;      // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
     int log_primal_step_norm; // compute and log the max norm of the primal steps
-    int qp_warm_
10000
start;   // qp_warm_start in all but the first sqp iterations
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     bool warm_start_first_qp_from_nlp;  // if True first QP will be initialized using values from NLP iterate, otherwise from previous QP solution.
     bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached

diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c
@@ -158,12 +158,6 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_,
     if ( ptr_module!=NULL && (!strcmp(ptr_module, "qp")) )
     {
         ocp_nlp_opts_set(config, nlp_opts, field, value);
-
-        if (!strcmp(field, "qp_warm_start"))
-        {
-            int* i_ptr = (int *) value;
-            opts->qp_warm_start = *i_ptr;
-        }
     }
     else // nlp opts
     {
@@ -607,6 +601,8 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *
         }
         else if (opts->warm_start_first_qp_from_nlp)
         {
+            int tmp_bool = true;
+            config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "initialize_next_xcond_qp_from_qp_out", &tmp_bool);
             ocp_nlp_initialize_qp_from_nlp(config, dims, nlp_mem->qp_in, nlp_out, nlp_mem->qp_out);
         }
     }
@@ -626,7 +622,7 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *
     // restore default warm start
     if (mem->is_first_call)
     {
-        config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "warm_start", &opts->qp_warm_start);
+        config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start);
     }
 
     // compute external QP residuals (for debugging)
@@ -1146,6 +1142,17 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
     return;
 }
 
+int ocp_nlp_sqp_rti_setup_qp_matrices_and_factorize(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_)
+{
+    ocp_nlp_sqp_rti_opts *opts = opts_;
+    ocp_nlp_sqp_rti_memory *mem = mem_;
+    ocp_nlp_sqp_rti_workspace *work = work_;
+
+    return ocp_nlp_common_setup_qp_matrices_and_factorize(config_, dims_, nlp_in_, nlp_out_, opts->nlp_opts, mem->nlp_mem, work->nlp_work);
+}
+
+
 
 int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
     void *opts_, void *mem_, void *work_)
@@ -1451,6 +1458,7 @@ void ocp_nlp_sqp_rti_config_initialize_default(void *config_)
     config->memory_assign = &ocp_nlp_sqp_rti_memory_assign;
     config->workspace_calculate_size = &ocp_nlp_sqp_rti_workspace_calculate_size;
     config->evaluate = &ocp_nlp_sqp_rti;
+    config->setup_qp_matrices_and_factorize = &ocp_nlp_sqp_rti_setup_qp_matrices_and_factorize;
     config->memory_reset_qp_solver = &ocp_nlp_sqp_rti_memory_reset_qp_solver;
     config->eval_param_sens = &ocp_nlp_sqp_rti_eval_param_sens;
     config->eval_lagr_grad_p = &ocp_nlp_sqp_rti_eval_lagr_grad_p;

diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h
@@ -81,7 +81,6 @@ typedef struct
     ocp_nlp_opts *nlp_opts;
     int compute_dual_sol;
     int ext_qp_res;           // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
-    int qp_warm_start;        // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     bool warm_start_first_qp_from_nlp;
     rti_phase_t rti_phase;

diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h
@@ -94,9 +94,10 @@ typedef struct
     void *(*memory_assign)(void *dims, void *opts, void *raw_memory);
     void (*memory_get)(void *config, void *mem, const char *field, void* value);
     acados_size_t (*workspace_calculate_size)(void *dims, void *opts);
-    int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
-    int (*condense_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
-    int (*condense_lhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    int (*condensing)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work);
+    int (*condense_rhs)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work);
+    int (*condense_lhs)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work);
+    int (*condense_qp_out)(void *qp_in, void *x_cond_qp_in, void *qp_out, void *p_cond_qp_out, void *opts, void *mem, void *work);
     int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
 } ocp_qp_xcond_config;