acados · FreyJo · Oct 1, 2024 · Sep 26, 2024 · Sep 26, 2024 · Sep 26, 2024
diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c
@@ -1060,6 +1060,7 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_)
     opts->print_level = 0;
     opts->levenberg_marquardt = 0.0;
     opts->log_primal_step_norm = 0;
+    opts->max_iter = 1;
 
     /* submodules opts */
     // qp solver
@@ -1098,6 +1099,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->store_iterates = false;
 
     return;
 }
@@ -1186,6 +1188,19 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value
             int* ext_qp_res = (int *) value;
             opts->ext_qp_res = *ext_qp_res;
         }
+        else if (!strcmp(field, "store_iterates"))
+        {
+            bool* store_iterates = (bool *) value;
+
+            if (*store_iterates && config->is_real_time_algorithm())
+            {
+                printf("Warning: Can not store intermediate iterates for real-time solvers.\n");
+            }
+            else
+            {
+                opts->store_iterates = *store_iterates;
+            }
+        }
         else if (!strcmp(field, "levenberg_marquardt"))
         {
             double* levenberg_marquardt = (double *) value;
@@ -1253,6 +1268,19 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value
             int* log_primal_step_norm = (int *) value;
             opts->log_primal_step_norm = *log_primal_step_norm;
         }
+        else if (!strcmp(field, "max_iter"))
+        {
+            int* max_iter = (int *) value;
+
+            if (*max_iter > 0 && config->is_real_time_algorithm())
+            {
+                printf("Warning: can not set max_iter > 1 for real-time solvers.");
+            }
+            else
+            {
+                opts->max_iter = *max_iter;
+            }
+        }
         else if (!strcmp(field, "print_level"))
         {
             int* print_level = (int *) value;
@@ -1398,12 +1426,24 @@ acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims
         size += constraints[i]->memory_calculate_size(constraints[i], dims->constraints[i], opts->constraints[i]);
     }
 
+    // intermediate iterates
+    if (opts->store_iterates)
+    {
+        size += (opts->max_iter + 1) * sizeof(struct ocp_nlp_out *);
+
+        for (int i = 0; i <= opts->max_iter; i++)
+        {
+            size += ocp_nlp_out_calculate_size(config, dims);
+        }
+    }
+
     // nlp res
     size += ocp_nlp_res_calculate_size(dims);
 
     // timings
     size += sizeof(struct ocp_nlp_timings);
 
+
     size += (N+1)*sizeof(bool); // set_sim_guess
 
     size += (N+1)*sizeof(struct blasfeo_dmat); // dzduxt
@@ -1476,6 +1516,13 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims
     mem->constraints = (void **) c_ptr;
     c_ptr += (N+1)*sizeof(void *);
 
+    // intermediate iterates
+    if (opts->store_iterates)
+    {
+        mem->iterates = (struct ocp_nlp_out **) c_ptr;
+        c_ptr += (opts->max_iter + 1)*sizeof(struct ocp_nlp_out *);
+    }
+
     // middle align
     align_char_to(8, &c_ptr);
 
@@ -1503,29 +1550,40 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims
     c_ptr += config->globalization->memory_calculate_size(config->globalization, dims);
     // ->memory_calculate_size(config->globalization, dims);
 
+    int i;
     // dynamics
-    for (int i = 0; i < N; i++)
+    for (i = 0; i < N; i++)
     {
         mem->dynamics[i] = dynamics[i]->memory_assign(dynamics[i], dims->dynamics[i], opts->dynamics[i], c_ptr);
         c_ptr += dynamics[i]->memory_calculate_size(dynamics[i], dims->dynamics[i], opts->dynamics[i]);
     }
 
     // cost
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         mem->cost[i] = cost[i]->memory_assign(cost[i], dims->cost[i], opts->cost[i], c_ptr);
         c_ptr += cost[i]->memory_calculate_size(cost[i], dims->cost[i], opts->cost[i]);
     }
 
     // constraints
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         mem->constraints[i] = constraints[i]->memory_assign(constraints[i],
                                             dims->constraints[i], opts->constraints[i], c_ptr);
         c_ptr += constraints[i]->memory_calculate_size( constraints[i], dims->constraints[i],
                                                                  opts->constraints[i]);
     }
 
+    // intermediate iterates
+    if (opts->store_iterates)
+    {
+        for (i = 0; i <= opts->max_iter; i++)
+        {
+            mem->iterates[i] = ocp_nlp_out_assign(config, dims, c_ptr);
+            c_ptr += ocp_nlp_out_calculate_size(config, dims);
+        }
+    }
+
     // nlp res
     mem->nlp_res = ocp_nlp_res_assign(dims, c_ptr);
     c_ptr += mem->nlp_res->memsize;
@@ -1534,6 +1592,7 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims
     mem->nlp_timings = (ocp_nlp_timings*) c_ptr;
     c_ptr += sizeof(ocp_nlp_timings);
 
+
     // zero timings
     ocp_nlp_timings_reset(mem->nlp_timings);
     mem->nlp_timings->time_feedback = 0;
@@ -1563,7 +1622,7 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims
 
     // set_sim_guess
     assign_and_advance_bool(N+1, &mem->set_sim_guess, &c_ptr);
-    for (int i = 0; i <= N; ++i)
+    for (i = 0; i <= N; ++i)
     {
         mem->set_sim_guess[i] = false;
     }
@@ -1572,42 +1631,42 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims
     align_char_to(64, &c_ptr);
 
     // dzduxt
-    for (int i=0; i<=N; i++)
+    for (i=0; i<=N; i++)
     {
         assign_and_advance_blasfeo_dmat_mem(nu[i]+nx[i], nz[i], mem->dzduxt+i, &c_ptr);
     }
     // z_alg
-    for (int i=0; i<=N; i++)
+    for (i=0; i<=N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nz[i], mem->z_alg + i, &c_ptr);
     }
     // cost_grad
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nv[i], mem->cost_grad + i, &c_ptr);
     }
     // ineq_fun
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(2 * ni[i], mem->ineq_fun + i, &c_ptr);
     }
     // ineq_adj
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nv[i], mem->ineq_adj + i, &c_ptr);
     }
     // dyn_fun
-    for (int i = 0; i < N; i++)
+    for (i = 0; i < N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nx[i + 1], mem->dyn_fun + i, &c_ptr);
     }
     // dyn_adj
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nu[i] + nx[i], mem->dyn_adj + i, &c_ptr);
     }
     // sim_guess
-    for (int i = 0; i <= N; i++)
+    for (i = 0; i <= N; i++)
     {
         assign_and_advance_blasfeo_dvec_mem(nx[i] + nz[i], mem->sim_guess + i, &c_ptr);
         // set to 0;

diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h
@@ -275,6 +275,7 @@ typedef struct ocp_nlp_opts
     int print_level;
     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
 
     // Flag for usage of adaptive levenberg marquardt strategy
     bool with_adaptive_levenberg_marquardt;
@@ -287,6 +288,9 @@ typedef struct ocp_nlp_opts
 
     int ext_qp_res;
 
+    bool store_iterates; // flag indicating whether intermediate iterates should be stored
+
+
 } ocp_nlp_opts;
 
 //
@@ -371,6 +375,9 @@ typedef struct ocp_nlp_memory
     void **cost;         // cost memory
     void **constraints;  // constraints memory
 
+    // intermediate iterates
+    struct ocp_nlp_out ** iterates;
+
     // residuals
     ocp_nlp_res *nlp_res;
 

diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c
@@ -110,7 +110,7 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_
     ocp_nlp_opts_initialize_default(config, dims, nlp_opts);
 
     // DDP opts
-    opts->max_iter = 20;
+    opts->nlp_opts->max_iter = 20;
     opts->tol_stat = 1e-8;
     opts->tol_eq   = 1e-8;
     opts->tol_ineq = 1e-8;
@@ -174,12 +174,7 @@ void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* v
     }
     else // nlp opts
     {
-        if (!strcmp(field, "max_iter"))
-        {
-            int* max_iter = (int *) value;
-            opts->max_iter = *max_iter;
-        }
-        else if (!strcmp(field, "tol_stat"))
+        if (!strcmp(field, "tol_stat"))
         {
             double* tol_stat = (double *) value;
             opts->tol_stat = *tol_stat;
@@ -269,7 +264,7 @@ acados_size_t ocp_nlp_ddp_memory_calculate_size(void *config_, void *dims_, void
     size += ocp_nlp_memory_calculate_size(config, dims, nlp_opts);
 
     // stat
-    int stat_m = opts->max_iter+1;
+    int stat_m = opts->nlp_opts->max_iter+1;
     int stat_n = 7;
     if (opts->ext_qp_res)
         stat_n += 4;
@@ -342,7 +337,7 @@ void *ocp_nlp_ddp_memory_assign(void *config_, void *dims_, void *opts_, void *r
 
     // stat
     mem->stat = (double *) c_ptr;
-    mem->stat_m = opts->max_iter+1;
+    mem->stat_m = opts->nlp_opts->max_iter+1;
     mem->stat_n = 7;
     if (opts->ext_qp_res)
         mem->stat_n += 4;
@@ -592,7 +587,7 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me
     }
 
     // Check for maximum iterations
-    if (ddp_iter >= opts->max_iter)
+    if (ddp_iter >= opts->nlp_opts->max_iter)
     {
         nlp_mem->status = ACADOS_MAXITER;
         if (opts->nlp_opts->print_level > 0){
@@ -665,12 +660,17 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
         printf("'with_adaptive_levenberg_marquardt' option is set to: %s\n", opts->nlp_opts->with_adaptive_levenberg_marquardt?"true":"false");
     }
 
-    for (; ddp_iter <= opts->max_iter; ddp_iter++)
+    for (; ddp_iter <= opts->nlp_opts->max_iter; ddp_iter++)
     {
+        // store current iterate
+        if (nlp_opts->store_iterates)
+        {
+            copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[ddp_iter]);
+        }
         // We always evaluate the residuals until the last iteration
         // If the option "eval_residual_at_max_iter" is set, then we will also
         // evaluate the data after the last iteration was performed
-        if (ddp_iter != opts->max_iter || opts->eval_residual_at_max_iter)
+        if (ddp_iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter)
         {
             /* Prepare the QP data */
             // linearize NLP, update QP matrices, and add Levenberg-Marquardt term

diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c
@@ -113,7 +113,7 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_
     ocp_nlp_opts_initialize_default(config, dims, nlp_opts);
 
     // SQP opts
-    opts->max_iter = 20;
+    opts->nlp_opts->max_iter = 20;
     opts->tol_stat = 1e-8;
     opts->tol_eq   = 1e-8;
     opts->tol_ineq = 1e-8;
@@ -176,12 +176,7 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v
     }
     else // nlp opts
     {
-        if (!strcmp(field, "max_iter"))
-        {
-            int* max_iter = (int *) value;
-            opts->max_iter = *max_iter;
-        }
-        else if (!strcmp(field, "tol_stat"))
+        if (!strcmp(field, "tol_stat"))
         {
             double* tol_stat = (double *) value;
             opts->tol_stat = *tol_stat;
@@ -283,10 +278,10 @@ acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void
     // primal step norm
     if (opts->nlp_opts->log_primal_step_norm)
     {
-        size += opts->max_iter*sizeof(double);
+        size += opts->nlp_opts->max_iter*sizeof(double);
     }
     // stat
-    int stat_m = opts->max_iter+1;
+    int stat_m = opts->nlp_opts->max_iter+1;
     int stat_n = 7;
     if (nlp_opts->ext_qp_res)
         stat_n += 4;
@@ -329,12 +324,12 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *r
     if (opts->nlp_opts->log_primal_step_norm)
     {
         mem->primal_step_norm = (double *) c_ptr;
-        c_ptr += opts->max_iter*sizeof(double);
+        c_ptr += opts->nlp_opts->max_iter*sizeof(double);
     }
 
     // stat
     mem->stat = (double *) c_ptr;
-    mem->stat_m = opts->max_iter+1;
+    mem->stat_m = opts->nlp_opts->max_iter+1;
     mem->stat_n = 7;
     if (nlp_opts->ext_qp_res)
         mem->stat_n += 4;
@@ -451,7 +446,7 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r
     }
 
     // check for maximum iterations
-    if (!opts->eval_residual_at_max_iter && n_iter >= opts->max_iter)
+    if (!opts->eval_residual_at_max_iter && n_iter >= opts->nlp_opts->max_iter)
     {
         mem->nlp_mem->status = ACADOS_MAXITER;
         if (opts->nlp_opts->print_level > 0)
@@ -505,7 +500,7 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r
     }
 
     // check for maximum iterations
-    if (n_iter >= opts->max_iter)
+    if (n_iter >= opts->nlp_opts->max_iter)
     {
         mem->nlp_mem->status = ACADOS_MAXITER;
         if (opts->nlp_opts->print_level > 0)
@@ -571,13 +566,18 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
      ************************************************/
     int sqp_iter = 0;
     double prev_levenberg_marquardt = 0.0;
-    for (; sqp_iter <= opts->max_iter; sqp_iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown.
+    for (; sqp_iter <= opts->nlp_opts->max_iter; sqp_iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown.
     {
         // We always evaluate the residuals until the last iteration
         // If the option "eval_residual_at_max_iter" is set, we also
         // evaluate the residuals after the last iteration.
-        if (sqp_iter != opts->max_iter || opts->eval_residual_at_max_iter)
+        if (sqp_iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter)
         {
+            // store current iterate
+            if (nlp_opts->store_iterates)
+            {
+                copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[sqp_iter]);
+            }
             /* Prepare the QP data */
             // linearize NLP and update QP matrices
             acados_tic(&timer1);