diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 817d3a2b5e..f48cdb21ea 100644 --- 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,22 +1550,23 @@ 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); @@ -1526,6 +1574,16 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims 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 index 064212204c..a772685f47 100644 --- 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 index ec0088346a..e0189d27ad 100644 --- 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 index e673bbd487..4768e5bab7 100644 --- 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); diff --git a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m index 0c0eb7d196..a2af8a8bdd 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -82,6 +82,7 @@ ocp.solver_options.exact_hess_cost = 1; ocp.solver_options.exact_hess_constr = 1; ocp.solver_options.print_level = 1; +ocp.solver_options.store_iterates = true; % can vary for integrators sim_method_num_stages = 1 * ones(N,1); @@ -295,6 +296,20 @@ disp(result.condition_number_blockwise) disp(['condition_number_global: ', num2str(result.condition_number_global)]) +% get second SQP iterate +% iteration index is 0-based with iterate 0 corresponding to the initial guess +iteration = 0; +iterate = ocp_solver.get_iterate(iteration); +iterates = ocp_solver.get_iterates(); +x_traj = iterates.as_array('x'); + +if ~(all(reshape(x_traj(iteration+1, end-1, :), 1, []) == reshape(iterate.x_traj{end-1}, 1, []))) + error("iterates don't match"); +end + +disp(['u iterate at iteration = ' num2str(iteration)]); +disp(cell2mat(iterate.u_traj)'); + %% Plot trajectories figure; hold on; States = {'p', 'theta', 'v', 'dtheta'}; diff --git a/examples/acados_matlab_octave/mocp_transition_example/main_multiphase_ocp.m b/examples/acados_matlab_octave/mocp_transition_example/main_multiphase_ocp.m index 666a9c735f..7287fd36f9 100644 --- a/examples/acados_matlab_octave/mocp_transition_example/main_multiphase_ocp.m +++ b/examples/acados_matlab_octave/mocp_transition_example/main_multiphase_ocp.m @@ -64,11 +64,14 @@ 1.0, ... % transition stage T_HORIZON_2 / N_list(3) * ones(1, N_list(3))]; +ocp.solver_options.store_iterates = true; + ocp_solver = AcadosOcpSolver(ocp); ocp_solver.solve(); ocp_solver.print() +iterate = ocp_solver.get_iterate(ocp_solver.get('sqp_iter')); %% extract solution x_traj = cell(N_horizon+1, 1); @@ -80,6 +83,10 @@ u_traj{i+1, 1} = ocp_solver.get('u', i); end +if iterate.x_traj{end} ~= x_traj{end} + error("x and last iterate for x should be the same.") +end + t_grid = ocp.solver_options.shooting_nodes; p_traj_1 = zeros(N_list(1)+1, 1); diff --git a/examples/acados_python/mocp_transition_example/main.py b/examples/acados_python/mocp_transition_example/main.py index ef52df3064..9e0b8e10a0 100644 --- a/examples/acados_python/mocp_transition_example/main.py +++ b/examples/acados_python/mocp_transition_example/main.py @@ -191,6 +191,7 @@ def create_multiphase_ocp_solver(N_list, t_horizon_1, name=None, use_cmake=False ocp.set_phase(phase_2, 2) ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.store_iterates = True # the transition stage uses discrete dynamics! ocp.mocp_opts.integrator_type = ['IRK', 'DISCRETE', 'IRK'] @@ -216,6 +217,9 @@ def main_multiphase_ocp(use_cmake=False): acados_ocp_solver.solve_for_x0(X0) acados_ocp_solver.print_statistics() + # get final iterate + iterate = acados_ocp_solver.get_iterate(acados_ocp_solver.get_stats('sqp_iter')) + n_phases = len(N_list) x_traj_phases = n_phases*[None] @@ -229,6 +233,8 @@ def main_multiphase_ocp(use_cmake=False): print(f"Phase {i_phase}:\nt grid \n {t_grid_phases[i_phase]} \nx traj\n {x_traj_phases[i_phase]} \nu traj {u_traj_phases[i_phase]}") print("-----------------------------------") + assert np.allclose(x_traj_phases[-1][-1], iterate.x_traj[-1]) + # plot solution t_grid_2_plot = t_grid_phases[2] - 1.0 diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py b/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py index 4564f77bda..1ea8661825 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py @@ -61,8 +61,10 @@ W_u = 1e-3 theta = model.x[1] -ocp.model.cost_expr_ext_cost = tanh(theta)**2 + .5*(model.x[0]**2 + W_u*model.u**2) -ocp.model.cost_expr_ext_cost_e = tanh(theta)**2 + .5*model.x[0]**2 +tanh_theta_squared = tanh(theta)**2 + +ocp.model.cost_expr_ext_cost = tanh_theta_squared + .5*(model.x[0]**2 + W_u*model.u**2) +ocp.model.cost_expr_ext_cost_e = tanh_theta_squared + .5*model.x[0]**2 custom_hess_u = W_u @@ -73,7 +75,7 @@ # diagonal matrix with second order terms of outer loss function. D = SX.sym('D', Sparsity.diag(2)) D[0, 0] = 1 -[hess_tan, grad_tan] = hessian(tanh(theta)**2, theta) +[hess_tan, grad_tan] = hessian(tanh_theta_squared, theta) D[1, 1] = if_else(theta == 0, hess_tan, grad_tan/theta) custom_hess_x = J.T @ D @ J @@ -82,7 +84,6 @@ cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T, custom_hess_x) cost_expr_ext_cost_custom_hess_e = custom_hess_x - ocp.model.cost_expr_ext_cost_custom_hess = cost_expr_ext_cost_custom_hess ocp.model.cost_expr_ext_cost_custom_hess_e = cost_expr_ext_cost_custom_hess_e @@ -95,36 +96,34 @@ x0 = np.array([0.0, np.pi, 0.0, 0.0]) xf = np.array([0.0, 0.0, 0.0, 0.0]) ocp.constraints.x0 = x0 -# ocp.constraints.x0 = np.array([0.0, 0.001, 0.0, 0.0]) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES -# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, -# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP -# ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' -# ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = 'MERIT_BACKTRACKING' -ocp.solver_options.nlp_solver_max_iter = 500 +ocp.solver_options.nlp_solver_max_iter = 150 +ocp.solver_options.store_iterates = True # store intermediate SQP iterates to access them after the solve +ocp.solver_options.tol = 1e-4 # set prediction horizon ocp.solver_options.tf = Tf ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') -ocp_solver.options_set("globalization_line_search_use_sufficient_descent", 0) +ocp_solver.options_set("globalization_line_search_use_sufficient_descent", 1) ocp_solver.options_set("globalization_full_step_dual", 1) simX = np.zeros((N+1, nx)) simU = np.zeros((N, nu)) +# initialization for i, tau in enumerate(np.linspace(0, 1, N+1)): ocp_solver.set(i, 'x', x0*(1-tau) + tau*xf) + status = ocp_solver.solve() if status != 0: - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # raise Exception(f'acados returned status {status}.') + print(f'acados returned status {status}.') # get solution for i in range(N): @@ -132,6 +131,14 @@ simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") -ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") +ocp_solver.print_statistics() + +iteration = 2 +print(f"primal iterate at iteration {iteration}:") +iterate = ocp_solver.get_iterate(iteration) +print(iterate.x_traj) +# get all iterates +iterates = ocp_solver.get_iterates() +# print(iterates.iterate_traj[-1]) plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) diff --git a/examples/acados_python/tests/armijo_test.py b/examples/acados_python/tests/armijo_test.py index ffef703b9a..ae124ae9a7 100644 --- a/examples/acados_python/tests/armijo_test.py +++ b/examples/acados_python/tests/armijo_test.py @@ -109,7 +109,7 @@ def solve_armijo_problem_with_setting(setting): ocp.solver_options.globalization_line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.globalization_eps_sufficient_descent = 5e-1 - + SQP_max_iter = 200 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.nlp_solver_max_iter = SQP_max_iter diff --git a/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py b/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py index f4b51effbc..f4b70a7e5a 100644 --- a/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py +++ b/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py @@ -239,7 +239,7 @@ def plot_trajectory(list_X_sol: list, labels: list): def main(): N = 20 - M = 2 + M = 2 Tf = 1.0 # # Test DDP here diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 34beddd0b9..1e8eaf9be6 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1403,6 +1403,23 @@ void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_so } +void ocp_nlp_get_from_iterate(ocp_nlp_dims *dims, ocp_nlp_solver *solver, + int iter, int stage, const char *field, void *value) +{ + ocp_nlp_config *config = solver->config; + ocp_nlp_memory *nlp_mem; + config->get(config, solver->dims, solver->mem, "nlp_mem", &nlp_mem); + + ocp_nlp_opts *nlp_opts; + config->opts_get(config, solver->dims, solver->opts, "nlp_opts", &nlp_opts); + + if (!nlp_opts->store_iterates) + { + printf("\nerror: ocp_nlp_get_from_iterate: store_iterates needs to be set to true in order to get iterates.\n"); + exit(1); + } + ocp_nlp_out_get(config, dims, nlp_mem->iterates[iter], stage, field, value); +} void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value) diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 5b3df01502..b719108651 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -305,6 +305,9 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims * ACADOS_SYMBOL_EXPORT void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver, int stage, const char *field, void *value); +ACADOS_SYMBOL_EXPORT void ocp_nlp_get_from_iterate(ocp_nlp_dims *dims, ocp_nlp_solver *solver, + int iter, int stage, const char *field, void *value); + // TODO(andrea): remove this once/if the MATLAB interface uses the new setters below? ACADOS_SYMBOL_EXPORT int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field); diff --git a/interfaces/acados_matlab_octave/AcadosOcpIterate.m b/interfaces/acados_matlab_octave/AcadosOcpIterate.m new file mode 100644 index 0000000000..15475abef3 --- /dev/null +++ b/interfaces/acados_matlab_octave/AcadosOcpIterate.m @@ -0,0 +1,66 @@ +% +% Copyright (c) The acados authors. +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; + + +classdef AcadosOcpIterate < handle + properties + x_traj + u_traj + z_traj + sl_traj + su_traj + pi_traj + lam_traj + end + + methods + function obj = AcadosOcpIterate(x_traj_, u_traj_, z_traj_, sl_traj_, su_traj_, pi_traj_, lam_traj_) + obj.x_traj = x_traj_; + obj.u_traj = u_traj_; + obj.z_traj = z_traj_; + obj.sl_traj = sl_traj_; + obj.su_traj = su_traj_; + obj.pi_traj = pi_traj_; + obj.lam_traj = lam_traj_; + end + + function s = struct(self) + if exist('properties') + publicProperties = eval('properties(self)'); + else + publicProperties = fieldnames(self); + end + s = struct(); + for fi = 1:numel(publicProperties) + s.(publicProperties{fi}) = self.(publicProperties{fi}); + end + end + end +end + diff --git a/interfaces/acados_matlab_octave/AcadosOcpIterates.m b/interfaces/acados_matlab_octave/AcadosOcpIterates.m new file mode 100644 index 0000000000..445d53c80a --- /dev/null +++ b/interfaces/acados_matlab_octave/AcadosOcpIterates.m @@ -0,0 +1,96 @@ +% +% Copyright (c) The acados authors. +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; + + +classdef AcadosOcpIterates < handle + properties (Access = public) + iterates_cell + end + + properties (Access = private) + fields = {'x', 'u', 'z', 'sl', 'su', 'lam', 'pi'}; + end + + methods + function obj = AcadosOcpIterates(iterates_cell_) + obj.iterates_cell = iterates_cell_; + end + + function iterate = as_array(obj, field) + % Return the iterates as matrix of size (nlp_iter + 1, N_horizon (+ 1), n_field) + % This will fail if the dimension of value `field` is varying stagewise. + if ~any(strcmp(obj.fields, field)) + error(["Invalid field: got " field]); + end + + n_iterates = length(obj.iterates_cell); % n_iterates = nlp_iter + 1 + field_iterates_cell = cell(n_iterates, 1); + + attr = [field '_traj']; + + iterate = obj.iterates_cell{1}; + traj = iterate.(attr); + num_0 = length(traj); + + try + % reshape to (num, n_field), num might be either N_horizon or N_horizon + 1 + for i=1:(n_iterates) + iterate = obj.iterates_cell{i}; + traj = iterate.(attr); + + num = length(traj); + if num ~= num_0 + error(['Stage-wise dimensions are not the same for ' field ' trajectory.']); + end + % NOTE: cannot change reshape order, thus need to transpose afterwards + field_iterates_cell{i} = reshape(cell2mat(traj), [], num).'; + end + + iterate = zeros(n_iterates, num_0, size(field_iterates_cell{1}, 2)); + for i=1:n_iterates + iterate(i, :, :) = field_iterates_cell{i}; + end + catch + error(['Stage-wise dimensions are not the same for ' field ' trajectory.']); + end + end + + function s = struct(obj) + if exist('properties') + publicProperties = eval('properties(obj)'); + else + publicProperties = fieldnames(obj); + end + s = struct(); + for fi = 1:numel(publicProperties) + s.(publicProperties{fi}) = obj.(publicProperties{fi}); + end + end + end +end \ No newline at end of file diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 2c415919f5..4eab7c721d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -97,6 +97,7 @@ adaptive_levenberg_marquardt_mu_min adaptive_levenberg_marquardt_mu0 log_primal_step_norm + store_iterates eval_residual_at_max_iter ext_fun_compile_flags @@ -176,6 +177,7 @@ obj.adaptive_levenberg_marquardt_mu_min = 1e-16; obj.adaptive_levenberg_marquardt_mu0 = 1e-3; obj.log_primal_step_norm = 0; + obj.store_iterates = false; obj.eval_residual_at_max_iter = []; obj.ext_fun_compile_flags = '-O2'; diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 00599795fe..dd0f1a7de0 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -179,6 +179,54 @@ function set(obj, field, value, varargin) obj.t_ocp.load_iterate(filename); end + function iterate = get_iterate(obj, iteration) + if iteration > obj.get('nlp_iter') + error("iteration needs to be nonnegative and <= nlp_iter."); + end + + if ~obj.ocp.solver_options.store_iterates + error("get_iterate: the solver option store_iterates needs to be true in order to get iterates."); + end + + N_horizon = obj.ocp.solver_options.N_horizon; + + x_traj = cell(N_horizon + 1, 1); + u_traj = cell(N_horizon, 1); + z_traj = cell(N_horizon, 1); + sl_traj = cell(N_horizon + 1, 1); + su_traj = cell(N_horizon + 1, 1); + pi_traj = cell(N_horizon, 1); + lam_traj = cell(N_horizon + 1, 1); + + for n=1:N_horizon + x_traj{n, 1} = obj.t_ocp.get('x', n-1, iteration); + u_traj{n, 1} = obj.t_ocp.get('u', n-1, iteration); + z_traj{n, 1} = obj.t_ocp.get('z', n-1, iteration); + sl_traj{n, 1} = obj.t_ocp.get('sl', n-1, iteration); + su_traj{n, 1} = obj.t_ocp.get('su', n-1, iteration); + pi_traj{n, 1} = obj.t_ocp.get('pi', n-1, iteration); + lam_traj{n, 1} = obj.t_ocp.get('lam', n-1, iteration); + end + + x_traj{N_horizon+1, 1} = obj.t_ocp.get('x', N_horizon, iteration); + sl_traj{N_horizon+1, 1} = obj.t_ocp.get('sl', N_horizon, iteration); + su_traj{N_horizon+1, 1} = obj.t_ocp.get('su', N_horizon, iteration); + lam_traj{N_horizon+1, 1} = obj.t_ocp.get('lam', N_horizon, iteration); + + iterate = AcadosOcpIterate(x_traj, u_traj, z_traj, ... + sl_traj, su_traj, pi_traj, lam_traj); + end + + function iterates = get_iterates(obj) + nlp_iter = obj.get('nlp_iter'); + iterates_cell = cell(nlp_iter+1, 1); + + for n=1:(nlp_iter+1) + iterates_cell{n} = obj.get_iterate(n-1); + end + + iterates = AcadosOcpIterates(iterates_cell); + end function print(obj, varargin) obj.t_ocp.print(varargin{:}); diff --git a/interfaces/acados_matlab_octave/acados_ocp_opts.m b/interfaces/acados_matlab_octave/acados_ocp_opts.m index 21d650fe56..fdbbfb7614 100644 --- a/interfaces/acados_matlab_octave/acados_ocp_opts.m +++ b/interfaces/acados_matlab_octave/acados_ocp_opts.m @@ -79,6 +79,7 @@ obj.opts_struct.qp_solver_iter_max = 50; obj.opts_struct.qp_solver_mu0 = 0; + obj.opts_struct.store_iterates = false; % obj.opts_struct.qp_solver_cond_N = 5; % New horizon after partial condensing obj.opts_struct.qp_solver_cond_ric_alg = 1; % 0: dont factorize hessian in the condensing; 1: factorize @@ -236,6 +237,8 @@ obj.opts_struct.globalization = value; elseif (strcmp(field, 'parameter_values')) obj.opts_struct.parameter_values = value; + elseif (strcmp(field, 'store_iterates')) + obj.opts_struct.store_iterates = value; elseif (strcmp(field, 'ext_fun_compile_flags')) obj.opts_struct.ext_fun_compile_flags = value; elseif (strcmp(field, 'compile_mex')) diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index 9336d62c75..a7ab2aff4c 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -83,8 +83,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) int N = dims->N; int stage; + int iteration; - if (nrhs==3) + if (nrhs>=3) { stage = mxGetScalar( prhs[2] ); if (stage < 0 || stage > N) @@ -99,6 +100,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) } } + if (nrhs == 4) + { + iteration = mxGetScalar(prhs[3]); + int nlp_iter; + ocp_nlp_get(config, solver, "nlp_iter", &nlp_iter); + if (iteration < 0 || iteration > nlp_iter) + { + sprintf(buffer, "\nocp_get: invalid iteration index, got stage = %d, should be nonnegative and <= nlp_iter = %d\n", iteration, nlp_iter); + mexErrMsgTxt(buffer); + } + } + if (!strcmp(field, "x")) { int nx; @@ -125,6 +138,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *x = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, "x", x); } + else if (nrhs==4) + { + nx = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "x"); + plhs[0] = mxCreateNumericMatrix(nx, 1, mxDOUBLE_CLASS, mxREAL); + double *x = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, "x", x); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -157,6 +177,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *u = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, "u", u); } + else if (nrhs==4) + { + nu = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "u"); + plhs[0] = mxCreateNumericMatrix(nu, 1, mxDOUBLE_CLASS, mxREAL); + double *u = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, "u", u); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -177,6 +204,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *value = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, field, value); } + else if (nrhs==4) + { + length = ocp_nlp_dims_get_from_attr(config, dims, out, stage, field); + plhs[0] = mxCreateNumericMatrix(length, 1, mxDOUBLE_CLASS, mxREAL); + double *value = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, field, value); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -209,6 +243,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *z = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, "z", z); } + else if (nrhs==4) + { + nz = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "z"); + plhs[0] = mxCreateNumericMatrix(nz, 1, mxDOUBLE_CLASS, mxREAL); + double *z = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, "z", z); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -242,6 +283,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *pi = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, "pi", pi); } + else if (nrhs==4) + { + npi = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "pi"); + plhs[0] = mxCreateNumericMatrix(npi, 1, mxDOUBLE_CLASS, mxREAL); + double *pi = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, "pi", pi); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -282,6 +330,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *lam = mxGetPr( plhs[0] ); ocp_nlp_out_get(config, dims, out, stage, "lam", lam); } + else if (nrhs==4) + { + int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "lam"); + plhs[0] = mxCreateNumericMatrix(nlam, 1, mxDOUBLE_CLASS, mxREAL); + double *lam = mxGetPr(plhs[0]); + ocp_nlp_get_from_iterate(dims, solver, iteration, stage, "lam", lam); + } else { sprintf(buffer, "\nocp_get: wrong nrhs: %d\n", nrhs); @@ -401,13 +456,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) ocp_nlp_get(config, solver, "status", &status); *mat_ptr = (double) status; } - else if (!strcmp(field, "sqp_iter")) + else if (!strcmp(field, "sqp_iter") || !strcmp(field, "nlp_iter")) { plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); double *mat_ptr = mxGetPr( plhs[0] ); - int sqp_iter; - ocp_nlp_get(config, solver, "sqp_iter", &sqp_iter); - *mat_ptr = (double) sqp_iter; + int nlp_iter; + ocp_nlp_get(config, solver, "nlp_iter", &nlp_iter); + *mat_ptr = (double) nlp_iter; } else if (!strcmp(field, "time_tot") || !strcmp(field, "time_lin") || !strcmp(field, "time_glob") || !strcmp(field, "time_reg") || !strcmp(field, "time_qp_sol") || !strcmp(field, "time_qp_solver_call") || !strcmp(field, "time_qp_solver") || !strcmp(field, "time_qp_xcond") || !strcmp(field, "time_sim") || !strcmp(field, "time_sim_la") || !strcmp(field, "time_sim_ad")) { @@ -425,14 +480,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) } else if (!strcmp(field, "stat")) { - int sqp_iter; + int nlp_iter; int stat_m, stat_n; double *stat; - ocp_nlp_get(config, solver, "sqp_iter", &sqp_iter); + ocp_nlp_get(config, solver, "nlp_iter", &nlp_iter); ocp_nlp_get(config, solver, "stat_m", &stat_m); ocp_nlp_get(config, solver, "stat_n", &stat_n); ocp_nlp_get(config, solver, "stat", &stat); - int min_size = stat_m np.ndarray: + """ + Returns the iterate given by field as a numpy array of shape (nlp_iter+1, N_horizon (+ 1), n_field). + This will fail if the dimension of value `field` is varying stagewise. + """ + + if field not in self.__iterate_fields: + raise Exception(f"Invalid field: got {field}, expected value in {self.__iterate_fields}") + + attr = f"{field}_traj" + traj_ = [getattr(iterate, attr) for iterate in self.iterate_list] + + try: + traj = np.array(traj_, dtype=float) + except ValueError: + raise Exception(f"Stage-wise dimensions are not the same for {field} trajectory.") + + return traj diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 3eda21d1aa..d9c777bdf1 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -111,7 +111,8 @@ def __init__(self): self.__adaptive_levenberg_marquardt_lam = 5.0 self.__adaptive_levenberg_marquardt_mu_min = 1e-16 self.__adaptive_levenberg_marquardt_mu0 = 1e-3 - self.__log_primal_step_norm : bool = False + self.__log_primal_step_norm: bool = False + self.__store_iterates: bool = False # TODO: move those out? they are more about generation than about the acados OCP solver. self.__ext_fun_compile_flags = '-O2' self.__model_external_shared_lib_dir = None @@ -544,6 +545,15 @@ def log_primal_step_norm(self,): """ return self.__log_primal_step_norm + @property + def store_iterates(self,): + """ + Flag indicating whether the intermediate primal-dual iterates should be stored. + This is implemented only for solver type `SQP` and `DDP`. + Default: False + """ + return self.__store_iterates + @property def tol(self): """ @@ -1369,6 +1379,13 @@ def log_primal_step_norm(self, val): else: raise Exception('Invalid log_primal_step_norm value. Expected bool.') + @store_iterates.setter + def store_iterates(self, val): + if isinstance(val, bool): + self.__store_iterates = val + else: + raise Exception('Invalid store_iterates value. Expected bool.') + @as_rti_iter.setter def as_rti_iter(self, as_rti_iter): if isinstance(as_rti_iter, int) and as_rti_iter >= 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 9bf28e0505..c270c6daf5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -55,6 +55,7 @@ from .utils import (get_shared_lib_ext, get_shared_lib_prefix, get_shared_lib_dir, get_shared_lib, make_object_json_dumpable, set_up_imported_gnsf_model, verbose_system_call, acados_lib_is_compiled_with_openmp) +from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates class AcadosOcpSolver: @@ -295,6 +296,8 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file= self.__acados_lib.ocp_nlp_get_at_stage.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_get_from_iterate.argtypes = [c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_get_from_iterate.restypes = c_void_p getattr(self.shared_lib, f"{self.name}_acados_solve").argtypes = [c_void_p] getattr(self.shared_lib, f"{self.name}_acados_solve").restype = c_int @@ -968,7 +971,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: """ Get the information of the last solver call. - :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'sqp_iter', 'residuals', 'qp_iter', 'alpha'] + :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'nlp_iter', 'sqp_iter', 'residuals', 'qp_iter', 'alpha'] Available fileds: - time_tot: total CPU time previous call @@ -1445,6 +1448,70 @@ def get_from_qp_in(self, stage_: int, field_: str): return out + def __ocp_nlp_get_from_iterate(self, iteration_, stage_, field_): + stage = c_int(stage_) + field = field_.encode('utf-8') + iteration = c_int(iteration_) + dim = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, + stage, field) + + out = np.ascontiguousarray(np.zeros((dim,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + out_data_p = cast((out_data), c_void_p) + self.__acados_lib.ocp_nlp_get_from_iterate(self.nlp_dims, self.nlp_solver, iteration, stage, field, out_data_p) + return out + + def get_iterate(self, iteration: int) -> AcadosOcpIterate: + + if iteration < 0 or iteration > self.get_stats('sqp_iter'): + raise Exception("get_iterate: iteration needs to be nonnegative and <= sqp_iter.") + + if not self.acados_ocp.solver_options.store_iterates: + raise Exception("get_iterate: the solver option store_iterates needs to be true in order to get iterates.") + + x_traj = [] + u_traj = [] + z_traj = [] + sl_traj = [] + su_traj = [] + pi_traj = [] + lam_traj = [] + + for n in range(self.acados_ocp.solver_options.N_horizon): + x_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "x")) + u_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "u")) + z_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "z")) + sl_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "sl")) + su_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "su")) + pi_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "pi")) + lam_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "lam")) + + n = self.acados_ocp.solver_options.N_horizon + x_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "x")) + sl_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "sl")) + su_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "su")) + lam_traj.append(self.__ocp_nlp_get_from_iterate(iteration, n, "lam")) + + iterate = AcadosOcpIterate(x_traj=tuple(x_traj), + u_traj=tuple(u_traj), + z_traj=tuple(z_traj), + sl_traj=tuple(sl_traj), + su_traj=tuple(su_traj), + pi_traj=tuple(pi_traj), + lam_traj=tuple(lam_traj)) + + return iterate + + + def get_iterates(self) -> AcadosOcpIterates: + return AcadosOcpIterates(iterate_list=[self.get_iterate(n) for n in range(self.get_stats('nlp_iter')+1)]) + + + def dims_get(self, field_, stage_): + return self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, + c_int(stage_), field_.encode('utf-8')) + + def options_set(self, field_, value_): """ Set options of the solver. diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c index d77a3f543d..793af17644 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c @@ -2201,6 +2201,9 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_epsilon", ®_epsilon); {%- endif %} + bool store_iterates = {{ solver_options.store_iterates }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); + int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index 3a90163019..c16ad6f226 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -2266,6 +2266,9 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); + bool store_iterates = {{ solver_options.store_iterates }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); + {%- if solver_options.nlp_solver_type == "SQP" %} int log_primal_step_norm = {{ solver_options.log_primal_step_norm }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m index 9991fa0c9b..7f61d68610 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m @@ -141,8 +141,12 @@ function eval_param_sens(obj, field, stage, index) elseif nargin==3 stage = varargin{3}; value = ocp_get(obj.C_ocp, field, stage); + elseif nargin==4 + stage = varargin{3}; + iteration = varargin{4}; + value = ocp_get(obj.C_ocp, field, stage, iteration); else - disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)'); + disp('acados_ocp.get: wrong number of input arguments (1, 2 or 3 allowed)'); end