From c882b860d6aa7ab9f8955bd7390d24ffc5d5035d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 17 Mar 2025 16:43:52 +0100 Subject: [PATCH 001/164] Refactor `ext_fun_expand` option, implement more granular options instead (#1467) - New options: `ext_fun_expand_constr, ext_fun_expand_cost, ext_fun_expand_dyn, ext_fun_expand_precompute`. - Adresses https://github.com/acados/acados/issues/1426 --- .../p_global_example/set_solver_options.m | 5 +- .../AcadosMultiphaseOcp.m | 5 +- interfaces/acados_matlab_octave/AcadosOcp.m | 6 +- .../acados_matlab_octave/AcadosOcpOptions.m | 11 +- .../acados_matlab_octave/AcadosSimOptions.m | 4 +- .../acados_matlab_octave/GenerateContext.m | 23 +-- .../acados_matlab_octave/acados_ocp_opts.m | 18 ++- .../acados_matlab_octave/acados_sim_opts.m | 6 +- .../generate_c_code_discrete_dynamics.m | 6 +- .../generate_c_code_explicit_ode.m | 8 +- .../generate_c_code_ext_cost.m | 24 ++-- .../generate_c_code_gnsf.m | 10 +- .../generate_c_code_implicit_ode.m | 10 +- .../generate_c_code_nonlinear_constr.m | 18 +-- .../generate_c_code_nonlinear_least_squares.m | 18 +-- ...up_AcadosOcp_from_legacy_ocp_description.m | 5 +- ...up_AcadosSim_from_legacy_sim_description.m | 2 +- .../sim_generate_c_code.m | 5 +- .../acados_template/acados_multiphase_ocp.py | 19 +-- .../acados_template/acados_ocp.py | 22 +-- .../acados_template/acados_ocp_options.py | 64 +++++++-- .../acados_template/acados_sim.py | 28 ++-- .../casadi_function_generation.py | 132 ++++++++++-------- 23 files changed, 282 insertions(+), 167 deletions(-) diff --git a/examples/acados_matlab_octave/p_global_example/set_solver_options.m b/examples/acados_matlab_octave/p_global_example/set_solver_options.m index 0204ff4633..7f56f45fa7 100644 --- a/examples/acados_matlab_octave/p_global_example/set_solver_options.m +++ b/examples/acados_matlab_octave/p_global_example/set_solver_options.m @@ -51,5 +51,8 @@ % These might be different depending on your compiler and operating system. flags = ['-I' casadi.GlobalOptions.getCasadiIncludePath ' -O2 -ffast-math -march=native -fno-omit-frame-pointer'] ocp.solver_options.ext_fun_compile_flags = flags; - ocp.solver_options.ext_fun_expand = true; + ocp.solver_options.ext_fun_expand_dyn = true; + ocp.solver_options.ext_fun_expand_constr = true; + ocp.solver_options.ext_fun_expand_cost = true; + ocp.solver_options.ext_fun_expand_precompute = false; end diff --git a/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m b/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m index 582ed2a4cc..e9b017b022 100644 --- a/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m +++ b/interfaces/acados_matlab_octave/AcadosMultiphaseOcp.m @@ -315,8 +315,11 @@ function make_consistent(self) code_gen_opts.with_solution_sens_wrt_params = self.solver_options.with_solution_sens_wrt_params; code_gen_opts.with_value_sens_wrt_params = self.solver_options.with_value_sens_wrt_params; code_gen_opts.code_export_directory = self.code_export_directory; - code_gen_opts.ext_fun_expand = self.solver_options.ext_fun_expand; + code_gen_opts.ext_fun_expand_dyn = self.solver_options.ext_fun_expand_dyn; + code_gen_opts.ext_fun_expand_cost = self.solver_options.ext_fun_expand_cost; + code_gen_opts.ext_fun_expand_constr = self.solver_options.ext_fun_expand_constr; + code_gen_opts.ext_fun_expand_precompute = self.solver_options.ext_fun_expand_precompute; context = GenerateContext(self.model{1}.p_global, self.name, code_gen_opts); for i=1:self.n_phases diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index f8c2cacdb0..515a08303d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -885,7 +885,11 @@ function make_consistent(self, is_mocp_phase) code_gen_opts.with_solution_sens_wrt_params = solver_opts.with_solution_sens_wrt_params; code_gen_opts.with_value_sens_wrt_params = solver_opts.with_value_sens_wrt_params; code_gen_opts.code_export_directory = ocp.code_export_directory; - code_gen_opts.ext_fun_expand = ocp.solver_options.ext_fun_expand; + + code_gen_opts.ext_fun_expand_dyn = solver_opts.ext_fun_expand_dyn; + code_gen_opts.ext_fun_expand_cost = solver_opts.ext_fun_expand_cost; + code_gen_opts.ext_fun_expand_constr = solver_opts.ext_fun_expand_constr; + code_gen_opts.ext_fun_expand_precompute = solver_opts.ext_fun_expand_precompute; context = GenerateContext(ocp.model.p_global, ocp.name, code_gen_opts); else diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 6435234147..960f783e33 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -122,7 +122,11 @@ timeout_heuristic ext_fun_compile_flags - ext_fun_expand + ext_fun_expand_dyn + ext_fun_expand_cost + ext_fun_expand_constr + ext_fun_expand_precompute + model_external_shared_lib_dir model_external_shared_lib_name custom_update_filename @@ -234,7 +238,10 @@ else obj.ext_fun_compile_flags = env_var; end - obj.ext_fun_expand = false; + obj.ext_fun_expand_dyn = false; + obj.ext_fun_expand_cost = false; + obj.ext_fun_expand_constr = false; + obj.ext_fun_expand_precompute = false; obj.model_external_shared_lib_dir = []; obj.model_external_shared_lib_name = []; diff --git a/interfaces/acados_matlab_octave/AcadosSimOptions.m b/interfaces/acados_matlab_octave/AcadosSimOptions.m index 01b6632fa2..1928e28aec 100644 --- a/interfaces/acados_matlab_octave/AcadosSimOptions.m +++ b/interfaces/acados_matlab_octave/AcadosSimOptions.m @@ -45,7 +45,7 @@ sens_hess output_z ext_fun_compile_flags - ext_fun_expand + ext_fun_expand_dyn num_threads_in_batch_solve compile_interface end @@ -72,7 +72,7 @@ else obj.ext_fun_compile_flags = env_var; end - obj.ext_fun_expand = false; + obj.ext_fun_expand_dyn = false; obj.num_threads_in_batch_solve = 1; obj.compile_interface = []; % corresponds to automatic detection, possible values: true, false, [] end diff --git a/interfaces/acados_matlab_octave/GenerateContext.m b/interfaces/acados_matlab_octave/GenerateContext.m index c3cafd6762..f39f405853 100644 --- a/interfaces/acados_matlab_octave/GenerateContext.m +++ b/interfaces/acados_matlab_octave/GenerateContext.m @@ -37,6 +37,7 @@ list_funname_dir_pairs % list of (function_name, output_dir) pairs, files that are generated generic_funname_dir_pairs % list of (function_name, output_dir) pairs, files that are not generated function_input_output_pairs + function_dyn_cost_constr_types casadi_fun_opts global_data_sym global_data_expr @@ -72,6 +73,7 @@ obj.list_funname_dir_pairs = {}; obj.function_input_output_pairs = {}; + obj.function_dyn_cost_constr_types = {}; obj.generic_funname_dir_pairs = {}; obj.casadi_fun_opts = struct(); @@ -85,9 +87,10 @@ end end - function obj = add_function_definition(obj, name, inputs, outputs, output_dir) + function obj = add_function_definition(obj, name, inputs, outputs, output_dir, dyn_cost_constr_type) obj.list_funname_dir_pairs{end+1} = {name, output_dir}; obj.function_input_output_pairs{end+1} = {inputs, outputs}; + obj.function_dyn_cost_constr_types{end+1} = dyn_cost_constr_type; end function obj = finalize(obj) @@ -207,7 +210,7 @@ fun_name = sprintf('%s_p_global_precompute_fun', self.problem_name); % Add function definition - self.add_function_definition(fun_name, {self.p_global}, {self.global_data_expr}, output_dir); + self.add_function_definition(fun_name, {self.p_global}, {self.global_data_expr}, output_dir, 'precompute'); else disp("WARNING: No CasADi function depends on p_global.") end @@ -224,6 +227,7 @@ output_dir = obj.list_funname_dir_pairs{i}{2}; inputs = obj.function_input_output_pairs{i}{1}; outputs = obj.function_input_output_pairs{i}{2}; + dyn_cost_constr_type = obj.function_dyn_cost_constr_types{i}; % fprintf('Generating function %s in directory %s\n', name, output_dir); % disp('Inputs:'); @@ -237,13 +241,14 @@ rethrow(e); end - if ~strcmp(name, sprintf('%s_p_global_precompute_fun', obj.problem_name)) - if obj.opts.ext_fun_expand - try - fun = fun.expand(); - catch - warning(['Failed to expand the CasADi function ' name '.']) - end + if ((strcmp(dyn_cost_constr_type, 'dyn') && obj.opts.ext_fun_expand_dyn) ... + || (strcmp(dyn_cost_constr_type, 'cost') && obj.opts.ext_fun_expand_cost) ... + || (strcmp(dyn_cost_constr_type, 'constr') && obj.opts.ext_fun_expand_constr) ... + || (strcmp(dyn_cost_constr_type, 'precompute') && obj.opts.ext_fun_expand_precompute)) + try + fun = fun.expand(); + catch + warning(['Failed to expand the CasADi function ' name '.']) end end diff --git a/interfaces/acados_matlab_octave/acados_ocp_opts.m b/interfaces/acados_matlab_octave/acados_ocp_opts.m index 4515ea89e2..bd289a0d57 100644 --- a/interfaces/acados_matlab_octave/acados_ocp_opts.m +++ b/interfaces/acados_matlab_octave/acados_ocp_opts.m @@ -110,7 +110,10 @@ else obj.opts_struct.ext_fun_compile_flags = env_var; end - obj.opts_struct.ext_fun_expand = false; + obj.opts_struct.ext_fun_expand_constr = false; + obj.opts_struct.ext_fun_expand_cost = false; + obj.opts_struct.ext_fun_expand_precompute = false; + obj.opts_struct.ext_fun_expand_dyn = false; obj.opts_struct.output_dir = fullfile(pwd, 'build'); obj.opts_struct.json_file = 'acados_ocp_nlp.json'; @@ -243,7 +246,18 @@ elseif (strcmp(field, 'ext_fun_compile_flags')) obj.opts_struct.ext_fun_compile_flags = value; elseif (strcmp(field, 'ext_fun_expand')) - obj.opts_struct.ext_fun_expand = value; + obj.opts_struct.ext_fun_expand_constr = value; + obj.opts_struct.ext_fun_expand_cost = value; + obj.opts_struct.ext_fun_expand_dyn = value; + elseif (strcmp(field, 'ext_fun_expand_constr')) + obj.opts_struct.ext_fun_expand_constr = value; + elseif (strcmp(field, 'ext_fun_expand_cost')) + obj.opts_struct.ext_fun_expand_cost = value; + elseif (strcmp(field, 'ext_fun_expand_dyn')) + obj.opts_struct.ext_fun_expand_dyn = value; + elseif (strcmp(field, 'ext_fun_expand_precompute')) + obj.opts_struct.ext_fun_expand_precompute = value; + elseif (strcmp(field, 'json_file')) obj.opts_struct.json_file = value; elseif (strcmp(field, 'timeout_max_time')) diff --git a/interfaces/acados_matlab_octave/acados_sim_opts.m b/interfaces/acados_matlab_octave/acados_sim_opts.m index 7c1497ccb4..5899f3ee67 100644 --- a/interfaces/acados_matlab_octave/acados_sim_opts.m +++ b/interfaces/acados_matlab_octave/acados_sim_opts.m @@ -66,7 +66,7 @@ else obj.opts_struct.ext_fun_compile_flags = env_var; end - obj.opts_struct.ext_fun_expand = false; + obj.opts_struct.ext_fun_expand_dyn = false; obj.opts_struct.parameter_values = []; end @@ -81,8 +81,8 @@ obj.opts_struct.compile_interface = value; elseif (strcmp(field, 'ext_fun_compile_flags')) obj.opts_struct.ext_fun_compile_flags = value; - elseif (strcmp(field, 'ext_fun_expand')) - obj.opts_struct.ext_fun_expand = value; + elseif (strcmp(field, 'ext_fun_expand') || strcmp(field, 'ext_fun_expand_dyn')) + obj.opts_struct.ext_fun_expand_dyn = value; elseif (strcmp(field, 'codgen_model')) warning('codgen_model is deprecated and has no effect.'); elseif (strcmp(field, 'compile_model')) diff --git a/interfaces/acados_matlab_octave/generate_c_code_discrete_dynamics.m b/interfaces/acados_matlab_octave/generate_c_code_discrete_dynamics.m index da2f919166..de9c466486 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_discrete_dynamics.m +++ b/interfaces/acados_matlab_octave/generate_c_code_discrete_dynamics.m @@ -66,10 +66,10 @@ function generate_c_code_discrete_dynamics(context, model, model_dir) % generate hessian hess_ux = jacobian(adj_ux, [u; x], struct('symmetric', isSX)); - context.add_function_definition([model.name,'_dyn_disc_phi_fun'], {x, u, p}, {phi}, model_dir); - context.add_function_definition([model.name,'_dyn_disc_phi_fun_jac'], {x, u, p}, {phi, jac_ux'}, model_dir); + context.add_function_definition([model.name,'_dyn_disc_phi_fun'], {x, u, p}, {phi}, model_dir, 'dyn'); + context.add_function_definition([model.name,'_dyn_disc_phi_fun_jac'], {x, u, p}, {phi, jac_ux'}, model_dir, 'dyn'); if context.opts.generate_hess - context.add_function_definition([model.name,'_dyn_disc_phi_fun_jac_hess'], {x, u, lam, p}, {phi, jac_ux', hess_ux}, model_dir); + context.add_function_definition([model.name,'_dyn_disc_phi_fun_jac_hess'], {x, u, lam, p}, {phi, jac_ux', hess_ux}, model_dir, 'dyn'); end end \ No newline at end of file diff --git a/interfaces/acados_matlab_octave/generate_c_code_explicit_ode.m b/interfaces/acados_matlab_octave/generate_c_code_explicit_ode.m index 0f7c209d4c..ee239e471e 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_explicit_ode.m +++ b/interfaces/acados_matlab_octave/generate_c_code_explicit_ode.m @@ -88,17 +88,17 @@ function generate_c_code_explicit_ode(context, model, model_dir) end fun_name = [model.name,'_expl_ode_fun']; - context.add_function_definition(fun_name, {x, u, p}, {f_expl}, model_dir); + context.add_function_definition(fun_name, {x, u, p}, {f_expl}, model_dir, 'dyn'); fun_name = [model.name,'_expl_vde_forw']; - context.add_function_definition(fun_name, {x, Sx, Su, u, p}, {f_expl, vdeX, vdeU}, model_dir); + context.add_function_definition(fun_name, {x, Sx, Su, u, p}, {f_expl, vdeX, vdeU}, model_dir, 'dyn'); fun_name = [model.name,'_expl_vde_adj']; - context.add_function_definition(fun_name, {x, lambdaX, u, p}, {adj}, model_dir); + context.add_function_definition(fun_name, {x, lambdaX, u, p}, {adj}, model_dir, 'dyn'); if context.opts.generate_hess fun_name = [model.name,'_expl_ode_hess']; - context.add_function_definition(fun_name, {x, Sx, Su, lambdaX, u, p}, {adj, hess2}, model_dir); + context.add_function_definition(fun_name, {x, Sx, Su, lambdaX, u, p}, {adj, hess2}, model_dir, 'dyn'); end end diff --git a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m index d5090be795..b01b0e5ae3 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m +++ b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m @@ -48,13 +48,13 @@ function generate_c_code_ext_cost(context, model, target_dir, stage_type) % generate jacobian, hessian [full_hess, grad] = hessian(ext_cost_0, vertcat(u, x, z)); % add functions to context - context.add_function_definition([model.name,'_cost_ext_cost_0_fun'], {x, u, z, p}, {ext_cost_0}, target_dir); - context.add_function_definition([model.name,'_cost_ext_cost_0_fun_jac'], {x, u, z, p}, {ext_cost_0, grad}, target_dir); + context.add_function_definition([model.name,'_cost_ext_cost_0_fun'], {x, u, z, p}, {ext_cost_0}, target_dir, 'cost'); + context.add_function_definition([model.name,'_cost_ext_cost_0_fun_jac'], {x, u, z, p}, {ext_cost_0, grad}, target_dir, 'cost'); if ~isempty(model.cost_expr_ext_cost_custom_hess_0) context.add_function_definition([model.name,'_cost_ext_cost_0_fun_jac_hess'], {x, u, z, p},... - {ext_cost_0, grad, model.cost_expr_ext_cost_custom_hess_0}, target_dir); + {ext_cost_0, grad, model.cost_expr_ext_cost_custom_hess_0}, target_dir, 'cost'); else - context.add_function_definition([model.name,'_cost_ext_cost_0_fun_jac_hess'], {x, u, z, p}, {ext_cost_0, grad, full_hess}, target_dir); + context.add_function_definition([model.name,'_cost_ext_cost_0_fun_jac_hess'], {x, u, z, p}, {ext_cost_0, grad, full_hess}, target_dir, 'cost'); end elseif strcmp(stage_type, "path") @@ -65,14 +65,14 @@ function generate_c_code_ext_cost(context, model, target_dir, stage_type) % generate jacobian, hessian [full_hess, grad] = hessian(ext_cost, vertcat(u, x, z)); % add functions to context - context.add_function_definition([model.name,'_cost_ext_cost_fun'], {x, u, z, p}, {ext_cost}, target_dir); - context.add_function_definition([model.name,'_cost_ext_cost_fun_jac'], {x, u, z, p}, {ext_cost, grad}, target_dir); + context.add_function_definition([model.name,'_cost_ext_cost_fun'], {x, u, z, p}, {ext_cost}, target_dir, 'cost'); + context.add_function_definition([model.name,'_cost_ext_cost_fun_jac'], {x, u, z, p}, {ext_cost, grad}, target_dir, 'cost'); if ~isempty(model.cost_expr_ext_cost_custom_hess) context.add_function_definition([model.name,'_cost_ext_cost_fun_jac_hess'], {x, u, z, p}, ... - {ext_cost, grad, model.cost_expr_ext_cost_custom_hess}, target_dir); + {ext_cost, grad, model.cost_expr_ext_cost_custom_hess}, target_dir, 'cost'); else context.add_function_definition([model.name,'_cost_ext_cost_fun_jac_hess'], {x, u, z, p}, ... - {ext_cost, grad, full_hess}, target_dir); + {ext_cost, grad, full_hess}, target_dir, 'cost'); end elseif strcmp(stage_type, "terminal") @@ -85,13 +85,13 @@ function generate_c_code_ext_cost(context, model, target_dir, stage_type) % generate hessians hes_xx_e = jacobian(jac_x_e', x); % add functions to context - context.add_function_definition([model.name,'_cost_ext_cost_e_fun'], {x, p}, {ext_cost_e}, target_dir); - context.add_function_definition([model.name,'_cost_ext_cost_e_fun_jac'], {x, p}, {ext_cost_e, jac_x_e'}, target_dir); + context.add_function_definition([model.name,'_cost_ext_cost_e_fun'], {x, p}, {ext_cost_e}, target_dir, 'cost'); + context.add_function_definition([model.name,'_cost_ext_cost_e_fun_jac'], {x, p}, {ext_cost_e, jac_x_e'}, target_dir, 'cost'); if ~isempty(model.cost_expr_ext_cost_custom_hess_e) context.add_function_definition([model.name,'_cost_ext_cost_e_fun_jac_hess'], {x, p},... - {ext_cost_e, jac_x_e', model.cost_expr_ext_cost_custom_hess_e}, target_dir); + {ext_cost_e, jac_x_e', model.cost_expr_ext_cost_custom_hess_e}, target_dir, 'cost'); else - context.add_function_definition([model.name, '_cost_ext_cost_e_fun_jac_hess'], {x, p}, {ext_cost_e, jac_x_e', hes_xx_e}, target_dir); + context.add_function_definition([model.name, '_cost_ext_cost_e_fun_jac_hess'], {x, p}, {ext_cost_e, jac_x_e', hes_xx_e}, target_dir, 'cost'); end else error("Unknown stage type.") diff --git a/interfaces/acados_matlab_octave/generate_c_code_gnsf.m b/interfaces/acados_matlab_octave/generate_c_code_gnsf.m index a47be859a3..566cc5aedc 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_gnsf.m +++ b/interfaces/acados_matlab_octave/generate_c_code_gnsf.m @@ -90,14 +90,14 @@ function generate_c_code_gnsf(context, model, model_dir) jac_phi_y = jacobian(phi,y); jac_phi_uhat = jacobian(phi,uhat); - context.add_function_definition([model_name,'_gnsf_phi_fun'], {y, uhat, p}, {phi}, model_dir); - context.add_function_definition([model_name,'_gnsf_phi_fun_jac_y'], {y, uhat, p}, {phi, jac_phi_y}, model_dir); - context.add_function_definition([model_name,'_gnsf_phi_jac_y_uhat'], {y, uhat, p}, {jac_phi_y, jac_phi_uhat}, model_dir); + context.add_function_definition([model_name,'_gnsf_phi_fun'], {y, uhat, p}, {phi}, model_dir, 'dyn'); + context.add_function_definition([model_name,'_gnsf_phi_fun_jac_y'], {y, uhat, p}, {phi, jac_phi_y}, model_dir, 'dyn'); + context.add_function_definition([model_name,'_gnsf_phi_jac_y_uhat'], {y, uhat, p}, {jac_phi_y, jac_phi_uhat}, model_dir, 'dyn'); if nontrivial_f_LO context.add_function_definition([model_name,'_gnsf_f_lo_fun_jac_x1k1uz'], {x1, x1dot, z1, u, p}, ... - {f_lo, [jacobian(f_lo,x1), jacobian(f_lo,x1dot), jacobian(f_lo,u), jacobian(f_lo,z1)]}, model_dir); + {f_lo, [jacobian(f_lo,x1), jacobian(f_lo,x1dot), jacobian(f_lo,u), jacobian(f_lo,z1)]}, model_dir, 'dyn'); end end @@ -105,5 +105,5 @@ function generate_c_code_gnsf(context, model, model_dir) dummy = x(1); context.add_function_definition([model_name,'_gnsf_get_matrices_fun'], {dummy},... {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,... - nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO}, model_dir); + nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO}, model_dir, 'dyn'); end diff --git a/interfaces/acados_matlab_octave/generate_c_code_implicit_ode.m b/interfaces/acados_matlab_octave/generate_c_code_implicit_ode.m index 1845dcb868..c8f654854f 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_implicit_ode.m +++ b/interfaces/acados_matlab_octave/generate_c_code_implicit_ode.m @@ -76,19 +76,19 @@ function generate_c_code_implicit_ode(context, model, model_dir) end fun_name = [model.name,'_impl_dae_fun']; - context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl}, model_dir); + context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl}, model_dir, 'dyn'); fun_name = [model.name,'_impl_dae_fun_jac_x_xdot_z']; - context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl, jac_x, jac_xdot, jac_z}, model_dir); + context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl, jac_x, jac_xdot, jac_z}, model_dir, 'dyn'); fun_name = [model.name,'_impl_dae_jac_x_xdot_u_z']; - context.add_function_definition(fun_name, {x, xdot, u, z, p}, {jac_x, jac_xdot, jac_u, jac_z}, model_dir); + context.add_function_definition(fun_name, {x, xdot, u, z, p}, {jac_x, jac_xdot, jac_u, jac_z}, model_dir, 'dyn'); fun_name = [model.name,'_impl_dae_fun_jac_x_xdot_u']; - context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl, jac_x, jac_xdot, jac_u}, model_dir); + context.add_function_definition(fun_name, {x, xdot, u, z, p}, {f_impl, jac_x, jac_xdot, jac_u}, model_dir, 'dyn'); if context.opts.generate_hess fun_name = [model.name,'_impl_dae_hess']; - context.add_function_definition(fun_name, {x, xdot, u, z, multiplier, p}, {HESS}, model_dir); + context.add_function_definition(fun_name, {x, xdot, u, z, multiplier, p}, {HESS}, model_dir, 'dyn'); end end \ No newline at end of file diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m index 24199f5fa2..cc20f83d25 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m @@ -69,10 +69,10 @@ function generate_c_code_nonlinear_constr(context, model, target_dir, stage_type hess_z_0 = jacobian(adj_z_0, z, struct('symmetric', isSX)); % add functions to context - context.add_function_definition([model.name,'_constr_h_0_fun'], {x, u, z, p}, {h_0}, target_dir); - context.add_function_definition([model.name,'_constr_h_0_fun_jac_uxt_zt'], {x, u, z, p}, {h_0, jac_ux_0', jac_z_0'}, target_dir); + context.add_function_definition([model.name,'_constr_h_0_fun'], {x, u, z, p}, {h_0}, target_dir, 'constr'); + context.add_function_definition([model.name,'_constr_h_0_fun_jac_uxt_zt'], {x, u, z, p}, {h_0, jac_ux_0', jac_z_0'}, target_dir, 'constr'); if context.opts.generate_hess - context.add_function_definition([model.name,'_constr_h_0_fun_jac_uxt_zt_hess'], {x, u, lam_h_0, z, p}, {h_0, jac_ux_0', hess_ux_0, jac_z_0', hess_z_0}, target_dir); + context.add_function_definition([model.name,'_constr_h_0_fun_jac_uxt_zt_hess'], {x, u, lam_h_0, z, p}, {h_0, jac_ux_0', hess_ux_0, jac_z_0', hess_z_0}, target_dir, 'constr'); end elseif strcmp(stage_type, 'path') @@ -99,12 +99,12 @@ function generate_c_code_nonlinear_constr(context, model, target_dir, stage_type hess_z = jacobian(adj_z, z, struct('symmetric', isSX)); % add functions to context - context.add_function_definition([model.name,'_constr_h_fun'], {x, u, z, p}, {h}, target_dir); - context.add_function_definition([model.name,'_constr_h_fun_jac_uxt_zt'], {x, u, z, p}, {h, jac_ux', jac_z'}, target_dir); + context.add_function_definition([model.name,'_constr_h_fun'], {x, u, z, p}, {h}, target_dir, 'constr'); + context.add_function_definition([model.name,'_constr_h_fun_jac_uxt_zt'], {x, u, z, p}, {h, jac_ux', jac_z'}, target_dir, 'constr'); if context.opts.generate_hess context.add_function_definition([model.name,'_constr_h_fun_jac_uxt_zt_hess'],... - {x, u, lam_h, z, p}, {h, jac_ux', hess_ux, jac_z', hess_z}, target_dir); + {x, u, lam_h, z, p}, {h, jac_ux', hess_ux, jac_z', hess_z}, target_dir, 'constr'); end elseif strcmp(stage_type, 'terminal') @@ -124,11 +124,11 @@ function generate_c_code_nonlinear_constr(context, model, target_dir, stage_type % generate hessian hess_ux_e = jacobian(adj_ux_e, x, struct('symmetric', isSX)); % add functions to context - context.add_function_definition([model.name,'_constr_h_e_fun'], {x, p}, {h_e}, target_dir); - context.add_function_definition([model.name,'_constr_h_e_fun_jac_uxt_zt'], {x, p}, {h_e, jac_x_e'}, target_dir); + context.add_function_definition([model.name,'_constr_h_e_fun'], {x, p}, {h_e}, target_dir, 'constr'); + context.add_function_definition([model.name,'_constr_h_e_fun_jac_uxt_zt'], {x, p}, {h_e, jac_x_e'}, target_dir, 'constr'); if context.opts.generate_hess - context.add_function_definition([model.name,'_constr_h_e_fun_jac_uxt_zt_hess'], {x, lam_h_e, p}, {h_e, jac_x_e', hess_ux_e}, target_dir); + context.add_function_definition([model.name,'_constr_h_e_fun_jac_uxt_zt_hess'], {x, lam_h_e, p}, {h_e, jac_x_e', hess_ux_e}, target_dir, 'constr'); end end end diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m index 81ee3cddee..4fae2178fc 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m @@ -73,10 +73,10 @@ function generate_c_code_nonlinear_least_squares(context, model, target_dir, sta y_0_hess = jacobian(y_0_adj, [u; x], struct('symmetric', isSX)); dy_dz = jacobian(fun, z); % add functions to context - context.add_function_definition([model.name,'_cost_y_0_fun'], {x, u, z, p}, {fun}, target_dir); - context.add_function_definition([model.name,'_cost_y_0_fun_jac_ut_xt'], {x, u, z, p}, {fun, [jac_u'; jac_x'], dy_dz}, target_dir); + context.add_function_definition([model.name,'_cost_y_0_fun'], {x, u, z, p}, {fun}, target_dir, 'cost'); + context.add_function_definition([model.name,'_cost_y_0_fun_jac_ut_xt'], {x, u, z, p}, {fun, [jac_u'; jac_x'], dy_dz}, target_dir, 'cost'); if context.opts.generate_hess - context.add_function_definition([model.name,'_cost_y_0_hess'], {x, u, z, y_0, p}, {y_0_hess}, target_dir); + context.add_function_definition([model.name,'_cost_y_0_hess'], {x, u, z, y_0, p}, {y_0_hess}, target_dir, 'cost'); end elseif strcmp(stage_type, 'path') fun = model.cost_y_expr; @@ -99,12 +99,12 @@ function generate_c_code_nonlinear_least_squares(context, model, target_dir, sta y_hess = jacobian(y_adj, [u; x], struct('symmetric', isSX)); dy_dz = jacobian(fun, z); % add functions to context - context.add_function_definition([model.name,'_cost_y_fun'], {x, u, z, p}, {fun}, target_dir); + context.add_function_definition([model.name,'_cost_y_fun'], {x, u, z, p}, {fun}, target_dir, 'cost'); context.add_function_definition([model.name,'_cost_y_fun_jac_ut_xt'], ... - {x, u, z, p}, {fun, [jac_u'; jac_x'], dy_dz}, target_dir); + {x, u, z, p}, {fun, [jac_u'; jac_x'], dy_dz}, target_dir, 'cost'); if context.opts.generate_hess - context.add_function_definition([model.name,'_cost_y_hess'], {x, u, z, y, p}, {y_hess}, target_dir); + context.add_function_definition([model.name,'_cost_y_hess'], {x, u, z, y, p}, {y_hess}, target_dir, 'cost'); end elseif strcmp(stage_type, 'terminal') @@ -129,11 +129,11 @@ function generate_c_code_nonlinear_least_squares(context, model, target_dir, sta y_e_adj = jtimes(fun, x, y_e, true); y_e_hess = jacobian(y_e_adj, x, struct('symmetric', isSX)); % add functions to context - context.add_function_definition([model.name,'_cost_y_e_fun'], {x, u, z, p}, {fun}, target_dir); - context.add_function_definition([model.name,'_cost_y_e_fun_jac_ut_xt'], {x, u, z, p}, {fun, jac_x', dy_dz}, target_dir); + context.add_function_definition([model.name,'_cost_y_e_fun'], {x, u, z, p}, {fun}, target_dir, 'cost'); + context.add_function_definition([model.name,'_cost_y_e_fun_jac_ut_xt'], {x, u, z, p}, {fun, jac_x', dy_dz}, target_dir, 'cost'); if context.opts.generate_hess - context.add_function_definition([model.name,'_cost_y_e_hess'], {x, u, z, y_e, p}, {y_e_hess}, target_dir); + context.add_function_definition([model.name,'_cost_y_e_hess'], {x, u, z, y_e, p}, {y_e_hess}, target_dir, 'cost'); end end diff --git a/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m b/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m index 226e81c12d..8e78c15e03 100644 --- a/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m +++ b/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m @@ -129,7 +129,10 @@ ocp.solver_options.fixed_hess = opts_struct.fixed_hess; ocp.solver_options.ext_fun_compile_flags = opts_struct.ext_fun_compile_flags; - ocp.solver_options.ext_fun_expand = opts_struct.ext_fun_expand; + ocp.solver_options.ext_fun_expand_dyn = opts_struct.ext_fun_expand_dyn + ocp.solver_options.ext_fun_expand_cost = opts_struct.ext_fun_expand_cost + ocp.solver_options.ext_fun_expand_constr = opts_struct.ext_fun_expand_constr + ocp.solver_options.ext_fun_expand_precompute = opts_struct.ext_fun_expand_precompute ocp.solver_options.time_steps = opts_struct.time_steps; ocp.solver_options.shooting_nodes = opts_struct.shooting_nodes; diff --git a/interfaces/acados_matlab_octave/setup_AcadosSim_from_legacy_sim_description.m b/interfaces/acados_matlab_octave/setup_AcadosSim_from_legacy_sim_description.m index 3d07bf9c93..b355e20df0 100644 --- a/interfaces/acados_matlab_octave/setup_AcadosSim_from_legacy_sim_description.m +++ b/interfaces/acados_matlab_octave/setup_AcadosSim_from_legacy_sim_description.m @@ -166,7 +166,7 @@ sim.solver_options.output_z = str2bool(opts.output_z); sim.solver_options.jac_reuse = str2bool(opts.jac_reuse); sim.solver_options.ext_fun_compile_flags = opts.ext_fun_compile_flags; - sim.solver_options.ext_fun_expand = opts.ext_fun_expand; + sim.solver_options.ext_fun_expand_dyn = opts.ext_fun_expand_dyn; end diff --git a/interfaces/acados_matlab_octave/sim_generate_c_code.m b/interfaces/acados_matlab_octave/sim_generate_c_code.m index 6675da09d9..faeaeadd1e 100644 --- a/interfaces/acados_matlab_octave/sim_generate_c_code.m +++ b/interfaces/acados_matlab_octave/sim_generate_c_code.m @@ -36,7 +36,10 @@ function sim_generate_c_code(sim, context) code_gen_opts = struct(); code_gen_opts.generate_hess = sim.solver_options.sens_hess; code_gen_opts.code_export_directory = 'c_generated_code'; % TODO: for OCP this is part of OCP class - code_gen_opts.ext_fun_expand = sim.solver_options.ext_fun_expand; + code_gen_opts.ext_fun_expand_dyn = sim.solver_options.ext_fun_expand_dyn; + code_gen_opts.ext_fun_expand_cost = false; + code_gen_opts.ext_fun_expand_constr = false; + code_gen_opts.ext_fun_expand_precompute = false; context = GenerateContext(sim.model.p_global, sim.model.name, code_gen_opts); else diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index 4311086684..f87ce9c03a 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -42,7 +42,7 @@ from .acados_ocp_constraints import AcadosOcpConstraints from .acados_ocp_options import AcadosOcpOptions, INTEGRATOR_TYPES, COLLOCATION_TYPES, COST_DISCRETIZATION_TYPES from .acados_ocp import AcadosOcp -from .casadi_function_generation import GenerateContext +from .casadi_function_generation import GenerateContext, AcadosCodegenOptions from .utils import make_object_json_dumpable, get_acados_path, format_class_dict, get_shared_lib_ext, render_template, is_empty @@ -451,13 +451,16 @@ def render_templates(self, cmake_builder=None): def generate_external_functions(self) -> GenerateContext: # options for code generation - code_gen_opts = dict() - code_gen_opts['generate_hess'] = self.solver_options.hessian_approx == 'EXACT' - code_gen_opts['with_solution_sens_wrt_params'] = self.solver_options.with_solution_sens_wrt_params - code_gen_opts['with_value_sens_wrt_params'] = self.solver_options.with_value_sens_wrt_params - code_gen_opts['code_export_directory'] = self.code_export_directory - code_gen_opts['ext_fun_expand'] = self.solver_options.ext_fun_expand - + code_gen_opts = AcadosCodegenOptions( + ext_fun_expand_constr = self.solver_options.ext_fun_expand_constr, + ext_fun_expand_cost = self.solver_options.ext_fun_expand_cost, + ext_fun_expand_precompute = self.solver_options.ext_fun_expand_precompute, + ext_fun_expand_dyn = self.solver_options.ext_fun_expand_dyn, + code_export_directory = self.code_export_directory, + with_solution_sens_wrt_params = self.solver_options.with_solution_sens_wrt_params, + with_value_sens_wrt_params = self.solver_options.with_value_sens_wrt_params, + generate_hess = self.solver_options.hessian_approx == 'EXACT', + ) context = GenerateContext(self.model[0].p_global, self.name, code_gen_opts) for i in range(self.n_phases): diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index d909baaad2..8953c69307 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -52,7 +52,7 @@ from .zoro_description import ZoroDescription, process_zoro_description from .casadi_function_generation import ( - GenerateContext, + GenerateContext, AcadosCodegenOptions, generate_c_code_conl_cost, generate_c_code_nls_cost, generate_c_code_external_cost, generate_c_code_explicit_ode, generate_c_code_implicit_ode, generate_c_code_discrete_dynamics, generate_c_code_gnsf, generate_c_code_constraint @@ -1047,12 +1047,16 @@ def generate_external_functions(self, context: Optional[GenerateContext] = None) if context is None: # options for code generation - code_gen_opts = dict() - code_gen_opts['generate_hess'] = self.solver_options.hessian_approx == 'EXACT' - code_gen_opts['with_solution_sens_wrt_params'] = self.solver_options.with_solution_sens_wrt_params - code_gen_opts['with_value_sens_wrt_params'] = self.solver_options.with_value_sens_wrt_params - code_gen_opts['code_export_directory'] = self.code_export_directory - code_gen_opts['ext_fun_expand'] = self.solver_options.ext_fun_expand + code_gen_opts = AcadosCodegenOptions( + ext_fun_expand_constr = self.solver_options.ext_fun_expand_constr, + ext_fun_expand_cost = self.solver_options.ext_fun_expand_cost, + ext_fun_expand_precompute = self.solver_options.ext_fun_expand_precompute, + ext_fun_expand_dyn = self.solver_options.ext_fun_expand_dyn, + code_export_directory = self.code_export_directory, + with_solution_sens_wrt_params = self.solver_options.with_solution_sens_wrt_params, + with_value_sens_wrt_params = self.solver_options.with_value_sens_wrt_params, + generate_hess = self.solver_options.hessian_approx == 'EXACT', + ) context = GenerateContext(self.model.p_global, self.name, code_gen_opts) @@ -1073,7 +1077,7 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia code_gen_opts = context.opts # create code_export_dir, model_dir - model_dir = os.path.join(code_gen_opts['code_export_directory'], model.name + '_model') + model_dir = os.path.join(code_gen_opts.code_export_directory, model.name + '_model') if not os.path.exists(model_dir): os.makedirs(model_dir) @@ -1094,7 +1098,7 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia else: raise Exception("ocp_generate_external_functions: unknown integrator type.") else: - target_dir = os.path.join(code_gen_opts['code_export_directory'], model_dir) + target_dir = os.path.join(code_gen_opts.code_export_directory, model_dir) target_location = os.path.join(target_dir, model.dyn_generic_source) shutil.copyfile(model.dyn_generic_source, target_location) context.add_external_function_file(model.dyn_generic_source, target_dir) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index cdba91ba94..3fa5a8f0a2 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -131,7 +131,10 @@ def __init__(self): # TODO: move those out? they are more about generation than about the acados OCP solver. env = os.environ self.__ext_fun_compile_flags = '-O2' if 'ACADOS_EXT_FUN_COMPILE_FLAGS' not in env else env['ACADOS_EXT_FUN_COMPILE_FLAGS'] - self.__ext_fun_expand = False + self.__ext_fun_expand_constr = False + self.__ext_fun_expand_cost = False + self.__ext_fun_expand_precompute = False + self.__ext_fun_expand_dyn = False self.__model_external_shared_lib_dir = None self.__model_external_shared_lib_name = None self.__custom_update_filename = '' @@ -157,12 +160,36 @@ def ext_fun_compile_flags(self): return self.__ext_fun_compile_flags @property - def ext_fun_expand(self): + def ext_fun_expand_constr(self): """ - Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation. + Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation for constraint functions. Default: False """ - return self.__ext_fun_expand + return self.__ext_fun_expand_constr + + @property + def ext_fun_expand_cost(self): + """ + Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation for cost functions. + Default: False + """ + return self.__ext_fun_expand_cost + + @property + def ext_fun_expand_dyn(self): + """ + Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation for dynamics functions. + Default: False + """ + return self.__ext_fun_expand_dyn + + @property + def ext_fun_expand_precompute(self): + """ + Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation for the precompute function. + Default: False + """ + return self.__ext_fun_expand_precompute @property def custom_update_filename(self): @@ -1247,12 +1274,29 @@ def ext_fun_compile_flags(self, ext_fun_compile_flags): else: raise Exception('Invalid ext_fun_compile_flags value, expected a string.\n') - @ext_fun_expand.setter - def ext_fun_expand(self, ext_fun_expand): - if isinstance(ext_fun_expand, bool): - self.__ext_fun_expand = ext_fun_expand - else: - raise Exception('Invalid ext_fun_expand value, expected bool.\n') + @ext_fun_expand_constr.setter + def ext_fun_expand_constr(self, ext_fun_expand_constr): + if not isinstance(ext_fun_expand_constr, bool): + raise Exception('Invalid ext_fun_expand_constr value, expected bool.\n') + self.__ext_fun_expand_constr = ext_fun_expand_constr + + @ext_fun_expand_cost.setter + def ext_fun_expand_cost(self, ext_fun_expand_cost): + if not isinstance(ext_fun_expand_cost, bool): + raise Exception('Invalid ext_fun_expand_cost value, expected bool.\n') + self.__ext_fun_expand_cost = ext_fun_expand_cost + + @ext_fun_expand_dyn.setter + def ext_fun_expand_dyn(self, ext_fun_expand_dyn): + if not isinstance(ext_fun_expand_dyn, bool): + raise Exception('Invalid ext_fun_expand_dyn value, expected bool.\n') + self.__ext_fun_expand_dyn = ext_fun_expand_dyn + + @ext_fun_expand_precompute.setter + def ext_fun_expand_precompute(self, ext_fun_expand_precompute): + if not isinstance(ext_fun_expand_precompute, bool): + raise Exception('Invalid ext_fun_expand_precompute value, expected bool.\n') + self.__ext_fun_expand_precompute = ext_fun_expand_precompute @custom_update_filename.setter def custom_update_filename(self, custom_update_filename): diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 18f68507ee..8c83e3dcdd 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -39,6 +39,7 @@ make_object_json_dumpable, render_template) from .casadi_function_generation import ( GenerateContext, + AcadosCodegenOptions, generate_c_code_explicit_ode, generate_c_code_gnsf, generate_c_code_implicit_ode) @@ -66,7 +67,7 @@ def __init__(self): self.__sim_method_jac_reuse = 0 env = os.environ self.__ext_fun_compile_flags = '-O2' if 'ACADOS_EXT_FUN_COMPILE_FLAGS' not in env else env['ACADOS_EXT_FUN_COMPILE_FLAGS'] - self.__ext_fun_expand = False + self.__ext_fun_expand_dyn = False self.__num_threads_in_batch_solve: int = 1 @property @@ -152,12 +153,12 @@ def ext_fun_compile_flags(self): @property - def ext_fun_expand(self): + def ext_fun_expand_dyn(self): """ Flag indicating whether CasADi.MX should be expanded to CasADi.SX before code generation. Default: False """ - return self.__ext_fun_expand + return self.__ext_fun_expand_dyn @property @@ -177,12 +178,12 @@ def ext_fun_compile_flags(self, ext_fun_compile_flags): else: raise Exception('Invalid ext_fun_compile_flags value, expected a string.\n') - @ext_fun_expand.setter - def ext_fun_expand(self, ext_fun_expand): - if isinstance(ext_fun_expand, bool): - self.__ext_fun_expand = ext_fun_expand + @ext_fun_expand_dyn.setter + def ext_fun_expand_dyn(self, ext_fun_expand_dyn): + if isinstance(ext_fun_expand_dyn, bool): + self.__ext_fun_expand_dyn = ext_fun_expand_dyn else: - raise Exception('Invalid ext_fun_expand value, expected bool.\n') + raise Exception('Invalid ext_fun_expand_dyn value, expected bool.\n') @integrator_type.setter def integrator_type(self, integrator_type): @@ -403,14 +404,17 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): def generate_external_functions(self, ): integrator_type = self.solver_options.integrator_type + code_export_dir = self.code_export_directory - opts = dict(generate_hess = self.solver_options.sens_hess, + opts = AcadosCodegenOptions(generate_hess = self.solver_options.sens_hess, code_export_directory = self.code_export_directory, - ext_fun_expand = self.solver_options.ext_fun_expand) + ext_fun_expand_dyn = self.solver_options.ext_fun_expand_dyn, + ext_fun_expand_cost = False, + ext_fun_expand_constr = False, + ext_fun_expand_precompute = False, + ) # create code_export_dir, model_dir - code_export_dir = self.code_export_directory - opts['code_export_directory'] = code_export_dir model_dir = os.path.join(code_export_dir, self.model.name + '_model') if not os.path.exists(model_dir): os.makedirs(model_dir) diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index dbf5d44b06..3f60a2e82c 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -29,6 +29,7 @@ # from typing import Union, List, Optional +from dataclasses import dataclass import os, warnings import casadi as ca @@ -42,9 +43,19 @@ def is_casadi_SX(x): return True return False +@dataclass +class AcadosCodegenOptions: + ext_fun_expand_constr: bool = False + ext_fun_expand_cost: bool = False + ext_fun_expand_dyn: bool = False + ext_fun_expand_precompute: bool = False + code_export_directory: str = "c_generated_code" + with_solution_sens_wrt_params: bool = False + with_value_sens_wrt_params: bool = False + generate_hess: bool = True class GenerateContext: - def __init__(self, p_global: Optional[Union[ca.SX, ca.MX]], problem_name: str, opts=None): + def __init__(self, p_global: Optional[Union[ca.SX, ca.MX]], problem_name: str, opts: AcadosCodegenOptions): self.p_global = p_global if not is_empty(p_global): check_casadi_version_supports_p_global() @@ -65,6 +76,7 @@ def __init__(self, p_global: Optional[Union[ca.SX, ca.MX]], problem_name: str, o self.generic_funname_dir_pairs = [] # list of (function_name, output_dir) of functions that are not generated by acados self.function_input_output_pairs: List[List[Union[ca.SX, ca.MX], Union[ca.SX, ca.MX]]] = [] + self.dyn_cost_constr_types = [] self.global_data_sym = None self.global_data_expr = None @@ -81,7 +93,7 @@ def __init__(self, p_global: Optional[Union[ca.SX, ca.MX]], problem_name: str, o def __generate_functions(self): - for (name, output_dir), (inputs, outputs) in zip(self.list_funname_dir_pairs, self.function_input_output_pairs): + for (name, output_dir), (inputs, outputs), dyn_cost_constr_type in zip(self.list_funname_dir_pairs, self.function_input_output_pairs, self.dyn_cost_constr_types): # create function try: fun = ca.Function(name, inputs, outputs, self.__casadi_fun_opts) @@ -92,7 +104,10 @@ def __generate_functions(self): raise e # expand function to SX - if "p_global_precompute" not in name and self.opts["ext_fun_expand"]: + if ((dyn_cost_constr_type == 'dyn' and self.opts.ext_fun_expand_dyn) + or (dyn_cost_constr_type == 'cost' and self.opts.ext_fun_expand_cost) + or (dyn_cost_constr_type == 'constr' and self.opts.ext_fun_expand_constr) + or (dyn_cost_constr_type == 'precompute' and self.opts.ext_fun_expand_precompute)): try: fun = fun.expand() except: @@ -121,9 +136,11 @@ def add_function_definition(self, name: str, inputs: List[Union[ca.MX, ca.SX]], outputs: List[Union[ca.MX, ca.SX]], - output_dir: str): + output_dir: str, + dyn_cost_constr_type: str): self.list_funname_dir_pairs.append((name, output_dir)) self.function_input_output_pairs.append([inputs, outputs]) + self.dyn_cost_constr_types.append(dyn_cost_constr_type) def __setup_p_global_precompute_fun(self): precompute_pairs = [] @@ -164,9 +181,9 @@ def __setup_p_global_precompute_fun(self): for i in range(len(self.function_input_output_pairs)): self.function_input_output_pairs[i][0].append(self.global_data_sym) - output_dir = os.path.abspath(self.opts["code_export_directory"]) + output_dir = os.path.abspath(self.opts.code_export_directory) fun_name = f'{self.problem_name}_p_global_precompute_fun' - self.add_function_definition(fun_name, [self.p_global], [self.global_data_expr], output_dir) + self.add_function_definition(fun_name, [self.p_global], [self.global_data_expr], output_dir, 'precompute') else: print("WARNING: No CasADi function depends on p_global.") @@ -186,7 +203,7 @@ def get_n_global_data(self): def get_external_function_file_list(self, ocp_specific=False): out = [] for (fun_name, fun_dir) in self.generic_funname_dir_pairs + self.list_funname_dir_pairs: - rel_fun_dir = os.path.relpath(fun_dir, self.opts["code_export_directory"]) + rel_fun_dir = os.path.relpath(fun_dir, self.opts.code_export_directory) is_ocp_specific = not rel_fun_dir.endswith("model") if ocp_specific != is_ocp_specific: continue @@ -230,33 +247,33 @@ def generate_c_code_discrete_dynamics(context: GenerateContext, model: AcadosMod # set up & generate ca.Functions fun_name = model_name + '_dyn_disc_phi_fun' - context.add_function_definition(fun_name, [x, u, p], [phi], model_dir) + context.add_function_definition(fun_name, [x, u, p], [phi], model_dir, 'dyn') fun_name = model_name + '_dyn_disc_phi_fun_jac' - context.add_function_definition(fun_name, [x, u, p], [phi, jac_ux.T], model_dir) + context.add_function_definition(fun_name, [x, u, p], [phi, jac_ux.T], model_dir, 'dyn') fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' - context.add_function_definition(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux], model_dir) + context.add_function_definition(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux], model_dir, 'dyn') - if opts["with_solution_sens_wrt_params"]: + if opts.with_solution_sens_wrt_params: # generate jacobian of lagrange gradient wrt p jac_p = ca.jacobian(phi, p_global) # hess_xu_p_old = ca.jacobian((lam.T @ jac_ux).T, p) hess_xu_p = ca.jacobian(adj_ux, p_global) # using adjoint fun_name = model_name + '_dyn_disc_phi_jac_p_hess_xu_p' - context.add_function_definition(fun_name, [x, u, lam, p], [jac_p, hess_xu_p], model_dir) + context.add_function_definition(fun_name, [x, u, lam, p], [jac_p, hess_xu_p], model_dir, 'dyn') - if opts["with_value_sens_wrt_params"]: + if opts.with_value_sens_wrt_params: adj_p = ca.jtimes(phi, p_global, lam, True) fun_name = model_name + '_dyn_disc_phi_adj_p' - context.add_function_definition(fun_name, [x, u, lam, p], [adj_p], model_dir) + context.add_function_definition(fun_name, [x, u, lam, p], [adj_p], model_dir, 'dyn') return def generate_c_code_explicit_ode(context: GenerateContext, model: AcadosModel, model_dir: str): - generate_hess = context.opts["generate_hess"] + generate_hess = context.opts.generate_hess # load model x = model.x @@ -289,17 +306,17 @@ def generate_c_code_explicit_ode(context: GenerateContext, model: AcadosModel, m # add to context fun_name = model_name + '_expl_ode_fun' - context.add_function_definition(fun_name, [x, u, p], [f_expl], model_dir) + context.add_function_definition(fun_name, [x, u, p], [f_expl], model_dir, 'dyn') fun_name = model_name + '_expl_vde_forw' - context.add_function_definition(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP], model_dir) + context.add_function_definition(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP], model_dir, 'dyn') fun_name = model_name + '_expl_vde_adj' - context.add_function_definition(fun_name, [x, lambdaX, u, p], [adj], model_dir) + context.add_function_definition(fun_name, [x, lambdaX, u, p], [adj], model_dir, 'dyn') if generate_hess: fun_name = model_name + '_expl_ode_hess' - context.add_function_definition(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2], model_dir) + context.add_function_definition(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2], model_dir, 'dyn') return @@ -329,28 +346,28 @@ def generate_c_code_implicit_ode(context: GenerateContext, model: AcadosModel, m # Set up functions p = model.p fun_name = model_name + '_impl_dae_fun' - context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl], model_dir, 'dyn') fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_z], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_z], model_dir, 'dyn') fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' - context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z], model_dir, 'dyn') fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' - context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_u], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl, jac_x, jac_xdot, jac_u], model_dir, 'dyn') fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' - context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [jac_x, jac_xdot, jac_u, jac_z], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [jac_x, jac_xdot, jac_u, jac_z], model_dir, 'dyn') - if context.opts["generate_hess"]: + if context.opts.generate_hess: x_xdot_z_u = ca.vertcat(x, xdot, z, u) symbol = model.get_casadi_symbol() multiplier = symbol('multiplier', nx + nz) ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True) HESS = ca.jacobian(ADJ, x_xdot_z_u, {"symmetric": is_casadi_SX(x)}) fun_name = model_name + '_impl_dae_hess' - context.add_function_definition(fun_name, [x, xdot, u, z, multiplier, t, p], [HESS], model_dir) + context.add_function_definition(fun_name, [x, xdot, u, z, multiplier, t, p], [HESS], model_dir, 'dyn') return @@ -386,15 +403,15 @@ def generate_c_code_gnsf(context: GenerateContext, model: AcadosModel, model_dir ## generate C code fun_name = model_name + '_gnsf_phi_fun' - context.add_function_definition(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)], model_dir) + context.add_function_definition(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)], model_dir, 'dyn') fun_name = model_name + '_gnsf_phi_fun_jac_y' phi_fun_jac_y = model.phi_fun_jac_y - context.add_function_definition(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p), model_dir) + context.add_function_definition(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p), model_dir, 'dyn') fun_name = model_name + '_gnsf_phi_jac_y_uhat' phi_jac_y_uhat = model.phi_jac_y_uhat - context.add_function_definition(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p), model_dir) + context.add_function_definition(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p), model_dir, 'dyn') fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz @@ -404,10 +421,10 @@ def generate_c_code_gnsf(context: GenerateContext, model: AcadosModel, model_dir if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): f_lo_fun_jac_x1k1uz_eval = [empty_var] - context.add_function_definition(fun_name, [x1, x1dot, z1, u, p], f_lo_fun_jac_x1k1uz_eval, model_dir) + context.add_function_definition(fun_name, [x1, x1dot, z1, u, p], f_lo_fun_jac_x1k1uz_eval, model_dir, 'dyn') fun_name = model_name + '_gnsf_get_matrices_fun' - context.add_function_definition(fun_name, [dummy], get_matrices_fun(1), model_dir) + context.add_function_definition(fun_name, [dummy], get_matrices_fun(1), model_dir, 'dyn') # remove fields for json dump del model.phi_fun @@ -482,22 +499,22 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, if not is_empty(custom_hess): hess_ux = custom_hess - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) + cost_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_cost')) - context.add_function_definition(fun_name, [x, u, z, p], [ext_cost], cost_dir) - context.add_function_definition(fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux], cost_dir) - context.add_function_definition(fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz], cost_dir) + context.add_function_definition(fun_name, [x, u, z, p], [ext_cost], cost_dir, 'cost') + context.add_function_definition(fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux], cost_dir, 'cost') + context.add_function_definition(fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz], cost_dir, 'cost') - if opts["with_solution_sens_wrt_params"]: + if opts.with_solution_sens_wrt_params: if casadi_length(z) > 0: raise Exception("acados: solution sensitivities wrt parameters not supported with algebraic variables.") grad_ux = ca.jacobian(ext_cost, ca.vertcat(u, x)) hess_xu_p = ca.jacobian(grad_ux, p_global) - context.add_function_definition(fun_name_param, [x, u, z, p], [hess_xu_p], cost_dir) + context.add_function_definition(fun_name_param, [x, u, z, p], [hess_xu_p], cost_dir, 'cost') - if opts["with_value_sens_wrt_params"]: + if opts.with_value_sens_wrt_params: grad_p = ca.jacobian(ext_cost, p_global).T - context.add_function_definition(fun_name_value_sens, [x, u, z, p], [grad_p], cost_dir) + context.add_function_definition(fun_name_value_sens, [x, u, z, p], [grad_p], cost_dir, 'cost') return @@ -526,7 +543,7 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage middle_name = '_cost_y' y_expr = model.cost_y_expr - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) + cost_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_cost')) # set up expressions cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x))) @@ -545,15 +562,15 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage ## generate C code suffix_name = '_fun' fun_name = model.name + middle_name + suffix_name - context.add_function_definition(fun_name, [x, u, z, t, p], [ y_expr ], cost_dir) + context.add_function_definition(fun_name, [x, u, z, t, p], [ y_expr ], cost_dir, 'cost') suffix_name = '_fun_jac_ut_xt' fun_name = model.name + middle_name + suffix_name - context.add_function_definition(fun_name, [x, u, z, t, p], [ y_expr, cost_jac_expr, dy_dz ], cost_dir) + context.add_function_definition(fun_name, [x, u, z, t, p], [ y_expr, cost_jac_expr, dy_dz ], cost_dir, 'cost') suffix_name = '_hess' fun_name = model.name + middle_name + suffix_name - context.add_function_definition(fun_name, [x, u, z, y, t, p], [ y_hess ], cost_dir) + context.add_function_definition(fun_name, [x, u, z, y, t, p], [ y_hess ], cost_dir, 'cost') return @@ -635,18 +652,18 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag Jt_z_expr = ca.jacobian(inner_expr, z).T # change directory - cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) + cost_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_cost')) context.add_function_definition( fun_name_cost_fun, [x, u, z, yref, t, p], - [cost_expr], cost_dir) + [cost_expr], cost_dir, 'cost') context.add_function_definition( fun_name_cost_fun_jac_hess, [x, u, z, yref, t, p], [cost_expr, outer_loss_grad_fun(inner_expr, t, p, p_global), Jt_ux_expr, Jt_z_expr, outer_hess_expr, outer_hess_is_diag], - cost_dir + cost_dir, 'cost' ) return @@ -701,7 +718,7 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con lam_h = symbol('lam_h', nh, 1) # directory - constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints')) + constraints_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_constraints')) # export casadi functions if constr_type == 'BGH': @@ -715,9 +732,9 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x))) jac_z_t = ca.jacobian(con_h_expr, z) context.add_function_definition(fun_name, [x, u, z, p], \ - [con_h_expr, jac_ux_t, jac_z_t], constraints_dir) + [con_h_expr, jac_ux_t, jac_z_t], constraints_dir, 'constr') - if opts['generate_hess']: + if opts.generate_hess: if stage_type == 'terminal': fun_name = model.name + '_constr_h_e_fun_jac_uxt_zt_hess' elif stage_type == 'initial': @@ -734,7 +751,7 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con hess_z = ca.jacobian(adj_z, z, {"symmetric": is_casadi_SX(x)}) context.add_function_definition(fun_name, [x, u, lam_h, z, p], \ - [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z], constraints_dir) + [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z], constraints_dir, 'constr') if stage_type == 'terminal': fun_name = model.name + '_constr_h_e_fun' @@ -742,9 +759,9 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con fun_name = model.name + '_constr_h_0_fun' else: fun_name = model.name + '_constr_h_fun' - context.add_function_definition(fun_name, [x, u, z, p], [con_h_expr], constraints_dir) + context.add_function_definition(fun_name, [x, u, z, p], [con_h_expr], constraints_dir, 'constr') - if opts["with_solution_sens_wrt_params"]: + if opts.with_solution_sens_wrt_params: jac_p = ca.jacobian(con_h_expr, model.p_global) adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True) hess_xu_p = ca.jacobian(adj_ux, model.p_global) @@ -757,9 +774,9 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con fun_name = model.name + '_constr_h_jac_p_hess_xu_p' context.add_function_definition(fun_name, [x, u, lam_h, z, p], \ - [jac_p, hess_xu_p], constraints_dir) + [jac_p, hess_xu_p], constraints_dir, 'constr') - if opts["with_value_sens_wrt_params"]: + if opts.with_value_sens_wrt_params: adj_p = ca.jtimes(con_h_expr, model.p_global, lam_h, True) if stage_type == 'terminal': fun_name = model.name + '_constr_h_e_adj_p' @@ -768,7 +785,7 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con else: fun_name = model.name + '_constr_h_adj_p' - context.add_function_definition(fun_name, [x, u, lam_h, p], [adj_p], constraints_dir) + context.add_function_definition(fun_name, [x, u, lam_h, p], [adj_p], constraints_dir, 'constr') else: # BGP constraint if stage_type == 'terminal': @@ -803,11 +820,12 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con ca.transpose(phi_jac_z), \ hess, ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))], - constraints_dir + constraints_dir, + 'constr', ) fun_name = fun_name_prefix + '_fun' - context.add_function_definition(fun_name, [x, u, z, p], [con_phi_expr_x_u_z], constraints_dir) + context.add_function_definition(fun_name, [x, u, z, p], [con_phi_expr_x_u_z], constraints_dir, 'constr') return From 645521ed1ef7a9289c5044e865e15c9358f9f4da Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 18 Mar 2025 15:09:03 +0100 Subject: [PATCH 002/164] C: Minor cleanup (#1465) --- acados/dense_qp/dense_qp_hpipm.h | 4 +-- acados/ocp_nlp/ocp_nlp_common.c | 16 ++++----- acados/ocp_nlp/ocp_nlp_common.h | 2 +- acados/ocp_nlp/ocp_nlp_ddp.c | 2 +- ...ocp_nlp_globalization_merit_backtracking.c | 2 +- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 36 ++----------------- acados/ocp_nlp/ocp_nlp_reg_convexify.h | 2 +- acados/ocp_nlp/ocp_nlp_sqp.c | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 18 +++++----- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 2 +- acados/utils/mem.h | 12 ------- 11 files changed, 28 insertions(+), 70 deletions(-) diff --git a/acados/dense_qp/dense_qp_hpipm.h b/acados/dense_qp/dense_qp_hpipm.h index 9b3d3f3174..7d05fe5138 100644 --- a/acados/dense_qp/dense_qp_hpipm.h +++ b/acados/dense_qp/dense_qp_hpipm.h @@ -73,9 +73,9 @@ void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts // void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_); // -acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_); +acados_size_t dense_qp_hpipm_memory_calculate_size(void *config_, void *dims_, void *opts_); // -void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory); +void *dense_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void *raw_memory); // acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_); // diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 53e237c5b4..1ffbce8f55 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1727,7 +1727,7 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims c_ptr += qp_solver->memory_calculate_size(qp_solver, dims->qp_solver, opts->qp_solver_opts); // regularization - mem->regularize_mem = config->regularize->memory_assign(config->regularize, dims->regularize, + mem->regularize = config->regularize->memory_assign(config->regularize, dims->regularize, opts->regularize, c_ptr); c_ptr += config->regularize->memory_calculate_size(config->regularize, dims->regularize, opts->regularize); @@ -2619,8 +2619,8 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di } // alias to regularize memory - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->qp_in); - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->qp_out); + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); // copy sampling times into dynamics model #if defined(ACADOS_WITH_OPENMP) @@ -3789,7 +3789,7 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims else { qp_in = qp_in_; - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, qp_in); + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, qp_in); } ocp_qp_out *qp_out = nlp_mem->qp_out; @@ -3800,7 +3800,7 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims else { qp_out = qp_out_; - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, qp_out); + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, qp_out); } ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings; @@ -3831,17 +3831,17 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims // compute correct dual solution in case of Hessian regularization acados_tic(&timer); config->regularize->correct_dual_sol(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize_mem); + nlp_opts->regularize, nlp_mem->regularize); nlp_timings->time_reg += acados_toc(&timer); // reset regularize pointers if necessary if (qp_in_ != NULL) { - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->qp_in); + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); } if (qp_out_ != NULL) { - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->qp_out); + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); } return qp_status; diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index b1a05e8ca9..669c890d1f 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -395,7 +395,7 @@ typedef struct ocp_nlp_memory { // void *qp_solver_mem; // xcond solver mem instead ??? ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ??? - void *regularize_mem; + void *regularize; void *globalization; // globalization memory void **dynamics; // dynamics memory void **cost; // cost memory diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index 4ec0983ce4..13143d2a22 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -755,7 +755,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // NOTE: this is done before termination, such that we can get the QP at the stationary point that is actually solved, if we exit with success. acados_tic(&timer1); config->regularize->regularize(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize_mem); + nlp_opts->regularize, nlp_mem->regularize); nlp_timings->time_reg += acados_toc(&timer1); // Termination diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index c1f74f72ef..1b7e3faa42 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -555,7 +555,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // compute correct dual solution in case of Hessian regularization config->regularize->correct_dual_sol(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize_mem); + nlp_opts->regularize, nlp_mem->regularize); // ocp_qp_out_get(qp_out, "qp_info", &qp_info_); // int qp_iter = qp_info_->num_iter; diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index 6e1534a985..bbb1cc2e24 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -105,7 +105,7 @@ void ocp_nlp_reg_convexify_opts_set(void *config_, void *opts_, const char *fiel * memory ************************************************/ -acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +acados_size_t ocp_nlp_reg_convexify_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) { int N = dims->N; @@ -288,7 +288,7 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, // assign_and_advance_blasfeo_dvec_mem(nxM, &mem->grad, &c_ptr); // assign_and_advance_blasfeo_dvec_mem(nxM, &mem->b2, &c_ptr); - assert((char *)mem + ocp_nlp_reg_convexify_calculate_memory_size(config_, dims, opts_) >= c_ptr); + assert((char *)mem + ocp_nlp_reg_convexify_memory_calculate_size(config_, dims, opts_) >= c_ptr); return mem; } @@ -300,13 +300,11 @@ void ocp_nlp_reg_convexify_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct b ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; for(ii=0; ii<=N; ii++) { memory->RSQrq[ii] = RSQrq+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0); } return; @@ -319,15 +317,11 @@ void ocp_nlp_reg_convexify_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blas ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; ii<=N; ii++) { memory->rq[ii] = rq+ii; -// blasfeo_print_dvec(nu[ii]+nx[ii], memory->rq[ii], 0); } return; @@ -340,15 +334,11 @@ void ocp_nlp_reg_convexify_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct bl ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; iiBAbt[ii] = BAbt+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nx[ii+1], memory->BAbt[ii], 0, 0); } return; @@ -361,15 +351,11 @@ void ocp_nlp_reg_convexify_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasf ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; iib[ii] = b+ii; -// blasfeo_print_dvec(nx[ii=1], memory->b[ii], 0); } return; @@ -382,7 +368,6 @@ void ocp_nlp_reg_convexify_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idx ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; for(ii=0; ii<=N; ii++) @@ -400,13 +385,11 @@ void ocp_nlp_reg_convexify_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct bla ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; for(ii=0; ii<=N; ii++) { memory->DCt[ii] = DCt+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, ng[ii], memory->DCt[ii], 0, 0); } return; @@ -419,15 +402,11 @@ void ocp_nlp_reg_convexify_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blas ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; ii<=N; ii++) { memory->ux[ii] = ux+ii; -// blasfeo_print_dvec(nu[ii]+nx[ii], memory->ux[ii], 0); } return; @@ -440,15 +419,11 @@ void ocp_nlp_reg_convexify_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blas ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; iipi[ii] = pi+ii; -// blasfeo_print_dvec(nx[ii+1], memory->pi[ii], 0); } return; @@ -461,16 +436,11 @@ void ocp_nlp_reg_convexify_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct bla ocp_nlp_reg_convexify_memory *memory = memory_; int ii; - int N = dims->N; - // int *nbu = dims->nbu; - // int *nbx = dims->nbx; - // int *ng = dims->ng; for(ii=0; ii<=N; ii++) { memory->lam[ii] = lam+ii; -// blasfeo_print_dvec(2*nbu[ii]+2*nbx[ii]+2*ng[ii], memory->lam[ii], 0); } return; @@ -944,7 +914,7 @@ void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config) config->opts_initialize_default = &ocp_nlp_reg_convexify_opts_initialize_default; config->opts_set = &ocp_nlp_reg_convexify_opts_set; // memory - config->memory_calculate_size = &ocp_nlp_reg_convexify_calculate_memory_size; + config->memory_calculate_size = &ocp_nlp_reg_convexify_memory_calculate_size; config->memory_assign = &ocp_nlp_reg_convexify_assign_memory; config->memory_set = &ocp_nlp_reg_convexify_memory_set; config->memory_set_RSQrq_ptr = &ocp_nlp_reg_convexify_memory_set_RSQrq_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/acados/ocp_nlp/ocp_nlp_reg_convexify.h index 72683a4e3c..c760f54543 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -120,7 +120,7 @@ typedef struct { } ocp_nlp_reg_convexify_memory; // -acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +acados_size_t ocp_nlp_reg_convexify_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); // void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 488c1932d4..2b16f5a216 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -681,7 +681,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // NOTE: this is done before termination, such that we can get the QP at the stationary point that is actually solved, if we exit with success. acados_tic(&timer1); config->regularize->regularize(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize_mem); + nlp_opts->regularize, nlp_mem->regularize); nlp_timings->time_reg += acados_toc(&timer1); // update timeout memory based on chosen heuristic diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index b88bc3dfe4..e67f3bf2a3 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -520,7 +520,7 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim // regularize Hessian acados_tic(&timer1); config->regularize->regularize_lhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); // condense lhs acados_tic(&timer1); @@ -571,13 +571,13 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * { // finish regularization config->regularize->regularize_rhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); } else if (opts->rti_phase == PREPARATION_AND_FEEDBACK) { // full regularization config->regularize->regularize(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); } else { @@ -878,7 +878,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // regularization rhs acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); // solve QP qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL); @@ -930,7 +930,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // rhs regularization acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); // QP solve @@ -946,7 +946,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // compute correct dual solution in case of Hessian regularization acados_tic(&timer1); config->regularize->correct_dual_sol(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) { @@ -1000,7 +1000,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // rhs regularization acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); // QP solve @@ -1072,7 +1072,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // full regularization acados_tic(&timer1); config->regularize->regularize(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); // QP solve @@ -1120,7 +1120,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // regularize Hessian acados_tic(&timer1); config->regularize->regularize_lhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); timings->time_reg += acados_toc(&timer1); // condense lhs qp_solver->condense_lhs(qp_solver, dims->qp_solver, diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 3b7572f0a7..4fe685f69c 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1007,7 +1007,7 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o // regularize Hessian acados_tic(&timer_qp); - config->regularize->regularize(config->regularize, dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + config->regularize->regularize(config->regularize, dims->regularize, nlp_opts->regularize, nlp_mem->regularize); nlp_timings->time_reg += acados_toc(&timer_qp); } diff --git a/acados/utils/mem.h b/acados/utils/mem.h index d0c12204ec..809d2c8f00 100644 --- a/acados/utils/mem.h +++ b/acados/utils/mem.h @@ -45,18 +45,6 @@ extern "C" { #include "blasfeo_d_aux.h" #include "blasfeo_d_aux_ext_dep.h" -// TODO(dimitris): probably does not belong here -typedef struct -{ - int (*fun)(void *); - acados_size_t (*calculate_args_size)(void *); - void *(*assign_args)(void *); - void (*initialize_default_args)(void *); - acados_size_t (*calculate_memory_size)(void *); - void *(*assign_memory)(void *); - acados_size_t (*calculate_workspace_size)(void *); -} module_solver; - // make int counter of memory multiple of a number (typically 8 or 64) void make_int_multiple_of(acados_size_t num, acados_size_t *size); From 7de086d1807b18af3d4b3e627ba90e524454a780 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 19 Mar 2025 14:52:00 +0100 Subject: [PATCH 003/164] Follow up to #1462 (#1471) - fix test, run test on CI - add option: `reg_min_epsilon`: Minimum value for epsilon if regularize_method in ['PROJECT', 'MIRROR'] is used with reg_adaptive_eps. Default: 1e-8. - fix `reg_max_cond_block` default: 1e7 instead of 1e-7. Add check that it must be >= 1.0. --- acados/ocp_nlp/ocp_nlp_reg_common.c | 14 ++---- acados/ocp_nlp/ocp_nlp_reg_common.h | 4 +- acados/ocp_nlp/ocp_nlp_reg_mirror.c | 8 +++- acados/ocp_nlp/ocp_nlp_reg_mirror.h | 1 + acados/ocp_nlp/ocp_nlp_reg_project.c | 8 +++- acados/ocp_nlp/ocp_nlp_reg_project.h | 1 + .../non_ocp_nlp/adaptive_eps_reg_test.py | 44 +++++++++++-------- interfaces/CMakeLists.txt | 7 +++ .../acados_matlab_octave/AcadosOcpOptions.m | 4 +- .../acados_template/acados_ocp_options.py | 22 +++++++++- .../c_templates_tera/acados_multi_solver.in.c | 3 ++ .../c_templates_tera/acados_solver.in.c | 3 ++ 12 files changed, 83 insertions(+), 36 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.c b/acados/ocp_nlp/ocp_nlp_reg_common.c index 39f09c3873..a334b4a739 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.c +++ b/acados/ocp_nlp/ocp_nlp_reg_common.c @@ -200,7 +200,7 @@ void acados_mirror(int dim, double *A, double *V, double *d, double *e, double e acados_reconstruct_A(dim, A, V, d); } -void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block) +void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block, double min_eps) { int i; acados_eigen_decomposition(dim, A, V, d, e); @@ -212,10 +212,7 @@ void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double { max_eig = MAX(max_eig, fabs(d[i])); } - if (max_eig == 0.0) - eps = 1.0; - else - eps = max_eig/max_cond_block; + eps = MAX(max_eig/max_cond_block, min_eps); // mirror for (i = 0; i < dim; i++) @@ -247,7 +244,7 @@ void acados_project(int dim, double *A, double *V, double *d, double *e, double } -void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block) +void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block, double min_eps) { int i; acados_eigen_decomposition(dim, A, V, d, e); @@ -259,10 +256,7 @@ void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, doubl { max_eig = MAX(max_eig, d[i]); } - if (max_eig == 0.0) - eps = 1.0; - else - eps = max_eig/max_cond_block; + eps = MAX(max_eig/max_cond_block, min_eps); // project for (i = 0; i < dim; i++) diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.h b/acados/ocp_nlp/ocp_nlp_reg_common.h index c62e17c8b1..8911a82fd1 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.h +++ b/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -111,9 +111,9 @@ void *ocp_nlp_reg_config_assign(void *raw_memory); /* regularization help functions */ void acados_reconstruct_A(int dim, double *A, double *V, double *d); void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon); -void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block); +void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block, double min_eps); void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon); -void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block); +void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block, double min_eps); #ifdef __cplusplus diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.c b/acados/ocp_nlp/ocp_nlp_reg_mirror.c index 4c81bf9482..e1f0605e51 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_mirror.c +++ b/acados/ocp_nlp/ocp_nlp_reg_mirror.c @@ -67,6 +67,7 @@ void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims ocp_nlp_reg_mirror_opts *opts = opts_; opts->epsilon = 1e-4; + opts->min_epsilon = 1e-8; opts->adaptive_eps = false; opts->max_cond_block = 1e7; @@ -85,6 +86,11 @@ void ocp_nlp_reg_mirror_opts_set(void *config_, void *opts_, const char *field, double *d_ptr = value; opts->epsilon = *d_ptr; } + else if (!strcmp(field, "min_epsilon")) + { + double *d_ptr = value; + opts->min_epsilon = *d_ptr; + } else if (!strcmp(field, "max_cond_block")) { double *d_ptr = value; @@ -299,7 +305,7 @@ void ocp_nlp_reg_mirror_regularize(void *config, ocp_nlp_reg_dims *dims, void *o blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); if (opts->adaptive_eps) { - acados_mirror_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block); + acados_mirror_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block, opts->min_epsilon); } else { diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/acados/ocp_nlp/ocp_nlp_reg_mirror.h index 6d2ab162e7..f629db594e 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_mirror.h +++ b/acados/ocp_nlp/ocp_nlp_reg_mirror.h @@ -64,6 +64,7 @@ extern "C" { typedef struct { double epsilon; + double min_epsilon; bool adaptive_eps; double max_cond_block; } ocp_nlp_reg_mirror_opts; diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.c b/acados/ocp_nlp/ocp_nlp_reg_project.c index d965af9ff8..d6fe6af84c 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project.c @@ -67,6 +67,7 @@ void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims ocp_nlp_reg_project_opts *opts = opts_; opts->epsilon = 1e-4; + opts->min_epsilon = 1e-8; opts->adaptive_eps = false; opts->max_cond_block = 1e7; @@ -85,6 +86,11 @@ void ocp_nlp_reg_project_opts_set(void *config_, void *opts_, const char *field, double *d_ptr = value; opts->epsilon = *d_ptr; } + else if (!strcmp(field, "min_epsilon")) + { + double *d_ptr = value; + opts->min_epsilon = *d_ptr; + } else if (!strcmp(field, "max_cond_block")) { double *d_ptr = value; @@ -298,7 +304,7 @@ void ocp_nlp_reg_project_regularize(void *config, ocp_nlp_reg_dims *dims, void * blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); if (opts->adaptive_eps) { - acados_project_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block); + acados_project_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block, opts->min_epsilon); } else { diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.h b/acados/ocp_nlp/ocp_nlp_reg_project.h index eb33bf23d5..1492c90ba5 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project.h +++ b/acados/ocp_nlp/ocp_nlp_reg_project.h @@ -64,6 +64,7 @@ extern "C" { typedef struct { double epsilon; + double min_epsilon; bool adaptive_eps; double max_cond_block; } ocp_nlp_reg_project_opts; diff --git a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py index 072ee53d7d..2613314016 100644 --- a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py +++ b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py @@ -79,6 +79,7 @@ def export_parametric_ocp() -> AcadosOcp: ocp.solver_options.eval_residual_at_max_iter = False ocp.solver_options.reg_adaptive_eps = True ocp.solver_options.reg_max_cond_block = 1e3 + ocp.solver_options.reg_min_epsilon = 1e-7 return ocp @@ -98,6 +99,8 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): nx = ocp.dims.nx nu = ocp.dims.nu + eps_min = ocp.solver_options.reg_min_epsilon + W_mat2 = np.zeros((nx+nu, nx+nu)) W_mat2[0,0] = 1e6 W_mat2[nx+nu-1, nx+nu-1] = 1e-4 @@ -112,7 +115,8 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): [0.5000, -0.5000, -0.5000, 0.5000], [0.2706, -0.6533, 0.6533, -0.2706]]) - mat_x = A_x.T @ np.diag([15, 4.0, -2e5, 1e-6]) @ A_x + W3_eig = [15, 4.0, -2e5, 1e-6] + mat_x = A_x.T @ np.diag(W3_eig) @ A_x W_mat3 = block_diag(mat_x, mat_u) @@ -122,39 +126,41 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): for i, W_mat in enumerate(W_mats): print(f"{regularize_method} i={i}") print("---------------------") - - # Test zero matrix set_cost_matrix(ocp_solver, W_mat, W_mat_e) status = ocp_solver.solve() ocp_solver.print_statistics() nlp_iter = ocp_solver.get_stats("nlp_iter") + hess_0 = ocp_solver.get_hessian_block(0) + hess_1 = ocp_solver.get_hessian_block(1) + # check condition numbers qp_diagnostics = ocp_solver.qp_diagnostics() - assert qp_diagnostics['condition_number_stage'][0] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][0]} for i = {i}" - assert qp_diagnostics['condition_number_stage'][1] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][1]} for i = {i}" + assert qp_diagnostics['condition_number_stage'][0] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][0]}" + assert qp_diagnostics['condition_number_stage'][1] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][1]}" - assert nlp_iter == 1, f"Number of NLP iterations should be 1, got {nlp_iter} for i = {i}" - assert status == 0, f"acados returned status {status} for i = {i}" + # check solver stats + assert status == 0, f"acados returned status {status}" - hessian_0 = ocp_solver.get_hessian_block(0) + # check hessian if i == 0: - assert np.equal(hessian_0, np.eye(nx+nu)).all(), f"Zero Hessian matrix should be transformed into identity for {regularize_method} for i = {i}" + assert np.equal(hess_0, eps_min*np.eye(nx+nu)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" elif i == 1: - assert np.equal(hessian_0, np.diag([1e3, 1e3, 1e6, 1e3, 1e3, 1e3])).all(), f"Something in adaptive {regularize_method} went wrong for i = {i}!" + assert np.equal(hess_0, np.diag([1e3, 1e3, 1e6, 1e3, 1e3, 1e3])).all(), f"Something in adaptive {regularize_method} went wrong!" elif i == 2: - print(np.linalg.eigvals(W_mat)) - print(hessian_0) - print(np.real(np.linalg.eigvals(hessian_0))) + # print(np.linalg.eigvals(W_mat)) + print(hess_0) + print(np.real(np.linalg.eigvals(hess_0))) if regularize_method == 'MIRROR': - reg_eps = 2e5/ocp.solver_options.reg_max_cond_block - assert np.allclose(np.linalg.eigvals(hessian_0), np.array([reg_eps, 2e5, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong for i = {i}!" + max_abs_eig = np.max(np.abs(W3_eig)) + reg_eps = max(max_abs_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(np.linalg.eigvals(hess_0), np.array([reg_eps, max_abs_eig, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong!" elif regularize_method == 'PROJECT': - reg_eps = 15/ocp.solver_options.reg_max_cond_block - assert np.allclose(np.real(np.linalg.eigvals(hessian_0)), np.array([15, 4, reg_eps, reg_eps, reg_eps, reg_eps]), rtol=1e-03, atol=1e-3), f"Something in adaptive {regularize_method} went wrong for i = {i}!" + max_pos_eig = np.max(W3_eig) + reg_eps = max(max_pos_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(np.real(np.linalg.eigvals(hess_0)), np.array([15, 4, reg_eps, reg_eps, reg_eps, reg_eps]), rtol=1e-03, atol=1e-3), f"Something in adaptive {regularize_method} went wrong!" - hessian_1 = ocp_solver.get_hessian_block(1) - assert np.equal(hessian_1, np.eye(nx)).all(), f"Zero Hessian matrix should be transformed into identity for {regularize_method} for i = {i}" + assert np.equal(hess_1, eps_min*np.eye(nx)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" if __name__ == "__main__": diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 5c6e8456c2..4202668607 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -258,6 +258,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp python maratos_test_problem.py) + add_test(NAME python_test_adaptive_reg + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp + python adaptive_eps_reg_test.py) + # Convex test problem where full step SQP does not converge, but globalized SQP does add_test(NAME python_convex_test_problem_globalization COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_problem_globalization_needed @@ -416,6 +420,9 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) set_tests_properties(python_pendulum_closed_loop_example PROPERTIES DEPENDS python_pendulum_sim_example) + # Directory non_ocp_nlp + set_tests_properties(python_maratos_test_problem_globalization PROPERTIES DEPENDS python_test_adaptive_reg) + # Directory acados_python/tests set_tests_properties(python_test_cython_vs_ctypes PROPERTIES DEPENDS python_test_reset) set_tests_properties(python_test_reset PROPERTIES DEPENDS python_test_ocp) diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 960f783e33..82f9c4dc6d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -81,6 +81,7 @@ regularize_method reg_epsilon reg_max_cond_block + reg_min_epsilon reg_adaptive_eps exact_hess_cost exact_hess_dyn @@ -186,7 +187,8 @@ obj.regularize_method = 'NO_REGULARIZE'; obj.reg_epsilon = 1e-4; obj.reg_adaptive_eps = false; - obj.reg_max_cond_block = 1e-7; + obj.reg_max_cond_block = 1e7; + obj.reg_min_epsilon = 1e-8; obj.shooting_nodes = []; obj.cost_scaling = []; obj.exact_hess_cost = 1; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 3fa5a8f0a2..3a40539bdf 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -89,8 +89,9 @@ def __init__(self): self.__cost_discretization = 'EULER' self.__regularize_method = 'NO_REGULARIZE' self.__reg_epsilon = 1e-4 - self.__reg_max_cond_block = 1e-7 + self.__reg_max_cond_block = 1e7 self.__reg_adaptive_eps = False + self.__reg_min_epsilon = 1e-8 self.__exact_hess_cost = 1 self.__exact_hess_dyn = 1 self.__exact_hess_constr = 1 @@ -773,7 +774,7 @@ def reg_max_cond_block(self): """Maximum condition number of each Hessian block after regularization with regularize_method in ['PROJECT', 'MIRROR'] and reg_adaptive_eps = True Type: float - Default: 1e-7 + Default: 1e7 """ return self.__reg_max_cond_block @@ -790,6 +791,15 @@ def reg_adaptive_eps(self): """ return self.__reg_adaptive_eps + @property + def reg_min_epsilon(self): + """Minimum value for epsilon if regularize_method in ['PROJECT', 'MIRROR'] is used with reg_adaptive_eps. + + Type: float + Default: 1e-8 + """ + return self.__reg_min_epsilon + @property def globalization_alpha_reduction(self): """Step size reduction factor for globalization MERIT_BACKTRACKING, @@ -1393,6 +1403,8 @@ def reg_epsilon(self, reg_epsilon): @reg_max_cond_block.setter def reg_max_cond_block(self, reg_max_cond_block): + if not isinstance(reg_max_cond_block, float) or reg_max_cond_block < 1.0: + raise Exception('Invalid reg_max_cond_block value, expected float > 1.0.') self.__reg_max_cond_block = reg_max_cond_block @reg_adaptive_eps.setter @@ -1401,6 +1413,12 @@ def reg_adaptive_eps(self, reg_adaptive_eps): raise Exception(f'Invalid reg_adaptive_eps value, expected bool, got {reg_adaptive_eps}') self.__reg_adaptive_eps = reg_adaptive_eps + @reg_min_epsilon.setter + def reg_min_epsilon(self, reg_min_epsilon): + if not isinstance(reg_min_epsilon, float) or reg_min_epsilon < 0: + raise Exception(f'Invalid reg_min_epsilon value, expected float > 0, got {reg_min_epsilon}') + self.__reg_min_epsilon = reg_min_epsilon + @globalization_alpha_min.setter def globalization_alpha_min(self, globalization_alpha_min): self.__globalization_alpha_min = globalization_alpha_min 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 076638b4d6..04fb7662cf 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 @@ -2280,6 +2280,9 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) double reg_max_cond_block = {{ solver_options.reg_max_cond_block }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_max_cond_block", ®_max_cond_block); + double reg_min_epsilon = {{ solver_options.reg_min_epsilon }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_min_epsilon", ®_min_epsilon); + bool reg_adaptive_eps = {{ solver_options.reg_adaptive_eps }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_adaptive_eps", ®_adaptive_eps); {%- endif %} 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 ada813f79e..a32a886ebd 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 @@ -2395,6 +2395,9 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps double reg_max_cond_block = {{ solver_options.reg_max_cond_block }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_max_cond_block", ®_max_cond_block); + double reg_min_epsilon = {{ solver_options.reg_min_epsilon }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_min_epsilon", ®_min_epsilon); + bool reg_adaptive_eps = {{ solver_options.reg_adaptive_eps }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_adaptive_eps", ®_adaptive_eps); {%- endif %} From 01452a6c902298da39947ccb6f44fb550cf51d07 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 18:24:00 +0100 Subject: [PATCH 004/164] Gershgorin Levenberg Marquardt regularization (#1473) Implement new regularization module: `GERSHGORIN_LEVENBERG_MARQUARDT`. Estimates the smallest eigenvalue of each Hessian block using Gershgorin circles and adds multiple of identity to each block, such that smallest eigenvalue after regularization is at least `reg_epsilon`. --- acados/ocp_nlp/ocp_nlp_reg_glm.c | 321 ++++++++++++++++++ acados/ocp_nlp/ocp_nlp_reg_glm.h | 109 ++++++ acados/utils/math.c | 45 +++ acados/utils/math.h | 6 + .../non_ocp_nlp/adaptive_eps_reg_test.py | 54 +-- interfaces/acados_c/ocp_nlp_interface.c | 4 + interfaces/acados_c/ocp_nlp_interface.h | 1 + .../acados_template/acados_ocp_options.py | 9 +- .../acados_template/acados_ocp_solver.py | 1 - .../c_templates_tera/acados_multi_solver.in.c | 2 +- .../c_templates_tera/acados_solver.in.c | 2 +- 11 files changed, 522 insertions(+), 32 deletions(-) create mode 100644 acados/ocp_nlp/ocp_nlp_reg_glm.c create mode 100644 acados/ocp_nlp/ocp_nlp_reg_glm.h diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.c b/acados/ocp_nlp/ocp_nlp_reg_glm.c new file mode 100644 index 0000000000..121086c94d --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.c @@ -0,0 +1,321 @@ +/* + * 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.; + */ + + +#include "acados/ocp_nlp/ocp_nlp_reg_glm.h" + +#include +#include +#include +#include +#include + +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/utils/math.h" + +#include "blasfeo_d_aux.h" +#include "blasfeo_d_blas.h" + + + +/************************************************ + * opts + ************************************************/ + +acados_size_t ocp_nlp_reg_glm_opts_calculate_size(void) +{ + return sizeof(ocp_nlp_reg_glm_opts); +} + + + +void *ocp_nlp_reg_glm_opts_assign(void *raw_memory) +{ + return raw_memory; +} + + + +void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + ocp_nlp_reg_glm_opts *opts = opts_; + opts->epsilon = 1e-6; + return; +} + + + +void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, void* value) +{ + + ocp_nlp_reg_glm_opts *opts = opts_; + if (!strcmp(field, "epsilon")) + { + double *d_ptr = value; + opts->epsilon = *d_ptr; + } + else + { + printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); + exit(1); + } + + return; +} + + + +/************************************************ + * memory + ************************************************/ + +acados_size_t ocp_nlp_reg_glm_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + } + + acados_size_t size = 0; + + size += sizeof(ocp_nlp_reg_glm_memory); + + size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + + return size; +} + + + +void *ocp_nlp_reg_glm_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + } + + char *c_ptr = (char *) raw_memory; + + ocp_nlp_reg_glm_memory *mem = (ocp_nlp_reg_glm_memory *) c_ptr; + c_ptr += sizeof(ocp_nlp_reg_glm_memory); + + mem->RSQrq = (struct blasfeo_dmat **) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + + assert((char *) mem + ocp_nlp_reg_glm_memory_calculate_size(config_, dims, opts_) >= c_ptr); + + return mem; +} + + + +void ocp_nlp_reg_glm_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_) +{ + ocp_nlp_reg_glm_memory *memory = memory_; + + int ii; + + int N = dims->N; + + for(ii=0; ii<=N; ii++) + { + memory->RSQrq[ii] = RSQrq+ii; + } + + return; +} + + + +void ocp_nlp_reg_glm_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) +{ + // TODO: remove this function in all regularizaiton modules + printf("\nerror: field %s not available in ocp_nlp_reg_glm_set\n", field); + exit(1); + + return; +} + + + +/************************************************ + * functions + ************************************************/ + +void ocp_nlp_reg_glm_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_glm_memory *mem = (ocp_nlp_reg_glm_memory *) mem_; + ocp_nlp_reg_glm_opts *opts = opts_; + + int ii; + + int *nx = dims->nx; + int *nu = dims->nu; + double tmp, alpha; + + for(ii=0; ii<=dims->N; ii++) + { + // make symmetric + blasfeo_dtrtr_l(nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + + // regularize + compute_gershgorin_min_eig_estimate(nu[ii]+nx[ii], mem->RSQrq[ii], &tmp); + if (tmp < opts->epsilon) + { + if (tmp < 0) + alpha = fabs(tmp)+opts->epsilon; + else + alpha = opts->epsilon; + blasfeo_ddiare(nu[ii]+nx[ii], alpha, mem->RSQrq[ii], 0, 0); + } + } +} + + +void ocp_nlp_reg_glm_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_glm_regularize(config, dims, opts_, mem_); +} + + +void ocp_nlp_reg_glm_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + +void ocp_nlp_reg_glm_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + + +void ocp_nlp_reg_glm_config_initialize_default(ocp_nlp_reg_config *config) +{ + // dims + config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size; + config->dims_assign = &ocp_nlp_reg_dims_assign; + config->dims_set = &ocp_nlp_reg_dims_set; + // opts + config->opts_calculate_size = &ocp_nlp_reg_glm_opts_calculate_size; + config->opts_assign = &ocp_nlp_reg_glm_opts_assign; + config->opts_initialize_default = &ocp_nlp_reg_glm_opts_initialize_default; + config->opts_set = &ocp_nlp_reg_glm_opts_set; + // memory + config->memory_calculate_size = &ocp_nlp_reg_glm_memory_calculate_size; + config->memory_assign = &ocp_nlp_reg_glm_memory_assign; + config->memory_set = &ocp_nlp_reg_glm_memory_set; + config->memory_set_RSQrq_ptr = &ocp_nlp_reg_glm_memory_set_RSQrq_ptr; + config->memory_set_rq_ptr = &ocp_nlp_reg_glm_memory_set_rq_ptr; + config->memory_set_BAbt_ptr = &ocp_nlp_reg_glm_memory_set_BAbt_ptr; + config->memory_set_b_ptr = &ocp_nlp_reg_glm_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_glm_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_glm_memory_set_DCt_ptr; + config->memory_set_ux_ptr = &ocp_nlp_reg_glm_memory_set_ux_ptr; + config->memory_set_pi_ptr = &ocp_nlp_reg_glm_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_glm_memory_set_lam_ptr; + // functions + config->regularize = &ocp_nlp_reg_glm_regularize; + config->regularize_rhs = &ocp_nlp_reg_glm_regularize_rhs; + config->regularize_lhs = &ocp_nlp_reg_glm_regularize_lhs; + config->correct_dual_sol = &ocp_nlp_reg_glm_correct_dual_sol; +} + diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.h b/acados/ocp_nlp/ocp_nlp_reg_glm.h new file mode 100644 index 0000000000..e880715320 --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.h @@ -0,0 +1,109 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_glm_opts; + +// +acados_size_t ocp_nlp_reg_glm_opts_calculate_size(void); +// +void *ocp_nlp_reg_glm_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_glm_memory; + +// +acados_size_t ocp_nlp_reg_glm_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_glm_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_glm_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ +/// @} +/// @} diff --git a/acados/utils/math.c b/acados/utils/math.c index 0d198cf270..a18d2ae6e5 100644 --- a/acados/utils/math.c +++ b/acados/utils/math.c @@ -1110,6 +1110,51 @@ void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double } +void compute_gershgorin_max_abs_eig_estimate(int n, struct blasfeo_dmat *A, double *out) +{ + double max_abs_eig = 0.0; + double r_i, lam, rho, a; + for (int ii = 0; ii < n; ii++) + { + r_i = 0.0; + for (int jj = 0; jj < n; jj++) + { + if (jj != ii) + { + r_i += fabs(BLASFEO_DMATEL(A, ii, jj)); + } + } + a = BLASFEO_DMATEL(A, ii, ii); + lam = a - r_i; + rho = a + r_i; + max_abs_eig = fmax(max_abs_eig, fmax(fabs(lam), fabs(rho))); + } + *out = max_abs_eig; +} + +void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double *out) +{ + // returns a lower bound for the minimum eigenvalue of A + double lam = ACADOS_INFTY; + double a, r_i, lam_i; + for (int ii = 0; ii < n; ii++) + { + r_i = 0.0; + for (int jj = 0; jj < n; jj++) + { + if (jj != ii) + { + r_i += fabs(BLASFEO_DMATEL(A, ii, jj)); + } + } + a = BLASFEO_DMATEL(A, ii, ii); + lam_i = a - r_i; + lam = fmin(lam, lam_i); + } + *out = lam; +} + + double minimum_of_doubles(double *x, int n) { diff --git a/acados/utils/math.h b/acados/utils/math.h index 4bbef9cbf5..d1e13539f8 100644 --- a/acados/utils/math.h +++ b/acados/utils/math.h @@ -36,6 +36,7 @@ extern "C" { #endif #include "acados/utils/types.h" +#include "blasfeo_common.h" #if defined(__MABX2__) double fmax(double a, double b); @@ -100,6 +101,11 @@ double minimum_of_doubles(double *x, int n); void neville_algorithm(double xx, int n, double *x, double *Q, double *out); +void compute_gershgorin_max_abs_eig_estimate(int n, struct blasfeo_dmat *A, double *out); + +void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double *out); + + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py index 2613314016..21c117b7fe 100644 --- a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py +++ b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py @@ -73,7 +73,7 @@ def export_parametric_ocp() -> AcadosOcp: ocp.solver_options.nlp_solver_type = "SQP" ocp.solver_options.N_horizon = 1 ocp.solver_options.tf = 1.0 - ocp.solver_options.print_level = 1 + ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_ext_qp_res = 1 ocp.solver_options.nlp_solver_max_iter = 2 ocp.solver_options.eval_residual_at_max_iter = False @@ -94,12 +94,12 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): ocp.solver_options.nlp_solver_max_iter = 2 # QP should converge in one iteration ocp.solver_options.regularize_method = regularize_method - ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", verbose=False) + ocp_solver = AcadosOcpSolver(ocp, verbose=False) nx = ocp.dims.nx nu = ocp.dims.nu - eps_min = ocp.solver_options.reg_min_epsilon + eps_min = ocp.solver_options.reg_min_epsilon if regularize_method != "GERSHGORIN_LEVENBERG_MARQUARDT" else ocp.solver_options.reg_epsilon W_mat2 = np.zeros((nx+nu, nx+nu)) W_mat2[0,0] = 1e6 @@ -134,31 +134,36 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): hess_0 = ocp_solver.get_hessian_block(0) hess_1 = ocp_solver.get_hessian_block(1) - # check condition numbers - qp_diagnostics = ocp_solver.qp_diagnostics() - assert qp_diagnostics['condition_number_stage'][0] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][0]}" - assert qp_diagnostics['condition_number_stage'][1] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][1]}" - # check solver stats assert status == 0, f"acados returned status {status}" + # check eigenvalues + eigvals_0 = np.real(np.linalg.eigvals(hess_0)) + assert np.min(eigvals_0) >= eps_min*0.99, f"Eigenvalues of Hessian must be >= {eps_min} = eps_min, got {eigvals_0}" # check hessian - if i == 0: - assert np.equal(hess_0, eps_min*np.eye(nx+nu)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" - elif i == 1: - assert np.equal(hess_0, np.diag([1e3, 1e3, 1e6, 1e3, 1e3, 1e3])).all(), f"Something in adaptive {regularize_method} went wrong!" - elif i == 2: - # print(np.linalg.eigvals(W_mat)) - print(hess_0) - print(np.real(np.linalg.eigvals(hess_0))) - if regularize_method == 'MIRROR': - max_abs_eig = np.max(np.abs(W3_eig)) - reg_eps = max(max_abs_eig/ocp.solver_options.reg_max_cond_block, eps_min) - assert np.allclose(np.linalg.eigvals(hess_0), np.array([reg_eps, max_abs_eig, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong!" - elif regularize_method == 'PROJECT': - max_pos_eig = np.max(W3_eig) - reg_eps = max(max_pos_eig/ocp.solver_options.reg_max_cond_block, eps_min) - assert np.allclose(np.real(np.linalg.eigvals(hess_0)), np.array([15, 4, reg_eps, reg_eps, reg_eps, reg_eps]), rtol=1e-03, atol=1e-3), f"Something in adaptive {regularize_method} went wrong!" + if regularize_method in ['MIRROR', 'PROJECT']: + # check condition numbers + qp_diagnostics = ocp_solver.qp_diagnostics() + assert qp_diagnostics['condition_number_stage'][0] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][0]}" + assert qp_diagnostics['condition_number_stage'][1] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][1]}" + if i == 0: + print(hess_0) + assert np.equal(hess_0, eps_min*np.eye(nx+nu)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" + elif i == 1: + min_eig = np.max(eigvals_0) / ocp.solver_options.reg_max_cond_block + assert np.equal(hess_0, np.diag([min_eig, min_eig, 1e6, min_eig, min_eig, min_eig])).all(), f"Something in adaptive {regularize_method} went wrong!" + elif i == 2: + # print(np.linalg.eigvals(W_mat)) + # print(hess_0) + # print(eigvals_0) + if regularize_method == 'MIRROR': + max_abs_eig = np.max(np.abs(W3_eig)) + reg_eps = max(max_abs_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(eigvals_0, np.array([reg_eps, max_abs_eig, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong!" + elif regularize_method == 'PROJECT': + max_pos_eig = np.max(W3_eig) + reg_eps = max(max_pos_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(eigvals_0, np.array([15, 4, reg_eps, reg_eps, reg_eps, reg_eps]), rtol=1e-03, atol=1e-3), f"Something in adaptive {regularize_method} went wrong!" assert np.equal(hess_1, eps_min*np.eye(nx)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" @@ -166,3 +171,4 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): if __name__ == "__main__": test_reg_adaptive_eps("MIRROR") test_reg_adaptive_eps("PROJECT") + test_reg_adaptive_eps("GERSHGORIN_LEVENBERG_MARQUARDT") diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 1d07d15f36..524e0961cc 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -48,6 +48,7 @@ #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" #include "acados/ocp_nlp/ocp_nlp_constraints_bgp.h" #include "acados/ocp_nlp/ocp_nlp_reg_convexify.h" +#include "acados/ocp_nlp/ocp_nlp_reg_glm.h" #include "acados/ocp_nlp/ocp_nlp_reg_mirror.h" #include "acados/ocp_nlp/ocp_nlp_reg_project.h" #include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h" @@ -237,6 +238,9 @@ ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan) case CONVEXIFY: ocp_nlp_reg_convexify_config_initialize_default(config->regularize); break; + case GERSHGORIN_LEVENBERG_MARQUARDT: + ocp_nlp_reg_glm_config_initialize_default(config->regularize); + break; default: printf("\nerror: ocp_nlp_config_create: unsupported plan->regularization\n"); exit(1); diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 69011dd1a1..fc3cb0cb7c 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -92,6 +92,7 @@ typedef enum PROJECT, PROJECT_REDUC_HESS, CONVEXIFY, + GERSHGORIN_LEVENBERG_MARQUARDT, INVALID_REGULARIZE, } ocp_nlp_reg_t; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 3a40539bdf..68d3e67eca 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -324,14 +324,13 @@ def collocation_type(self): @property def regularize_method(self): """Regularization method for the Hessian. - String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. + String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'). - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii)) - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf, does not support nonlinear constraints - PROJECT_REDUC_HESS: experimental - - Note: default eps = 1e-4 + - GERSHGORIN_LEVENBERG_MARQUARDT: estimates the smallest eigenvalue of each Hessian block using Gershgorin circles and adds multiple of identity to each block, such that smallest eigenvalue after regularization is at least reg_epsilon Default: 'NO_REGULARIZE'. """ @@ -766,7 +765,7 @@ def alpha_min(self): @property def reg_epsilon(self): - """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY']""" + """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'].""" return self.__reg_epsilon @property @@ -1239,7 +1238,7 @@ def qp_solver(self, qp_solver): @regularize_method.setter def regularize_method(self, regularize_method): regularize_methods = ('NO_REGULARIZE', 'MIRROR', 'PROJECT', \ - 'PROJECT_REDUC_HESS', 'CONVEXIFY') + 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT') if regularize_method in regularize_methods: self.__regularize_method = regularize_method else: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index ca9b912ed0..56a642dde8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1568,7 +1568,6 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: nlp_iter = self.get_stats("nlp_iter") stat_m = self.get_stats("stat_m") stat_n = self.get_stats("stat_n") - print("stat_n: ", stat_n) min_size = min([stat_m, nlp_iter+1]) out = np.zeros((stat_n+1, min_size), dtype=np.float64, order="C") out_data = cast(out.ctypes.data, POINTER(c_double)) 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 04fb7662cf..2db025a1fc 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 @@ -2271,7 +2271,7 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) {%- endif %} {%- endif %} -{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" %} +{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" or solver_options.regularize_method == "GERSHGORIN_LEVENBERG_MARQUARDT"%} double reg_epsilon = {{ solver_options.reg_epsilon }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_epsilon", ®_epsilon); {%- endif %} 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 a32a886ebd..448ea099e4 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 @@ -2386,7 +2386,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {%- endif %} {%- endif %} -{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" %} +{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" or solver_options.regularize_method == "GERSHGORIN_LEVENBERG_MARQUARDT"%} double reg_epsilon = {{ solver_options.reg_epsilon }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_epsilon", ®_epsilon); {%- endif %} From 356f21b94b21ee6b80ac9d3000d6bfafaf16ae5a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 24 Mar 2025 10:49:40 +0100 Subject: [PATCH 005/164] Export OCP solver in Python and use it in Matlab (#1472) ## MATLAB: implement code reuse - add `solver_creation_opts` argument to `AcadosOcpSolver` - with ``` default_solver_creation_opts = struct('json_file', '', ... 'build', true, ... 'generate', true, ... 'compile_mex_wrapper', true, ... 'compile_interface', [], ... 'output_dir', fullfile(pwd, 'build')); ``` - `solver_creation_opts` can be used to specify some of those options. For the others, the default is used. ## MATLAB and Python: implement code reuse without specifying OCP formulation - allow creating `AcadosOcpSolver` with `acados_ocp=None` and just provide json file instead - in this case generated code must also be available - load information from json file that is needed within the solver; NOTE: we don't recover the full OCP description as class with subclasses, but only load what is needed in form of dicts / structs. This is less restrictive and should make it easier to use solvers generated with Python in Matlab and vice versa. - Minor fixes to allow loading Python solver in Matlab - Add test for MOCP and OCP python to octave workflow on Github actions - test non default `code_export_directory` in MATLAB --- .github/linux/export_paths.sh | 2 +- .github/workflows/full_build.yml | 33 +- .../pendulum_on_cart_model/example_ocp.m | 394 +++++------------- ...osModel.m => get_pendulum_on_cart_model.m} | 2 +- .../pendulum_on_cart_model/zoro_example.m | 2 +- .../test/create_ocp_solver_code_reuse.m | 121 ++++++ .../test/run_matlab_tests.m | 4 +- .../test/simulink_sparse_param_test.m | 4 +- .../test/test_code_reuse.m | 66 +++ .../crane/time_optimal_example.py | 49 ++- .../mocp_transition_example/main.py | 1 - .../p_global_example/code_reuse_py2matlab.m | 61 +++ .../p_global_example/example_p_global.py | 26 +- interfaces/CMakeLists.txt | 6 +- .../acados_matlab_octave/AcadosOcpSolver.m | 190 ++++++--- interfaces/acados_matlab_octave/acados_ocp.m | 3 +- .../acados_template/acados_multiphase_ocp.py | 4 +- .../acados_template/acados_ocp.py | 42 +- .../acados_template/acados_ocp_solver.py | 132 +++--- .../acados_template/acados_ocp_solver_pyx.pyx | 55 --- .../matlab_templates/make_mex.in.m | 12 +- .../matlab_templates/mex_solver.in.m | 6 +- .../acados_template/acados_template/utils.py | 4 +- 23 files changed, 700 insertions(+), 519 deletions(-) rename examples/acados_matlab_octave/pendulum_on_cart_model/{get_pendulum_on_cart_AcadosModel.m => get_pendulum_on_cart_model.m} (98%) create mode 100644 examples/acados_matlab_octave/test/create_ocp_solver_code_reuse.m create mode 100644 examples/acados_matlab_octave/test/test_code_reuse.m create mode 100644 examples/acados_python/p_global_example/code_reuse_py2matlab.m diff --git a/.github/linux/export_paths.sh b/.github/linux/export_paths.sh index c864827c48..9c4937543f 100755 --- a/.github/linux/export_paths.sh +++ b/.github/linux/export_paths.sh @@ -34,5 +34,5 @@ echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV -echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code" >> $GITHUB_ENV +echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase" >> $GITHUB_ENV echo "ENV_RUN=true" >> $GITHUB_ENV diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index efb3750825..9c10c5b9ef 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -110,7 +110,7 @@ jobs: source ${{runner.workspace}}/acados/acadosenv/bin/activate ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4; - python_interface_new_casadi: + python_interface_new_casadi_and_py2octave: needs: core_build runs-on: ubuntu-22.04 @@ -149,6 +149,27 @@ jobs: shell: bash run: ${{runner.workspace}}/acados/.github/linux/install_new_casadi_python.sh'' + - name: Prepare Octave + working-directory: ${{runner.workspace}}/acados/external + shell: bash + run: | + sudo apt-get update + sudo apt-get install liboctave-dev -y --fix-missing + octave --version + ${{runner.workspace}}/acados/.github/linux/install_new_casadi_octave.sh + + # just needed for blasfeo_target.h in MEX interface + - name: Configure CMake + shell: bash + working-directory: ${{runner.workspace}}/acados/build + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + + - name: Export Paths for octave + working-directory: ${{runner.workspace}}/acados + shell: bash + run: | + ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + - name: Run Python tests that need new CasADi working-directory: ${{runner.workspace}}/acados/build shell: bash @@ -156,6 +177,8 @@ jobs: source ${{runner.workspace}}/acados/acadosenv/bin/activate cd ${{runner.workspace}}/acados/examples/acados_python/p_global_example python example_p_global.py + echo "\nPython run done; testing tranfer to Octave\n" + octave code_reuse_py2matlab.m - name: Run more Python tests working-directory: ${{runner.workspace}}/acados/build @@ -228,7 +251,7 @@ jobs: release: R2021a cache: true - # just needed for blasfeo_target.h + # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build @@ -285,7 +308,7 @@ jobs: products: Simulink Simulink_Test cache: true - # just needed for blasfeo_target.h + # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build @@ -344,7 +367,7 @@ jobs: products: Simulink Simulink_Test cache: true - # just needed for blasfeo_target.h + # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build @@ -397,7 +420,7 @@ jobs: products: Simulink Simulink_Test cache: true - # just needed for blasfeo_target.h + # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp.m index b1c4bb356c..b83af6fd1d 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp.m @@ -27,307 +27,109 @@ % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE.; - - -% NOTE: `acados` currently supports both an old MATLAB/Octave interface (< v0.4.0) -% as well as a new interface (>= v0.4.0). - -% THIS EXAMPLE still uses the OLD interface. If you are new to `acados` please start -% with the examples that have been ported to the new interface already. -% see https://github.com/acados/acados/issues/1196#issuecomment-2311822122) - - -clear all; clc; - -% check that env.sh has been run -env_run = getenv('ENV_RUN'); -if (~strcmp(env_run, 'true')) - error('env.sh has not been sourced! Before executing this example, run: source env.sh'); -end - -%% arguments -compile_interface = 'true'; %'auto'; -gnsf_detect_struct = 'true'; - -% discretization -N = 100; -h = 0.01; - -nlp_solver = 'sqp'; -%nlp_solver = 'sqp_rti'; -%nlp_solver_exact_hessian = 'false'; -nlp_solver_exact_hessian = 'true'; -%regularize_method = 'no_regularize'; -%regularize_method = 'project'; -regularize_method = 'project_reduc_hess'; -%regularize_method = 'mirror'; -%regularize_method = 'convexify'; -nlp_solver_max_iter = 20; %100; -nlp_solver_tol_stat = 1e-8; -nlp_solver_tol_eq = 1e-8; -nlp_solver_tol_ineq = 1e-8; -nlp_solver_tol_comp = 1e-8; -nlp_solver_ext_qp_res = 1; -qp_solver = 'partial_condensing_hpipm'; -%qp_solver = 'full_condensing_hpipm'; -%qp_solver = 'full_condensing_qpoases'; -qp_solver_cond_N = 5; -qp_solver_cond_ric_alg = 0; -qp_solver_ric_alg = 0; -qp_solver_warm_start = 2; -qp_solver_max_iter = 100; -%sim_method = 'erk'; -sim_method = 'irk'; -%sim_method = 'irk_gnsf'; -sim_method_num_stages = 4; -sim_method_num_steps = 3; -cost_type = 'linear_ls'; -%cost_type = 'ext_cost'; -model_name = 'ocp_pendulum'; - - -%% create model entries -model = pendulum_on_cart_model(); - -% dims -T = N*h; % horizon length time -nx = model.nx; -nu = model.nu; -ny = nu+nx; % number of outputs in lagrange term -ny_e = nx; % number of outputs in mayer term -if 0 - nbx = 0; - nbu = nu; - ng = 0; - ng_e = 0; - nh = 0; - nh_e = 0; -else - nbx = 0; - nbu = 0; - ng = 0; - ng_e = 0; - nh = nu; - nh_e = 0; -end - -% cost -Vu = zeros(ny, nu); for ii=1:nu Vu(ii,ii)=1.0; end % input-to-output matrix in lagrange term -Vx = zeros(ny, nx); for ii=1:nx Vx(nu+ii,ii)=1.0; end % state-to-output matrix in lagrange term -Vx_e = zeros(ny_e, nx); for ii=1:nx Vx_e(ii,ii)=1.0; end % state-to-output matrix in mayer term -W = eye(ny); % weight matrix in lagrange term -for ii=1:nu W(ii,ii)=1e-2; end -for ii=nu+1:nu+nx/2 W(ii,ii)=1e3; end -for ii=nu+nx/2+1:nu+nx W(ii,ii)=1e-2; end -W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in mayer term -yr = zeros(ny, 1); % output reference in lagrange term -yr_e = zeros(ny_e, 1); % output reference in mayer term - -% constraints -x0 = [0; pi; 0; 0]; -%Jbx = zeros(nbx, nx); for ii=1:nbx Jbx(ii,ii)=1.0; end -%lbx = -4*ones(nbx, 1); -%ubx = 4*ones(nbx, 1); -Jbu = zeros(nbu, nu); for ii=1:nbu Jbu(ii,ii)=1.0; end -lbu = -80*ones(nu, 1); -ubu = 80*ones(nu, 1); - - -%% acados ocp model -ocp_model = acados_ocp_model(); -ocp_model.set('name', model_name); -ocp_model.set('T', T); - -% symbolics -ocp_model.set('sym_x', model.sym_x); -if isfield(model, 'sym_u') - ocp_model.set('sym_u', model.sym_u); -end -if isfield(model, 'sym_xdot') - ocp_model.set('sym_xdot', model.sym_xdot); -end -% cost -ocp_model.set('cost_type', cost_type); -ocp_model.set('cost_type_e', cost_type); -%if (strcmp(cost_type, 'linear_ls')) - ocp_model.set('cost_Vu', Vu); - ocp_model.set('cost_Vx', Vx); - ocp_model.set('cost_Vx_e', Vx_e); - ocp_model.set('cost_W', W); - ocp_model.set('cost_W_e', W_e); - ocp_model.set('cost_y_ref', yr); - ocp_model.set('cost_y_ref_e', yr_e); -%else % if (strcmp(cost_type, 'ext_cost')) -% ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); -% ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); -%end -% dynamics -if (strcmp(sim_method, 'erk')) - ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); -else % irk irk_gnsf - ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); -end -% constraints -ocp_model.set('constr_x0', x0); -if (ng>0) - ocp_model.set('constr_C', C); - ocp_model.set('constr_D', D); - ocp_model.set('constr_lg', lg); - ocp_model.set('constr_ug', ug); - ocp_model.set('constr_C_e', C_e); - ocp_model.set('constr_lg_e', lg_e); - ocp_model.set('constr_ug_e', ug_e); -elseif (nh>0) - ocp_model.set('constr_expr_h_0', model.constr_expr_h); - ocp_model.set('constr_lh_0', lbu); - ocp_model.set('constr_uh_0', ubu); - ocp_model.set('constr_expr_h', model.constr_expr_h); - ocp_model.set('constr_lh', lbu); - ocp_model.set('constr_uh', ubu); -% ocp_model.set('constr_expr_h_e', model.expr_h_e); -% ocp_model.set('constr_lh_e', lh_e); -% ocp_model.set('constr_uh_e', uh_e); -else -% ocp_model.set('constr_Jbx', Jbx); -% ocp_model.set('constr_lbx', lbx); -% ocp_model.set('constr_ubx', ubx); - ocp_model.set('constr_Jbu', Jbu); - ocp_model.set('constr_lbu', lbu); - ocp_model.set('constr_ubu', ubu); -end - -%% acados ocp opts -ocp_opts = acados_ocp_opts(); -ocp_opts.set('compile_interface', compile_interface); -ocp_opts.set('param_scheme_N', N); -ocp_opts.set('nlp_solver', nlp_solver); -ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); -ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); -if (strcmp(nlp_solver, 'sqp')) - ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); - ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat); - ocp_opts.set('nlp_solver_tol_eq', nlp_solver_tol_eq); - ocp_opts.set('nlp_solver_tol_ineq', nlp_solver_tol_ineq); - ocp_opts.set('nlp_solver_tol_comp', nlp_solver_tol_comp); -end -ocp_opts.set('qp_solver', qp_solver); -ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); -ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); -ocp_opts.set('qp_solver_iter_max', qp_solver_max_iter); -if (~isempty(strfind(qp_solver, 'partial_condensing'))) - ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); -end -if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); -end -ocp_opts.set('sim_method', sim_method); -ocp_opts.set('sim_method_num_stages', sim_method_num_stages); -ocp_opts.set('sim_method_num_steps', sim_method_num_steps); -if (strcmp(sim_method, 'irk_gnsf')) - ocp_opts.set('gnsf_detect_struct', gnsf_detect_struct); -end - -%% create acados OCP solver -solver_creation = 'transcribe_explicit'; - -if strcmp(solver_creation, 'legacy') - % legacy interface - ocp_solver = acados_ocp(ocp_model, ocp_opts); -elseif strcmp(solver_creation, 'transcribe_explicit') - % test translation to new OCP formulation object - ocp = setup_AcadosOcp_from_legacy_ocp_description(ocp_model, ocp_opts); - ocp_solver = AcadosOcpSolver(ocp); -end - - -% set trajectory initialization -%x_traj_init = zeros(nx, N+1); -%for ii=1:N x_traj_init(:,ii) = [0; pi; 0; 0]; end -x_traj_init = [linspace(0, 0, N+1); linspace(pi, 0, N+1); linspace(0, 0, N+1); linspace(0, 0, N+1)]; - +import casadi.* + +check_acados_requirements() + + +%% solver settings +N = 20; % number of discretization steps +T = 1; % [s] prediction horizon length +x0 = [0; pi; 0; 0]; % initial state + +%% model dynamics +model = get_pendulum_on_cart_model(); +nx = length(model.x); % state size +nu = length(model.u); % input size + +%% OCP formulation object +ocp = AcadosOcp(); +ocp.model = model; + +%% cost in nonlinear least squares form +W_x = diag([1e3, 1e3, 1e-2, 1e-2]); +W_u = 1e-2; + +% initial cost term +ny_0 = nu; +ocp.cost.cost_type_0 = 'NONLINEAR_LS'; +ocp.cost.W_0 = W_u; +ocp.cost.yref_0 = zeros(ny_0, 1); +ocp.model.cost_y_expr_0 = model.u; + +% path cost term +ny = nx + nu; +ocp.cost.cost_type = 'NONLINEAR_LS'; +ocp.cost.W = blkdiag(W_x, W_u); +ocp.cost.yref = zeros(ny, 1); +ocp.model.cost_y_expr = vertcat(model.x, model.u); + +% terminal cost term +ny_e = nx; +ocp.cost.cost_type_e = 'NONLINEAR_LS'; +ocp.model.cost_y_expr_e = model.x; +ocp.cost.yref_e = zeros(ny_e, 1); +ocp.cost.W_e = W_x; + +%% define constraints +% only bound on u on initial stage and path +ocp.model.con_h_expr = model.u; +ocp.model.con_h_expr_0 = model.u; + +U_max = 80; +ocp.constraints.lh = -U_max; +ocp.constraints.lh_0 = -U_max; +ocp.constraints.uh = U_max; +ocp.constraints.uh_0 = U_max; +ocp.constraints.x0 = x0; + +% define solver options +ocp.solver_options.N_horizon = N; +ocp.solver_options.tf = T; +ocp.solver_options.nlp_solver_type = 'SQP'; +ocp.solver_options.integrator_type = 'ERK'; +ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; +ocp.solver_options.qp_solver_mu0 = 1e3; +ocp.solver_options.qp_solver_cond_N = 5; +ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; +ocp.solver_options.ext_fun_compile_flags = '-O2'; +ocp.solver_options.globalization = 'MERIT_BACKTRACKING'; + +% create solver +ocp_solver = AcadosOcpSolver(ocp); + +% solver initial guess +x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); -% if not set, the trajectory is initialized with the previous solution -ocp_solver.set('init_x', x_traj_init); -ocp_solver.set('init_u', u_traj_init); - -% change number of sqp iterations -%ocp_solver.set('nlp_solver_max_iter', 20); +%% call ocp solver +% set trajectory initialization +ocp_solver.set('init_x', x_traj_init); % states +ocp_solver.set('init_u', u_traj_init); % inputs +ocp_solver.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints % solve -tic; - -% solve ocp ocp_solver.solve(); - -time_ext = toc; -% TODO: add getter for internal timing -fprintf(['time for ocp_solver.solve (matlab tic-toc): ', num2str(time_ext), ' s\n']) - % get solution -u = ocp_solver.get('u'); -x = ocp_solver.get('x'); - -%% evaluation -status = ocp_solver.get('status'); -sqp_iter = ocp_solver.get('sqp_iter'); -time_tot = ocp_solver.get('time_tot'); -time_lin = ocp_solver.get('time_lin'); -time_reg = ocp_solver.get('time_reg'); -time_qp_sol = ocp_solver.get('time_qp_sol'); - -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); - -ocp_solver.print('stat'); - - -%% figures - -for ii=1:N+1 - x_cur = x(:,ii); -% visualize; +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); + +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') + +%% plots +ts = linspace(0, T, N+1); +figure; hold on; +states = {'p', 'theta', 'v', 'dtheta'}; +for i=1:length(states) + subplot(length(states), 1, i); + plot(ts, xtraj(i,:)); grid on; + ylabel(states{i}); + xlabel('t [s]') end -figure; -subplot(2,1,1); -plot(0:N, x); -xlim([0 N]); -legend('p', 'theta', 'v', 'omega'); -subplot(2,1,2); -plot(0:N-1, u); -xlim([0 N]); -legend('F'); - -stat = ocp_solver.get('stat'); -if (strcmp(nlp_solver, 'sqp')) - figure; - plot([0: size(stat,1)-1], log10(stat(:,2)), 'r-x'); - hold on - plot([0: size(stat,1)-1], log10(stat(:,3)), 'b-x'); - plot([0: size(stat,1)-1], log10(stat(:,4)), 'g-x'); - plot([0: size(stat,1)-1], log10(stat(:,5)), 'k-x'); -% semilogy(0: size(stat,1)-1, stat(:,2), 'r-x'); -% hold on -% semilogy(0: size(stat,1)-1, stat(:,3), 'b-x'); -% semilogy(0: size(stat,1)-1, stat(:,4), 'g-x'); -% semilogy(0: size(stat,1)-1, stat(:,5), 'k-x'); - hold off - xlabel('iter') - ylabel('res') - legend('res stat', 'res eq', 'res ineq', 'res compl'); -end - - -if status==0 - fprintf('\nsuccess!\n\n'); -else - fprintf('\nsolution failed!\n\n'); -end - - -if is_octave() - waitforbuttonpress; -end +figure +stairs(ts, [utraj'; utraj(end)]) +ylabel('F [N]') +xlabel('t [s]') +grid on diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_AcadosModel.m b/examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_model.m similarity index 98% rename from examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_AcadosModel.m rename to examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_model.m index 8b6522bb45..af7fc2a19b 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_AcadosModel.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/get_pendulum_on_cart_model.m @@ -27,7 +27,7 @@ % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE.; -function model = get_pendulum_on_cart_AcadosModel() +function model = get_pendulum_on_cart_model() import casadi.* diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m b/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m index 3768d85bd9..5981d571a0 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m @@ -40,7 +40,7 @@ %% model dynamics -model = get_pendulum_on_cart_AcadosModel(); +model = get_pendulum_on_cart_model(); nx = length(model.x); % state size nu = length(model.u); % input size diff --git a/examples/acados_matlab_octave/test/create_ocp_solver_code_reuse.m b/examples/acados_matlab_octave/test/create_ocp_solver_code_reuse.m new file mode 100644 index 0000000000..9f18ab106e --- /dev/null +++ b/examples/acados_matlab_octave/test/create_ocp_solver_code_reuse.m @@ -0,0 +1,121 @@ +% +% 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.; + + +function ocp_solver = create_ocp_solver_code_reuse(creation_mode) + + json_file = 'pendulum_ocp.json'; + solver_creation_opts = struct(); + solver_creation_opts.json_file = json_file; + if strcmp(creation_mode, 'standard') + disp('Standard creation mode'); + elseif strcmp(creation_mode, 'precompiled') || strcmp(creation_mode, 'no_ocp') + solver_creation_opts.generate = false; + solver_creation_opts.build = false; + solver_creation_opts.compile_mex_wrapper = false; + else + error('Invalid creation mode') + end + + if strcmp(creation_mode, 'no_ocp') + ocp = []; + else + ocp = create_acados_ocp_formulation(); + end + + % create solver + ocp_solver = AcadosOcpSolver(ocp, solver_creation_opts); + +end + +function ocp = create_acados_ocp_formulation() + %% solver settings + N = 20; % number of discretization steps + T = 1; % [s] prediction horizon length + x0 = [0; pi; 0; 0]; % initial state + + %% model + addpath('../pendulum_on_cart_model/'); + model = get_pendulum_on_cart_model(); + nx = length(model.x); % state size + nu = length(model.u); % input size + + %% OCP formulation object + ocp = AcadosOcp(); + ocp.model = model; + + %% cost in nonlinear least squares form + W_x = diag([1e3, 1e3, 1e-2, 1e-2]); + W_u = 1e-2; + + % initial cost term + ny_0 = nu; + ocp.cost.cost_type_0 = 'NONLINEAR_LS'; + ocp.cost.W_0 = W_u; + ocp.cost.yref_0 = zeros(ny_0, 1); + ocp.model.cost_y_expr_0 = model.u; + + % path cost term + ny = nx + nu; + ocp.cost.cost_type = 'NONLINEAR_LS'; + ocp.cost.W = blkdiag(W_x, W_u); + ocp.cost.yref = zeros(ny, 1); + ocp.model.cost_y_expr = vertcat(model.x, model.u); + + % terminal cost term + ny_e = nx; + ocp.cost.cost_type_e = 'NONLINEAR_LS'; + ocp.model.cost_y_expr_e = model.x; + ocp.cost.yref_e = zeros(ny_e, 1); + ocp.cost.W_e = W_x; + + %% define constraints + % only bound on u on initial stage and path + ocp.model.con_h_expr = model.u; + ocp.model.con_h_expr_0 = model.u; + + U_max = 80; + ocp.constraints.lh = -U_max; + ocp.constraints.lh_0 = -U_max; + ocp.constraints.uh = U_max; + ocp.constraints.uh_0 = U_max; + ocp.constraints.x0 = x0; + + % define solver options + ocp.solver_options.N_horizon = N; + ocp.solver_options.tf = T; + ocp.solver_options.nlp_solver_type = 'SQP'; + ocp.solver_options.integrator_type = 'ERK'; + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + ocp.solver_options.qp_solver_mu0 = 1e3; + ocp.solver_options.qp_solver_cond_N = 5; + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; + ocp.solver_options.ext_fun_compile_flags = '-O2'; + ocp.solver_options.globalization = 'MERIT_BACKTRACKING'; +end diff --git a/examples/acados_matlab_octave/test/run_matlab_tests.m b/examples/acados_matlab_octave/test/run_matlab_tests.m index 16ff8c8940..ee19069e9b 100644 --- a/examples/acados_matlab_octave/test/run_matlab_tests.m +++ b/examples/acados_matlab_octave/test/run_matlab_tests.m @@ -53,7 +53,9 @@ disp('running tests') %% run all tests -test_names = ["run_test_dim_check", +test_names = [ + "test_code_reuse", + "run_test_dim_check", "run_test_ocp_mass_spring", % "run_test_ocp_pendulum", "run_test_ocp_wtnx6", diff --git a/examples/acados_matlab_octave/test/simulink_sparse_param_test.m b/examples/acados_matlab_octave/test/simulink_sparse_param_test.m index d7ffe3acbb..d992288969 100644 --- a/examples/acados_matlab_octave/test/simulink_sparse_param_test.m +++ b/examples/acados_matlab_octave/test/simulink_sparse_param_test.m @@ -69,6 +69,8 @@ ocp.simulink_opts = add_sparse_param_port_simulink(ocp.simulink_opts, 12, 'p12_stage3', 3, 3); ocp.simulink_opts = add_sparse_param_port_simulink(ocp.simulink_opts, 12, 'p12_stage6', 6, 6); +ocp.code_export_directory = 'acados_generated_code'; + %% create ocp solver ocp_solver = AcadosOcpSolver(ocp); @@ -99,7 +101,7 @@ end %% simulink test -cd c_generated_code +cd(ocp.code_export_directory) make_sfun; % ocp solver cd ..; diff --git a/examples/acados_matlab_octave/test/test_code_reuse.m b/examples/acados_matlab_octave/test/test_code_reuse.m new file mode 100644 index 0000000000..52621c13b1 --- /dev/null +++ b/examples/acados_matlab_octave/test/test_code_reuse.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.; + +import casadi.* + +check_acados_requirements() +creation_modes = {'standard', 'precompiled', 'no_ocp'}; +for i = 1:length(creation_modes) + ocp_solver = create_ocp_solver_code_reuse(creation_modes{i}); + nx = length(ocp_solver.get('x', 0)); + [nu, N] = size(ocp_solver.get('u')); + T = 1; + + % solver initial guess + x_traj_init = zeros(nx, N+1); + u_traj_init = zeros(nu, N); + + %% call ocp solver + % set trajectory initialization + ocp_solver.set('init_x', x_traj_init); % states + ocp_solver.set('init_u', u_traj_init); % inputs + ocp_solver.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints + + % solve + ocp_solver.solve(); + % get solution + utraj = ocp_solver.get('u'); + xtraj = ocp_solver.get('x'); + + status = ocp_solver.get('status'); % 0 - success + ocp_solver.print('stat'); + stat = ocp_solver.get('stat'); + if i == 1 + stat_ref = stat; + elseif max(abs(stat-stat_ref)) > 1e-6 + error('solvers should have the same log independent of compilation options'); + end + + clear ocp_solver +end diff --git a/examples/acados_python/crane/time_optimal_example.py b/examples/acados_python/crane/time_optimal_example.py index 3f2a131714..e4d3555b80 100644 --- a/examples/acados_python/crane/time_optimal_example.py +++ b/examples/acados_python/crane/time_optimal_example.py @@ -57,7 +57,8 @@ def plot_crane_trajectories(ts, simX, simU): plt.show() -def setup_solver_and_integrator(x0: np.ndarray, xf: np.ndarray, N: int, use_cython: bool = True) -> Tuple[AcadosOcpSolver, AcadosSimSolver]: + +def setup_solver_and_integrator(x0: np.ndarray, xf: np.ndarray, N: int, creation_mode: str) -> Tuple[AcadosOcpSolver, AcadosSimSolver]: # (very) simple crane model beta = 0.001 @@ -126,13 +127,20 @@ def setup_solver_and_integrator(x0: np.ndarray, xf: np.ndarray, N: int, use_cyth ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.exact_hess_dyn = 0 - if use_cython: - AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json') + ocp_json_file = 'acados_ocp.json' + if creation_mode == 'cython': + AcadosOcpSolver.generate(ocp, json_file=ocp_json_file) AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) - ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json') - else: # ctypes + ocp_solver = AcadosOcpSolver.create_cython_solver(ocp_json_file) + elif creation_mode == 'ctypes_precompiled': ## Note: skip generate and build assuming this is done before (in cython run) - ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', build=False, generate=False) + ocp_solver = AcadosOcpSolver(ocp, json_file=ocp_json_file, build=False, generate=False) + elif creation_mode == 'ctypes_precompiled_no_ocp': + ocp_solver = AcadosOcpSolver(None, json_file=ocp_json_file, build=False, generate=False) + elif creation_mode == 'ctypes': + ocp_solver = AcadosOcpSolver(ocp, json_file=ocp_json_file) + else: + raise Exception(f"Invalid creation mode: {creation_mode}") ocp_solver.reset() @@ -148,20 +156,20 @@ def setup_solver_and_integrator(x0: np.ndarray, xf: np.ndarray, N: int, use_cyth return ocp_solver, integrator -def main(use_cython=True): +def main(creation_mode=True): nu = 2 nx = 4 - N = 7 # N - maximum number of bangs + N_horizon = 7 # N_horizon - maximum number of bangs x0 = np.array([2.0, 0.0, 2.0, 0.0]) xf = np.zeros((nx,)) - ocp_solver, integrator = setup_solver_and_integrator(x0, xf, N, use_cython) + ocp_solver, integrator = setup_solver_and_integrator(x0, xf, N_horizon, creation_mode) # initialization - for i, tau in enumerate(np.linspace(0, 1, N)): + for i, tau in enumerate(np.linspace(0, 1, N_horizon)): ocp_solver.set(i, 'x', (1-tau)*x0 + tau*xf) ocp_solver.set(i, 'u', np.array([0.1, 0.5])) @@ -172,12 +180,12 @@ def main(use_cython=True): raise Exception(f'acados returned status {status}.') # get solution - simX = np.zeros((N+1, nx)) - simU = np.zeros((N, nu)) - for i in range(N): + simX = np.zeros((N_horizon+1, nx)) + simU = np.zeros((N_horizon, nu)) + for i in range(N_horizon): simX[i,:] = ocp_solver.get(i, "x") simU[i,:] = ocp_solver.get(i, "u") - simX[N,:] = ocp_solver.get(N, "x") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") dts = simU[:, 1] @@ -187,11 +195,11 @@ def main(use_cython=True): # simulate on finer grid dt_approx = 0.0005 - dts_fine = np.zeros((N,)) - Ns_fine = np.zeros((N,), dtype='int16') + dts_fine = np.zeros((N_horizon,)) + Ns_fine = np.zeros((N_horizon,), dtype='int16') # compute number of simulation steps for bang interval + dt_fine - for i in range(N): + for i in range(N_horizon): N_approx = max(int(dts[i]/dt_approx), 1) dts_fine[i] = dts[i]/N_approx Ns_fine[i] = int(round(dts[i]/dts_fine[i])) @@ -204,7 +212,7 @@ def main(use_cython=True): simX_fine[0, :] = x0 k = 0 - for i in range(N): + for i in range(N_horizon): u = simU[i, 0] integrator.set("u", np.hstack((u, np.ones(1, )))) @@ -225,8 +233,9 @@ def main(use_cython=True): plot_crane_trajectories(ts_fine, simX_fine, simU_fine) +CREATION_MODES = ['cython', 'ctypes_precompiled', 'ctypes', 'ctypes_precompiled_no_ocp'] if __name__ == "__main__": - for use_cython in [True, False]: - main(use_cython=use_cython) + for creation_mode in ['cython', 'ctypes_precompiled_no_ocp', 'ctypes_precompiled']: + main(creation_mode=creation_mode) diff --git a/examples/acados_python/mocp_transition_example/main.py b/examples/acados_python/mocp_transition_example/main.py index 9e0b8e10a0..a8da19e6bb 100644 --- a/examples/acados_python/mocp_transition_example/main.py +++ b/examples/acados_python/mocp_transition_example/main.py @@ -181,7 +181,6 @@ def create_multiphase_ocp_solver(N_list, t_horizon_1, name=None, use_cmake=False phase_1.model = get_transition_model() phase_1.cost.cost_type = 'NONLINEAR_LS' phase_1.model.cost_y_expr = phase_1.model.x - # TODO: how to choose this transition cost phase_1.cost.W = np.diag([L2_COST_P, 1e-1 * L2_COST_V]) phase_1.cost.yref = np.array([0., 0.]) diff --git a/examples/acados_python/p_global_example/code_reuse_py2matlab.m b/examples/acados_python/p_global_example/code_reuse_py2matlab.m new file mode 100644 index 0000000000..2fb7ed84eb --- /dev/null +++ b/examples/acados_python/p_global_example/code_reuse_py2matlab.m @@ -0,0 +1,61 @@ +% +% 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.; + +import casadi.* + +check_acados_requirements() + +json_files = {'acados_ocp_pendulum_blazing_True_p_global_True.json', 'mocp.json'}; + +for i = 1:length(json_files) + json_file = json_files{i}; + disp('testing solver creation with code reuse with json file: ') + disp(json_file) + solver_creation_opts = struct(); + solver_creation_opts.json_file = json_file; + solver_creation_opts.generate = false; + solver_creation_opts.build = false; + solver_creation_opts.compile_mex_wrapper = true; + ocp = []; + + % create solver + ocp_solver = AcadosOcpSolver(ocp, solver_creation_opts); + + nx = length(ocp_solver.get('x', 0)); + [nu, N] = size(ocp_solver.get('u')); + + for i = 1:5 + ocp_solver.solve(); + + status = ocp_solver.get('status'); + ocp_solver.print('stat'); + end + clear ocp_solver +end + diff --git a/examples/acados_python/p_global_example/example_p_global.py b/examples/acados_python/p_global_example/example_p_global.py index f7878893d2..393f037f0c 100644 --- a/examples/acados_python/p_global_example/example_p_global.py +++ b/examples/acados_python/p_global_example/example_p_global.py @@ -28,7 +28,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, AcadosMultiphaseOcp +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, AcadosMultiphaseOcp, get_simulink_default_opts import numpy as np import scipy.linalg from utils import plot_pendulum @@ -194,7 +194,7 @@ def create_ocp_formulation_without_opts(p_global, m, l, C, lut=True, use_p_globa return ocp -def main(use_cython=False, lut=True, use_p_global=True, blazing=True): +def main(use_cython=False, lut=True, use_p_global=True, blazing=True, with_matlab_templates=False, code_export_directory=None): print(f"\n\nRunning example with lut={lut}, use_p_global={use_p_global}, {blazing=}") p_global, m, l, C, p_global_values = create_p_global(lut=lut) @@ -217,6 +217,8 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True): ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP ocp.solver_options.ext_fun_compile_flags += ' -I' + ca.GlobalOptions.getCasadiIncludePath() + ' -ffast-math -march=native' + if code_export_directory is not None: + ocp.code_export_directory = code_export_directory # set prediction horizon ocp.solver_options.tf = Tf @@ -224,7 +226,8 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True): # create ocp solver print(f"Creating ocp solver with p_global = {ocp.model.p_global}, p = {ocp.model.p}") - + if with_matlab_templates: + ocp.simulink_opts = get_simulink_default_opts() solver_json = 'acados_ocp_' + ocp.model.name + '.json' if use_cython: AcadosOcpSolver.generate(ocp, json_file=solver_json) @@ -258,7 +261,7 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True): return residuals, timing -def main_mocp(lut=True, use_p_global=True): +def main_mocp(lut=True, use_p_global=True, with_matlab_templates=False): print(f"\n\nRunning multi-phase example with lut={lut}, use_p_global={use_p_global}") p_global, m, l, C, p_global_values = create_p_global(lut=lut) @@ -299,6 +302,8 @@ def main_mocp(lut=True, use_p_global=True): # create ocp solver print(f"Creating ocp solver with p_global = {mocp.model[0].p_global}, p_phase_1 = {mocp.model[0].p}, p_phase_2 = {mocp.model[1].p}") + if with_matlab_templates: + mocp.simulink_opts = get_simulink_default_opts() ocp_solver = AcadosOcpSolver(mocp, generate=True, build=True) # call SQP_RTI solver in the loop: @@ -320,12 +325,12 @@ def main_mocp(lut=True, use_p_global=True): if __name__ == "__main__": # OCP with lookuptable, comparing blazing, bspline, p_global - ref_lut, t_lin_lut_ref = main(use_cython=False, use_p_global=False, lut=True) - res_lut, t_lin_lut = main(use_cython=False, use_p_global=True, lut=True) - ref_lut_no_blazing, t_lin_lut_no_blazing_ref = main(use_cython=False, use_p_global=False, lut=True, blazing=False) res_lut_no_blazing, t_lin_lut_no_blazing = main(use_cython=False, use_p_global=True, lut=True, blazing=False) + ref_lut, t_lin_lut_ref = main(use_cython=False, use_p_global=False, lut=True) + res_lut, t_lin_lut = main(use_cython=False, use_p_global=True, lut=True) + print(f"\t\t bspline \t blazing") print(f"ref\t\t {t_lin_lut_no_blazing_ref:.5f} \t {t_lin_lut_ref:.5f}") print(f"p_global\t {t_lin_lut_no_blazing:.5f} \t {t_lin_lut:.5f}") @@ -337,7 +342,6 @@ def main_mocp(lut=True, use_p_global=True): np.testing.assert_almost_equal(res_lut, res_lut_no_blazing) np.testing.assert_almost_equal(ref_lut, res_lut_no_blazing) - ref_nolut, _ = main(use_cython=False, use_p_global=False, lut=False) res_nolut, _ = main(use_cython=False, use_p_global=True, lut=False) np.testing.assert_almost_equal(ref_nolut, res_nolut) @@ -349,12 +353,12 @@ def main_mocp(lut=True, use_p_global=True): np.testing.assert_almost_equal(ref_nolut, res_mocp_nolut_p_global) res_mocp_lut_p, _ = main_mocp(use_p_global=False, lut=True) - res_mocp_lut_p_global, _ = main_mocp(use_p_global=True, lut=True) + res_mocp_lut_p_global, _ = main_mocp(use_p_global=True, lut=True, with_matlab_templates=True) np.testing.assert_almost_equal(ref_lut, res_mocp_lut_p) np.testing.assert_almost_equal(ref_lut, res_mocp_lut_p_global) - with np.testing.assert_raises(Exception): np.testing.assert_almost_equal(ref_lut, ref_nolut) - # main(use_cython=True) TODO: fix cython \ No newline at end of file + # to test transfer to MATLAB/Octave + res_lut, t_lin_lut = main(use_cython=False, use_p_global=True, lut=True, with_matlab_templates=True, code_export_directory='c_generated_code_single_phase') diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 4202668607..859c987e17 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -64,6 +64,9 @@ if(ACADOS_OCTAVE) add_test(NAME octave_test_ocp_pendulum COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test octave --no-gui --no-window-system ./run_test_ocp_pendulum.m) + add_test(NAME octave_test_ocp_pendulum_code_reuse + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test + octave --no-gui --no-window-system ./test_code_reuse.m) add_test(NAME octave_test_ocp_wtnx6 COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test octave --no-gui --no-window-system ./run_test_ocp_wtnx6.m) @@ -119,8 +122,9 @@ if(ACADOS_OCTAVE) set_tests_properties(octave_test_ocp_wtnx6 PROPERTIES DEPENDS octave_test_ocp_mass_spring) set_tests_properties(octave_test_ocp_mass_spring PROPERTIES DEPENDS octave_test_ocp_simple_dae) set_tests_properties(octave_test_ocp_simple_dae PROPERTIES DEPENDS octave_test_target_selector) + set_tests_properties(octave_test_target_selector PROPERTIES DEPENDS octave_test_ocp_pendulum_code_reuse) if(ACADOS_WITH_OSQP) - set_tests_properties(octave_test_target_selector PROPERTIES DEPENDS octave_test_OSQP) + set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS octave_test_OSQP) endif() if(ACADOS_WITH_QPDUNES) set_tests_properties(octave_test_OSQP PROPERTIES DEPENDS octave_test_qpDUNES) diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 25de67440e..94cad1cacf 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -31,51 +31,125 @@ classdef AcadosOcpSolver < handle - properties - t_ocp % templated solver + properties (Access = public) ocp % Matlab class AcadosOcp describing the OCP formulation - qp_gettable_fields = {'qp_Q', 'qp_R', 'qp_S', 'qp_q', 'qp_r', 'qp_A', 'qp_B', 'qp_b', 'qp_C', 'qp_D', 'qp_lg', 'qp_ug', 'qp_lbx', 'qp_ubx', 'qp_lbu', 'qp_ubu', 'qp_zl', 'qp_zu', 'qp_Zl', 'qp_Zu'} end % properties + properties (Access = private) + fields = {'x', 'u', 'z', 'sl', 'su', 'lam', 'pi'}; + qp_gettable_fields = {'qp_Q', 'qp_R', 'qp_S', 'qp_q', 'qp_r', 'qp_A', 'qp_B', 'qp_b', 'qp_C', 'qp_D', 'qp_lg', 'qp_ug', 'qp_lbx', 'qp_ubx', 'qp_lbu', 'qp_ubu', 'qp_zl', 'qp_zu', 'qp_Zl', 'qp_Zu'} + t_ocp % templated solver + % required info loaded from json + N_horizon + solver_options + problem_class + name + has_x0 + nsbu_0 + nbxe_0 + end methods - function obj = AcadosOcpSolver(ocp, output_dir) + function obj = AcadosOcpSolver(ocp, varargin) + %% optional arguments: + % varagin{1}: solver_creation_opts: this is a struct in which some of the fields can be defined to overwrite the default values. + % The fields are: + % - json_file: path to the json file containing the ocp description + % - build: boolean, if true, the problem specific shared library is compiled + % - generate: boolean, if true, the C code is generated + % - compile_mex_wrapper: boolean, if true, the mex wrapper is compiled + % - compile_interface: can be [], true or false. If [], the interface is compiled if it does not exist. + % - output_dir: path to the directory where the MEX interface is compiled + obj.ocp = ocp; - if nargin < 2 - output_dir = fullfile(pwd, 'build'); + % optional arguments + % solver creation options + default_solver_creation_opts = struct('json_file', '', ... + 'build', true, ... + 'generate', true, ... + 'compile_mex_wrapper', true, ... + 'compile_interface', [], ... + 'output_dir', fullfile(pwd, 'build')); + if length(varargin) > 0 + solver_creation_opts = varargin{1}; + % set non-specified opts to default + fields = fieldnames(default_solver_creation_opts); + for i = 1:length(fields) + if ~isfield(solver_creation_opts, fields{i}) + solver_creation_opts.(fields{i}) = default_solver_creation_opts.(fields{i}); + end + end + else + solver_creation_opts = default_solver_creation_opts; end - % detect dimensions & sanity checks - obj.ocp = ocp; - obj.ocp.make_consistent() + if isempty(ocp) && isempty(solver_creation_opts.json_file) + error('AcadosOcpSolver: provide either an OCP object or a json file'); + end - % compile mex interface if needed - obj.compile_mex_interface_if_needed(output_dir) + if isempty(ocp) + json_file = solver_creation_opts.json_file; - % generate - check_dir_and_create(fullfile(pwd, ocp.code_export_directory)); - context = ocp.generate_external_functions(); + else + % OCP / MOCP provided + if ~isempty(solver_creation_opts.json_file) + ocp.json_file = solver_creation_opts.json_file; + end + json_file = ocp.json_file; + if ~isempty(ocp.solver_options.compile_interface) && ~isempty(solver_creation_opts.compile_interface) + error('AcadosOcpSolver: provide either compile_interface in OCP object or solver_creation_opts'); + end + if ~isempty(ocp.solver_options.compile_interface) + solver_creation_opts.compile_interface = ocp.solver_options.compile_interface; + end - ocp.dump_to_json() - ocp.render_templates() + end - % build - acados_template_mex.compile_ocp_shared_lib(ocp.code_export_directory) - % templated MEX + %% compile mex interface if needed + obj.compile_mex_interface_if_needed(solver_creation_opts); + + %% generate + if solver_creation_opts.generate + obj.generate(ocp); + end + + %% load json, store options in object + acados_ocp_struct = loadjson(fileread(json_file), 'SimplifyCell', 0); + obj.problem_class = acados_ocp_struct.problem_class; + obj.solver_options = acados_ocp_struct.solver_options; + obj.N_horizon = acados_ocp_struct.solver_options.N_horizon; + obj.name = acados_ocp_struct.name; + + if strcmp(obj.problem_class, "OCP") + obj.has_x0 = acados_ocp_struct.constraints.has_x0; + obj.nsbu_0 = acados_ocp_struct.dims.nsbu; + obj.nbxe_0 = acados_ocp_struct.dims.nbxe_0; + elseif strcmp(obj.problem_class, "MOCP") + obj.has_x0 = acados_ocp_struct.constraints{1}.has_x0; + obj.nsbu_0 = acados_ocp_struct.phases_dims{1}.nsbu; + obj.nbxe_0 = acados_ocp_struct.phases_dims{1}.nbxe_0; + end + code_export_directory = acados_ocp_struct.code_export_directory; + + %% compile problem specific shared library + if solver_creation_opts.build + acados_template_mex.compile_ocp_shared_lib(code_export_directory) + end + + %% create solver return_dir = pwd(); - cd(obj.ocp.code_export_directory) + cd(code_export_directory) - mex_solver_name = sprintf('%s_mex_solver', obj.ocp.name); + mex_solver_name = sprintf('%s_mex_solver', obj.name); mex_solver = str2func(mex_solver_name); - obj.t_ocp = mex_solver(); + obj.t_ocp = mex_solver(solver_creation_opts); addpath(pwd()); cd(return_dir); end - function solve(obj) obj.t_ocp.solve(); end @@ -113,7 +187,7 @@ function set(obj, field, value, varargin) if length(varargin) > 0 n = varargin{1}; - if n < obj.ocp.solver_options.N_horizon + if n < obj.solver_options.N_horizon Q = obj.get('qp_Q', n); R = obj.get('qp_R', n); S = obj.get('qp_S', n); @@ -125,20 +199,20 @@ function set(obj, field, value, varargin) return; else - value = cell(obj.ocp.solver_options.N_horizon, 1); - for n=0:(obj.ocp.solver_options.N_horizon-1) + value = cell(obj.solver_options.N_horizon, 1); + for n=0:(obj.solver_options.N_horizon-1) Q = obj.get('qp_Q', n); R = obj.get('qp_R', n); S = obj.get('qp_S', n); value{n+1} = [R, S; S', Q]; end - value{end+1} = obj.get('qp_Q', obj.ocp.solver_options.N_horizon); + value{end+1} = obj.get('qp_Q', obj.solver_options.N_horizon); return; end elseif strcmp('pc_hess_block', field) - if ~strncmp(obj.ocp.solver_options.qp_solver, 'PARTIAL_CONDENSING', length('PARTIAL_CONDENSING')) + if ~strncmp(obj.solver_options.qp_solver, 'PARTIAL_CONDENSING', length('PARTIAL_CONDENSING')) error("Getting hessian block of partially condensed QP only works for PARTIAL_CONDENSING QP solvers"); end if length(varargin) > 0 @@ -188,15 +262,15 @@ function set(obj, field, value, varargin) error("iteration needs to be nonnegative and <= nlp_iter."); end - if ~obj.ocp.solver_options.store_iterates + if ~obj.solver_options.store_iterates error("get_iterate: the solver option store_iterates needs to be true in order to get iterates."); end - if strcmp(obj.ocp.solver_options.nlp_solver_type, 'SQP_RTI') + if strcmp(obj.solver_options.nlp_solver_type, 'SQP_RTI') error("get_iterate: SQP_RTI not supported."); end - N_horizon = obj.ocp.solver_options.N_horizon; + N_horizon = obj.solver_options.N_horizon; x_traj = cell(N_horizon + 1, 1); u_traj = cell(N_horizon, 1); @@ -267,9 +341,9 @@ function reset(obj) end if partially_condensed_qp - num_blocks = obj.ocp.solver_options.qp_solver_cond_N + 1; + num_blocks = obj.solver_options.qp_solver_cond_N + 1; else - num_blocks = obj.ocp.dims.N + 1; + num_blocks = obj.N_horizon + 1; end result = struct(); result.min_eigv_stage = zeros(num_blocks, 1); @@ -306,12 +380,12 @@ function reset(obj) function dump_last_qp_to_json(obj, filename) qp_data = struct(); - lN = length(num2str(obj.ocp.solver_options.N_horizon+1)); + lN = length(num2str(obj.solver_options.N_horizon+1)); n_fields = length(obj.qp_gettable_fields); for n=1:n_fields field = obj.qp_gettable_fields{n}; - for i=0:obj.ocp.solver_options.N_horizon-1 + for i=0:obj.solver_options.N_horizon-1 s_indx = sprintf(strcat('%0', num2str(lN), 'd'), i); key = strcat(field, '_', s_indx); val = obj.get(field, i); @@ -328,9 +402,9 @@ function dump_last_qp_to_json(obj, filename) strcmp(field, 'qp_zu') || ... strcmp(field, 'qp_Zl') || ... strcmp(field, 'qp_Zu') - s_indx = sprintf(strcat('%0', num2str(lN), 'd'), obj.ocp.solver_options.N_horizon); + s_indx = sprintf(strcat('%0', num2str(lN), 'd'), obj.solver_options.N_horizon); key = strcat(field, '_', s_indx); - val = obj.get(field, obj.ocp.solver_options.N_horizon); + val = obj.get(field, obj.solver_options.N_horizon); qp_data = setfield(qp_data, key, val); end end @@ -371,25 +445,36 @@ function set_p_global_and_precompute_dependencies(obj, val) end % methods methods (Access = private) + function generate(obj, ocp) + % detect dimensions & sanity checks + obj.ocp.make_consistent() - function compile_mex_interface_if_needed(obj, output_dir) + % generate + check_dir_and_create(fullfile(pwd, ocp.code_export_directory)); + context = ocp.generate_external_functions(); + + ocp.dump_to_json() + ocp.render_templates() + end + + function compile_mex_interface_if_needed(obj, solver_creation_opts) % check if path contains spaces - [~,~] = mkdir(output_dir); - addpath(output_dir); - if ~isempty(strfind(output_dir, ' ')) + [~,~] = mkdir(solver_creation_opts.output_dir); + addpath(solver_creation_opts.output_dir); + if ~isempty(strfind(solver_creation_opts.output_dir, ' ')) error(strcat('AcadosOcpSolver: Path should not contain spaces, got: ',... - output_dir)); + solver_creation_opts.output_dir)); end % auto detect whether to compile the interface or not - if isempty(obj.ocp.solver_options.compile_interface) + if isempty(solver_creation_opts.compile_interface) % check if mex interface exists already if is_octave() - mex_exists = exist( fullfile(output_dir,... + mex_exists = exist( fullfile(solver_creation_opts.output_dir,... '/ocp_get.mex'), 'file'); else - mex_exists = exist( fullfile(output_dir,... + mex_exists = exist( fullfile(solver_creation_opts.output_dir,... ['ocp_get.', mexext]), 'file'); end % check if mex interface is linked against the same external libs as the core @@ -403,30 +488,29 @@ function compile_mex_interface_if_needed(obj, output_dir) end core_links = loadjson(fileread(json_filename)); - json_filename = fullfile(output_dir, 'link_libs.json'); + json_filename = fullfile(solver_creation_opts.output_dir, 'link_libs.json'); if ~exist(json_filename, 'file') - obj.ocp.solver_options.compile_interface = true; + solver_creation_opts.compile_interface = true; else interface_links = loadjson(fileread(json_filename)); if isequal(core_links, interface_links) - obj.ocp.solver_options.compile_interface = false; + solver_creation_opts.compile_interface = false; else - obj.ocp.solver_options.compile_interface = true; + solver_creation_opts.compile_interface = true; end end else - obj.ocp.solver_options.compile_interface = true; + solver_creation_opts.compile_interface = true; end end - if obj.ocp.solver_options.compile_interface - ocp_compile_interface(output_dir); + if solver_creation_opts.compile_interface + ocp_compile_interface(solver_creation_opts.output_dir); disp('acados MEX interface compiled successfully') else disp('found compiled acados MEX interface') end end - end end % class diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index ebee1c9c60..92fda8f6e7 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -35,9 +35,8 @@ if nargin < 3 simulink_opts = get_acados_simulink_opts(); end - output_dir = opts.opts_struct.output_dir; ocp = setup_AcadosOcp_from_legacy_ocp_description(model, opts, simulink_opts); - solver = AcadosOcpSolver(ocp, output_dir); + solver = AcadosOcpSolver(ocp, struct('output_dir', opts.opts_struct.output_dir)); end diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index f87ce9c03a..253c463229 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -396,11 +396,11 @@ def __get_template_list(self, cmake_builder=None) -> list: template_list.append(('multi_Makefile.in', 'Makefile')) if self.phases_dims[0].np_global > 0: - template_list.append(('p_global_precompute_fun.in.h', f'{self.name}_p_global_precompute_fun.h')) + template_list.append(('p_global_precompute_fun.in.h', f'{name}_p_global_precompute_fun.h')) # Simulink if self.simulink_opts is not None: - raise NotImplementedError('Simulink not yet supported for multiphase OCPs.') + template_list += AcadosOcp._get_matlab_simulink_template_list(name) return template_list diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 8953c69307..3fc09c2a68 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1004,18 +1004,44 @@ def __get_template_list(self, cmake_builder=None) -> list: # Simulink if self.simulink_opts is not None: - template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') - template_list.append((template_file, f'acados_solver_sfunction_{name}.c')) - template_file = os.path.join('matlab_templates', 'make_sfun.in.m') - template_list.append((template_file, f'make_sfun_{name}.m')) - template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') - template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) - template_file = os.path.join('matlab_templates', 'make_sfun_sim.in.m') - template_list.append((template_file, f'make_sfun_sim_{name}.m')) + template_list += self._get_matlab_simulink_template_list(name) + template_list += self._get_integrator_simulink_template_list(name) return template_list + @classmethod + def _get_matlab_simulink_template_list(cls, name: str) -> list: + template_list = [] + template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') + template_list.append((template_file, f'acados_solver_sfunction_{name}.c')) + template_file = os.path.join('matlab_templates', 'make_sfun.in.m') + template_list.append((template_file, f'make_sfun_{name}.m')) + # MEX wrapper files + template_file = os.path.join('matlab_templates', 'mex_solver.in.m') + template_list.append((template_file, f'{name}_mex_solver.m')) + template_file = os.path.join('matlab_templates', 'make_mex.in.m') + template_list.append((template_file, f'make_mex_{name}.m')) + template_file = os.path.join('matlab_templates', 'acados_mex_create.in.c') + template_list.append((template_file, f'acados_mex_create_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_mex_free.in.c') + template_list.append((template_file, f'acados_mex_free_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_mex_solve.in.c') + template_list.append((template_file, f'acados_mex_solve_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_mex_set.in.c') + template_list.append((template_file, f'acados_mex_set_{name}.c')) + return template_list + + # dont render sim sfunctions for MOCP + @classmethod + def _get_integrator_simulink_template_list(cls, name: str) -> list: + template_list = [] + template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') + template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) + template_file = os.path.join('matlab_templates', 'make_sfun_sim.in.m') + template_list.append((template_file, f'make_sfun_sim_{name}.m')) + return template_list + def render_templates(self, cmake_builder=None): # check json file diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 56a642dde8..5e1af14374 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -89,7 +89,6 @@ def shared_lib(self,): """`shared_lib` - solver shared library""" return self.__shared_lib - # TODO move this to AcadosOcp @classmethod def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None): """ @@ -103,10 +102,14 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: `MS Visual Studio`); default: `None` """ acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) - acados_ocp.simulink_opts = simulink_opts # add kwargs to acados_ocp acados_ocp.json_file = json_file + if simulink_opts is not None: + if acados_ocp.simulink_opts is not None: + raise Exception('simulink_opts are already set in acados_ocp.') + else: + acados_ocp.simulink_opts = simulink_opts # make consistent acados_ocp.make_consistent() @@ -195,7 +198,15 @@ def create_cython_solver(cls, json_file): def save_p_global(self) -> bool: return self.__save_p_global - def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file=None, simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True, save_p_global=False): + @property + def N(self) -> int: + return self.__N + + @property + def name(self) -> int: + return self.__name + + def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json_file=None, simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True, save_p_global=False): self.solver_created = False self.__save_p_global = save_p_global @@ -204,26 +215,58 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file= else: self.__p_global_values = np.array([]) - if not (isinstance(acados_ocp, AcadosOcp) or isinstance(acados_ocp, AcadosMultiphaseOcp)): + if not (isinstance(acados_ocp, (AcadosOcp, AcadosMultiphaseOcp)) or acados_ocp is None): raise Exception('acados_ocp should be of type AcadosOcp or AcadosMultiphaseOcp.') - - if json_file is not None: - acados_ocp.json_file = json_file + if acados_ocp is None: + if json_file is None: + raise Exception('json_file should be provided if acados_ocp is None.') + if generate or build: + raise Exception('generate and build should be False if acados_ocp is None.') + if not os.path.exists(json_file): + raise Exception(f'json_file {json_file} does not exist.') if generate: + if json_file is not None: + acados_ocp.json_file = json_file self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) + json_file = acados_ocp.json_file else: - acados_ocp.make_consistent() + if acados_ocp is not None: + acados_ocp.make_consistent() # load json, store options in object - with open(acados_ocp.json_file, 'r') as f: + with open(json_file, 'r') as f: acados_ocp_json = json.load(f) - if isinstance(acados_ocp, AcadosOcp): - self.N = acados_ocp_json['dims']['N'] - elif isinstance(acados_ocp, AcadosMultiphaseOcp): - self.N = acados_ocp_json['N_horizon'] + self.__problem_class = acados_ocp_json['problem_class'] self.__solver_options = acados_ocp_json['solver_options'] - self.name = acados_ocp_json['name'] + self.__N = acados_ocp_json['solver_options']['N_horizon'] + self.__name = acados_ocp_json['name'] + + if self.__problem_class == "OCP": + self.__has_x0 = acados_ocp_json['constraints']['has_x0'] + self.__nsbu_0 = acados_ocp_json['dims']['nsbu'] + self.__nbxe_0 = acados_ocp_json['dims']['nbxe_0'] + has_custom_hess = not (is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess_0']) and + is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess']) and + is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess_e'])) + elif self.__problem_class == "MOCP": + self.__has_x0 = acados_ocp_json['constraints'][0]['has_x0'] + self.__nsbu_0 = acados_ocp_json['phases_dims'][0]['nsbu'] + self.__nbxe_0 = acados_ocp_json['phases_dims'][0]['nbxe_0'] + has_custom_hess = any([not (is_empty(model['cost_expr_ext_cost_custom_hess_0']) and + is_empty(model['cost_expr_ext_cost_custom_hess']) and + is_empty(model['cost_expr_ext_cost_custom_hess_e'])) for model in acados_ocp_json['model']]) + + self.__uses_exact_hessian = ( + self.__solver_options["hessian_approx"] == 'EXACT' and + self.__solver_options["regularize_method"] == 'NO_REGULARIZE' and + self.__solver_options["levenberg_marquardt"] == 0 and + self.__solver_options["exact_hess_constr"] == 1 and + self.__solver_options["exact_hess_cost"] == 1 and + self.__solver_options["exact_hess_dyn"] == 1 and + self.__solver_options["fixed_hess"] == 0 and + not has_custom_hess + ) acados_lib_path = acados_ocp_json['acados_lib_path'] code_export_directory = acados_ocp_json['code_export_directory'] @@ -372,7 +415,7 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file= getattr(self.shared_lib, f"{self.name}_acados_set_p_global_and_precompute_dependencies").restype = c_int # these do not work for multi phase OCPs - if isinstance(self.acados_ocp, AcadosOcp): + if self.__problem_class == "OCP": getattr(self.shared_lib, f'{self.name}_acados_update_qp_solver_cond_N').argtypes = [c_void_p, c_int] getattr(self.shared_lib, f'{self.name}_acados_update_qp_solver_cond_N').restype = c_int getattr(self.shared_lib, f"{self.name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p] @@ -457,7 +500,7 @@ def setup_qp_matrices_and_factorize(self) -> int: This is only implemented for HPIPM QP solver without condensing. """ - if self.acados_ocp.solver_options.qp_solver != 'PARTIAL_CONDENSING_HPIPM' or self.acados_ocp.solver_options.qp_solver_cond_N != self.acados_ocp.dims.N: + if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM' or self.__solver_options["qp_solver_cond_N"] != self.N: raise Exception('This function is only implemented for HPIPM QP solver without condensing!') self.status = getattr(self.shared_lib, f"{self.name}_acados_setup_qp_matrices_and_factorize")(self.capsule) @@ -509,7 +552,7 @@ def set_new_time_steps(self, new_time_steps): the shooting nodes without changing the number, e.g., to reach a different final time. Both cases do not require a new code export and compilation. """ - if isinstance(self.acados_ocp, AcadosMultiphaseOcp): + if self.__problem_class == "MOCP": raise Exception('This function can only be used for single phase OCPs!') # unlikely but still possible @@ -542,7 +585,7 @@ def set_new_time_steps(self, new_time_steps): # store time_steps, N self.__solver_options['time_steps'] = new_time_steps - self.N = N + self.__N = N self.__solver_options['Tsim'] = self.__solver_options['time_steps'][0] @@ -560,6 +603,9 @@ def update_qp_solver_cond_N(self, qp_solver_cond_N: int): necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically `qp_solver_cond_N < N`. """ + if self.__problem_class == "MOCP": + raise Exception('This function can only be used for single phase OCPs!') + # unlikely but still possible if not self.solver_created: raise Exception('Solver was not yet created!') @@ -603,7 +649,7 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st with_respect_to = "p_global" if with_respect_to == "initial_state": - if not self.acados_ocp.constraints.has_x0: + if not self.__has_x0: raise Exception("OCP does not have an initial state constraint.") nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) @@ -621,7 +667,7 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st lbu = self.get_from_qp_in(0, 'lbu') ubu = self.get_from_qp_in(0, 'ubu') - if not (nbu == nu and np.all(lbu == ubu) and self.acados_ocp.dims.nsbu == 0): + if not (nbu == nu and np.all(lbu == ubu) and self.__nsbu_0 == 0): raise Exception("OCP does not have an equality constraint on the initial control.") lam = self.get(0, 'lam') @@ -649,25 +695,14 @@ def get_optimal_value_gradient(self, with_respect_to: str = "initial_state") -> def _sanity_check_solution_sensitivities(self, parametric=True) -> None: - if not (self.acados_ocp.solver_options.qp_solver == 'FULL_CONDENSING_HPIPM' or - self.acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_HPIPM'): + if not (self.__solver_options["qp_solver"] == 'FULL_CONDENSING_HPIPM' or + self.__solver_options["qp_solver"] == 'PARTIAL_CONDENSING_HPIPM'): raise Exception("Parametric sensitivities are only available with HPIPM as QP solver.") - if not ( - self.acados_ocp.solver_options.hessian_approx == 'EXACT' and - self.acados_ocp.solver_options.regularize_method == 'NO_REGULARIZE' and - self.acados_ocp.solver_options.levenberg_marquardt == 0 and - self.acados_ocp.solver_options.exact_hess_constr == 1 and - self.acados_ocp.solver_options.exact_hess_cost == 1 and - self.acados_ocp.solver_options.exact_hess_dyn == 1 and - self.acados_ocp.solver_options.fixed_hess == 0 and - is_empty(self.acados_ocp.model.cost_expr_ext_cost_custom_hess_0) and - is_empty(self.acados_ocp.model.cost_expr_ext_cost_custom_hess) and - is_empty(self.acados_ocp.model.cost_expr_ext_cost_custom_hess_e) - ): + if not self.__uses_exact_hessian: raise Exception("Parametric sensitivities are only correct if an exact Hessian is used!") - if parametric and not self.acados_ocp.solver_options.with_solution_sens_wrt_params: + if parametric and not self.__solver_options["with_solution_sens_wrt_params"]: raise Exception("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") @@ -728,10 +763,8 @@ def eval_solution_sensitivity(self, sens_sl = [] sens_su = [] - N = self.acados_ocp.solver_options.N_horizon - for s in stages_: - if not isinstance(s, int) or s < 0 or s > N: + if not isinstance(s, int) or s < 0 or s > self.N: raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or list[int] and in [0, N], got stages = {stages_}.") if with_respect_to == "initial_state": @@ -773,7 +806,7 @@ def eval_solution_sensitivity(self, ns = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, s, "s".encode('utf-8')) sens_su.append(np.zeros((ns, ngrad))) - if s < N: + if s < self.N: if return_sens_u: nu = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, s, "u".encode('utf-8')) sens_u.append(np.zeros((nu, ngrad))) @@ -802,7 +835,7 @@ def eval_solution_sensitivity(self, if return_sens_su: sens_su[n][:, k] = self.get(s, "sens_su") - if s < N: + if s < self.N: if return_sens_u: sens_u[n][:, k] = self.get(s, "sens_u") if return_sens_pi: @@ -876,7 +909,6 @@ def eval_adjoint_solution_sensitivity(self, n_seeds = seed_u[0][1].shape[1] if sanity_checks: - N_horizon = self.acados_ocp.solver_options.N_horizon self._sanity_check_solution_sensitivities() nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) nu = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "u".encode('utf-8')) @@ -884,7 +916,7 @@ def eval_adjoint_solution_sensitivity(self, # check seeds for seed, name, dim in [(seed_x, "seed_x", nx), (seed_u, "seed_u", nu)]: for stage, seed_stage in seed: - if not isinstance(stage, int) or stage < 0 or stage > N_horizon: + if not isinstance(stage, int) or stage < 0 or stage > self.N: raise Exception(f"AcadosOcpSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") if not isinstance(seed_stage, np.ndarray): raise Exception(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") @@ -2059,13 +2091,13 @@ def get_from_qp_in(self, stage_: int, field_: str): if field_ not in self.__all_qp_fields | self.__all_relaxed_qp_fields: raise Exception(f"field {field_} not supported.") if field_ in self.__qp_pc_hpipm_fields: - if self.acados_ocp.solver_options.qp_solver != "PARTIAL_CONDENSING_HPIPM" or self.acados_ocp.solver_options.qp_solver_cond_N != self.acados_ocp.solver_options.N_horizon: + if self.__solver_options["qp_solver"] != "PARTIAL_CONDENSING_HPIPM" or self.__solver_options["qp_solver_cond_N"] != self.N: raise Exception(f"field {field_} only works for PARTIAL_CONDENSING_HPIPM QP solver with qp_solver_cond_N == N.") - if field_ in ["P", "K", "p"] and stage_ == 0 and self.acados_ocp.dims.nbxe_0 > 0: + if field_ in ["P", "K", "p"] and stage_ == 0 and self.__nbxe_0 > 0: raise Exception(f"getting field {field_} at stage 0 only works without x0 elimination (see nbxe_0).") - if field_ in self.__qp_pc_fields and not self.acados_ocp.solver_options.qp_solver.startswith("PARTIAL_CONDENSING"): + if field_ in self.__qp_pc_fields and not self.__solver_options["qp_solver"].startswith("PARTIAL_CONDENSING"): raise Exception(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") - if field_ in self.__all_relaxed_qp_fields and not self.acados_ocp.solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP": + if field_ in self.__all_relaxed_qp_fields and not self.__solver_options["nlp_solver_type"] == "SQP_WITH_FEASIBLE_QP": raise Exception(f"field {field_} only works for SQP_WITH_FEASIBLE_QP nlp_solver_type.") field = field_.encode('utf-8') @@ -2117,10 +2149,10 @@ def get_iterate(self, iteration: int) -> AcadosOcpIterate: if iteration < -1 or iteration > nlp_iter: raise Exception("get_iterate: iteration needs to be nonnegative and <= nlp_iter or -1.") - if not self.acados_ocp.solver_options.store_iterates: + if not self.__solver_options["store_iterates"]: raise Exception("get_iterate: the solver option store_iterates needs to be true in order to get iterates.") - if self.acados_ocp.solver_options.nlp_solver_type == "SQP_RTI": + if self.__solver_options["nlp_solver_type"] == "SQP_RTI": raise Exception("get_iterate: SQP_RTI not supported.") # set to nlp_iter if -1 @@ -2134,7 +2166,7 @@ def get_iterate(self, iteration: int) -> AcadosOcpIterate: pi_traj = [] lam_traj = [] - for n in range(self.acados_ocp.solver_options.N_horizon): + for n in range(self.N): 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")) @@ -2143,7 +2175,7 @@ def get_iterate(self, iteration: int) -> AcadosOcpIterate: 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 + n = self.N 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")) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index c135f197b5..5c319aee01 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -174,42 +174,6 @@ cdef class AcadosOcpSolverCython: """ raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature") - # # unlikely but still possible - # if not self.solver_created: - # raise Exception('Solver was not yet created!') - - # ## check if time steps really changed in value - # # get time steps - # cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64) - # assert acados_solver.acados_get_time_steps(self.capsule, self.N, old_time_steps.data) - - # if np.array_equal(old_time_steps, new_time_steps): - # return - - # N = new_time_steps.size - # cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64) - - # # check if recreation of acados is necessary (no need to recreate acados if sizes are identical) - # if len(old_time_steps) == N: - # assert acados_solver.acados_update_time_steps(self.capsule, N, value.data) == 0 - - # else: # recreate the solver with the new time steps - # self.solver_created = False - - # # delete old memory (analog to __del__) - # acados_solver.acados_free(self.capsule) - - # # create solver with new time steps - # assert acados_solver.acados_create_with_discretization(self.capsule, N, value.data) == 0 - - # self.solver_created = True - - # # get pointers solver - # self.__get_pointers_solver() - - # # store time_steps, N - # self.time_steps = new_time_steps - # self.N = N def update_qp_solver_cond_N(self, qp_solver_cond_N: int): @@ -227,25 +191,6 @@ cdef class AcadosOcpSolverCython: """ raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature") - # # unlikely but still possible - # if not self.solver_created: - # raise Exception('Solver was not yet created!') - # if self.N < qp_solver_cond_N: - # raise Exception('Setting qp_solver_cond_N to be larger than N does not work!') - # if self.qp_solver_cond_N != qp_solver_cond_N: - # self.solver_created = False - - # # recreate the solver - # acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N) - - # # store the new value - # self.qp_solver_cond_N = qp_solver_cond_N - # self.solver_created = True - - # # get pointers solver - # self.__get_pointers_solver() - - def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_state") -> np.ndarray: """ diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_mex.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_mex.in.m index eb2dcede00..79b7f27a16 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_mex.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_mex.in.m @@ -31,7 +31,7 @@ function make_mex_{{ name }}() - opts.output_dir = pwd; + output_dir = pwd; % get acados folder acados_folder = getenv('ACADOS_INSTALL_DIR'); @@ -86,12 +86,12 @@ %% octave C flags if is_octave() - if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file') - diary(fullfile(opts.output_dir, 'cflags_octave.txt')); + if ~exist(fullfile(output_dir, 'cflags_octave.txt'), 'file') + diary(fullfile(output_dir, 'cflags_octave.txt')); diary on mkoctfile -p CFLAGS diary off - input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + input_file = fopen(fullfile(output_dir, 'cflags_octave.txt'), 'r'); cflags_tmp = fscanf(input_file, '%[^\n]s'); fclose(input_file); if ~ismac() @@ -99,12 +99,12 @@ else cflags_tmp = [cflags_tmp, ' -std=c99']; end - input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w'); + input_file = fopen(fullfile(output_dir, 'cflags_octave.txt'), 'w'); fprintf(input_file, '%s', cflags_tmp); fclose(input_file); end % read cflags from file - input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r'); + input_file = fopen(fullfile(output_dir, 'cflags_octave.txt'), 'r'); cflags_tmp = fscanf(input_file, '%[^\n]s'); fclose(input_file); setenv('CFLAGS', cflags_tmp); 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 7f73c7d74a..e905a89f21 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 @@ -43,8 +43,10 @@ methods % constructor - function obj = {{ name }}_mex_solver() - make_mex_{{ name }}(); + function obj = {{ name }}_mex_solver(solver_creation_opts) + if solver_creation_opts.compile_mex_wrapper + make_mex_{{ name }}(); + end obj.C_ocp = acados_mex_create_{{ name }}(); % to have path to destructor when changing directory addpath('.') diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 0cff428700..8b5b4ca707 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -165,7 +165,7 @@ def check_casadi_version_supports_p_global(): raise Exception("CasADi version does not support extract_parametric or cse functions.\nNeeds nightly-se2 release or later, see: https://github.com/casadi/casadi/releases/tag/nightly-se2") -def get_simulink_default_opts(): +def get_simulink_default_opts() -> dict: python_interface_path = get_python_interface_path() abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json') with open(abs_path , 'r') as f: @@ -202,7 +202,7 @@ def is_empty(x): return True if np.prod(x.shape) == 0 else False elif x is None: return True - elif isinstance(x, (set, list)): + elif isinstance(x, (set, list, str)): return True if len(x) == 0 else False elif isinstance(x, (float, int)): return False From 58fd2ce10b1e16aba85b5ddfdad0d2c986f13fd2 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 24 Mar 2025 15:04:53 +0100 Subject: [PATCH 006/164] Python: Implement reformulation as external cost with Gauss-Newton for LS/NLS (#1476) This is in particularly useful when computing parametric solution sensitivities with a two-solver approach. It allows to use the same parameterization of the problem in both solvers, but to use a Gauss-Newton Hessian in the "nominal" solver. Note: for linear LS cost, there is nothing to do, as the exact Hessian and its GN approximation coincide. --- .../ocp/ocp_example_cost_formulations.py | 25 +++++++---- .../acados_template/acados_ocp.py | 42 +++++++++++++++---- 2 files changed, 49 insertions(+), 18 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py index 608c02da5d..305c0e9886 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py @@ -39,7 +39,7 @@ from utils import plot_pendulum import casadi as ca -COST_VERSIONS = ['LS', 'EXTERNAL', 'EXTERNAL_Z', 'NLS', 'NLS_Z', 'LS_Z', 'CONL', 'CONL_Z', 'AUTO'] +COST_VERSIONS = ['LS', 'EXTERNAL', 'EXTERNAL_Z', 'NLS', 'NLS_TO_EXTERNAL', 'NLS_Z', 'LS_Z', 'CONL', 'CONL_Z', 'AUTO'] HESSIAN_APPROXIMATION = 'GAUSS_NEWTON' # 'GAUSS_NEWTON N = 20 T_HORIZON = 1.0 @@ -77,6 +77,13 @@ def formulate_ocp(cost_version: str) -> AcadosOcp: cost_W = scipy.linalg.block_diag(Q_mat, R_mat) + if cost_version in ['LS', 'NLS', 'NLS_TO_EXTERNAL', 'NLS_Z', 'LS_Z', 'CONL', 'CONL_Z']: + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) + if cost_version in ['LS', 'NLS', 'NLS_TO_EXTERNAL', 'NLS_Z', 'LS_Z']: + ocp.cost.W_e = Q_mat + ocp.cost.W = cost_W + if cost_version in ['CONL', 'CONL_Z', 'EXTERNAL', 'EXTERNAL_Z', 'AUTO']: cost_W = ca.sparsify(ca.DM(cost_W)) Q_mat = ca.sparsify(ca.DM(Q_mat)) @@ -185,16 +192,16 @@ def formulate_ocp(cost_version: str) -> AcadosOcp: ocp.model.cost_expr_ext_cost = .5*ca.vertcat(x, u).T @ cost_W @ ca.vertcat(x, u) ocp.model.cost_expr_ext_cost_e = .5*x.T @ Q_mat @ x + elif cost_version == 'NLS_TO_EXTERNAL': + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + ocp.model.cost_y_expr = ca.vertcat(x, u) + ocp.model.cost_y_expr_e = x + ocp.translate_cost_to_external_cost(cost_hessian='GAUSS_NEWTON') else: raise Exception('Unknown cost_version.') - if cost_version in ['LS', 'NLS', 'NLS_Z', 'LS_Z', 'CONL', 'CONL_Z']: - ocp.cost.yref = np.zeros((ny, )) - ocp.cost.yref_e = np.zeros((ny_e, )) - if cost_version in ['LS', 'NLS', 'NLS_Z', 'LS_Z']: - ocp.cost.W_e = Q_mat - ocp.cost.W = cost_W - # set constraints ocp.constraints.lbu = np.array([-FMAX]) ocp.constraints.ubu = np.array([+FMAX]) @@ -263,7 +270,7 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', refor ocp.translate_cost_to_external_cost(p=p, p_values=p_values, yref=yref, yref_e=yref_e) # create solver - ocp_solver = AcadosOcpSolver(ocp) + ocp_solver = AcadosOcpSolver(ocp, verbose=False) # NOTE: hessian is wrt [u,x] if ext_cost_use_num_hess and cost_version in ['EXTERNAL', 'EXTERNAL_Z']: diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 3fc09c2a68..26cfd40f27 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1254,17 +1254,27 @@ def translate_cost_to_external_cost(self, W_0: Optional[Union[ca.SX, ca.MX]] = None, W: Optional[Union[ca.SX, ca.MX]] = None, W_e: Optional[Union[ca.SX, ca.MX]] = None, + cost_hessian: str = 'EXACT', ): """ Translates cost to EXTERNAL cost and optionally provide parametrization of references and weighting matrices. - p: Optional CasADi symbolics with additional stagewise parameters which are used to define yref_0, yref, yref_e, W_0, W, W_e. Will be appended to model.p. - p_values: numpy array with the same shape as p providing initial parameter values. - p_global: Optional CasADi symbolics with additional global parameters which are used to define yref_0, yref, yref_e, W_0, W, W_e. Will be appended to model.p_global. - p_global_values: numpy array with the same shape as p_global providing initial global parameter values. - W_0, W, W_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, ny_0), (ny, ny), (ny_e, ny_e). - yref_0, yref, yref_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, 1), (ny, 1), (ny_e, 1). + + :param p: Optional CasADi symbolics with additional stagewise parameters which are used to define yref_0, yref, yref_e, W_0, W, W_e. Will be appended to model.p. + :param p_values: numpy array with the same shape as p providing initial parameter values. + :param p_global: Optional CasADi symbolics with additional global parameters which are used to define yref_0, yref, yref_e, W_0, W, W_e. Will be appended to model.p_global. + :param p_global_values: numpy array with the same shape as p_global providing initial global parameter values. + :param W_0, W, W_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, ny_0), (ny, ny), (ny_e, ny_e). + :param yref_0, yref, yref_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, 1), (ny, 1), (ny_e, 1). + cost_hessian: 'EXACT' or 'GAUSS_NEWTON', determines how the cost hessian is computed. """ + if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: + raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + if cost_hessian == 'GAUSS_NEWTON': + for attr_name, cost_type in ([('cost_type', self.cost.cost_type), ('cost_type_0', self.cost.cost_type_0), ('cost_type_e', self.cost.cost_type_e)]): + if cost_type in ['EXTERNAL', 'AUTO', 'CONVEX_OVER_NONLINEAR']: + raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got {attr_name} = {cost_type}.") + casadi_symbolics_type = type(self.model.x) # check p, p_values and append @@ -1360,6 +1370,8 @@ def translate_cost_to_external_cost(self, self.model.cost_expr_ext_cost_0 = \ self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_0, yref_0, W_0) + if cost_hessian == 'GAUSS_NEWTON': + self.model.cost_expr_ext_cost_custom_hess_0 = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_0, yref_0, W_0, self.model.x, self.model.u, self.model.z) elif self.cost.cost_type_0 == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_0 = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0, @@ -1371,8 +1383,10 @@ def translate_cost_to_external_cost(self, self.cost.Vx, self.cost.Vu, self.cost.Vz, yref, W) elif self.cost.cost_type == "NONLINEAR_LS": - self.model.cost_expr_ext_cost = \ - self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, W) + self.model.cost_expr_ext_cost = \ + self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, W) + if cost_hessian == 'GAUSS_NEWTON': + self.model.cost_expr_ext_cost_custom_hess = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr, yref, W, self.model.x, self.model.u, self.model.z) elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr, @@ -1386,6 +1400,8 @@ def translate_cost_to_external_cost(self, elif self.cost.cost_type_e == "NONLINEAR_LS": self.model.cost_expr_ext_cost_e = \ self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_e, yref_e, W_e) + if cost_hessian == 'GAUSS_NEWTON': + self.model.cost_expr_ext_cost_custom_hess_e = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_e, yref_e, W_e, self.model.x, [], self.model.z) elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_e = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e, @@ -1414,6 +1430,14 @@ def __translate_nls_cost_to_external_cost(y_expr, yref, W): res = y_expr - yref return 0.5 * (res.T @ W @ res) + @staticmethod + def __get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, x, u, z): + res = y_expr - yref + ux = ca.vertcat(u, x) + inner_jac = ca.jacobian(res, ux) + gn_hess = inner_jac.T @ W @ inner_jac + return gn_hess + @staticmethod def __translate_conl_cost_to_external_cost(r, psi, y_expr, yref): return ca.substitute(psi, r, y_expr - yref) @@ -1728,7 +1752,7 @@ def augment_with_t0_param(self) -> None: self.parameter_values = np.append(self.parameter_values, [0.0]) self.p_global_values = np.append(self.p_global_values, [0.0]) return - + def detect_cost_type(self, model: AcadosModel, cost: AcadosOcpCost, dims: AcadosOcpDims, stage_type: str) -> None: """ From 1a32c08545e1ec7f07a27923370f974f0afad956 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 24 Mar 2025 17:48:42 +0100 Subject: [PATCH 007/164] Python: checks on external checks (#1477) Breaking: `cost_expr_ext_cost_custom_hess*` can not be float or int anymore in Python, but should be a CasADi MX/SX/DM --- examples/acados_python/tests/armijo_test.py | 8 ++--- .../acados_template/acados_ocp.py | 32 +++++++++++++++++++ .../acados_template/acados_template/utils.py | 1 + 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/examples/acados_python/tests/armijo_test.py b/examples/acados_python/tests/armijo_test.py index ae124ae9a7..ffe95a8d2e 100644 --- a/examples/acados_python/tests/armijo_test.py +++ b/examples/acados_python/tests/armijo_test.py @@ -30,7 +30,7 @@ from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel import numpy as np -from casadi import * +import casadi as ca from itertools import product # tests different globalization settings on simple test problem @@ -70,12 +70,12 @@ def solve_armijo_problem_with_setting(setting): # set model model = AcadosModel() - x = SX.sym('x') + x = ca.SX.sym('x') # dynamics: identity model.disc_dyn_expr = x model.x = x - model.u = SX.sym('u', 0, 0) # [] / None doesnt work + model.u = ca.SX.sym('u', 0, 0) # [] / None doesnt work model.p = [] model.name = f'armijo_problem' ocp.model = model @@ -89,7 +89,7 @@ def solve_armijo_problem_with_setting(setting): # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = x @ x - ocp.model.cost_expr_ext_cost_custom_hess_e = 1.0 # 2.0 is the actual hessian + ocp.model.cost_expr_ext_cost_custom_hess_e = ca.DM(1.0) # 2.0 is the actual hessian # constarints ocp.constraints.idxbx = np.array([0]) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 26cfd40f27..b5f90b01d8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -241,6 +241,17 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + elif cost.cost_type_0 == 'EXTERNAL': + if isinstance(model.cost_expr_ext_cost_0, (float, int)): + model.cost_expr_ext_cost_0 = ca.DM(model.cost_expr_ext_cost_0) + if not isinstance(model.cost_expr_ext_cost_0, (ca.MX, ca.SX, ca.DM)): + raise Exception('cost_expr_ext_cost_0 should be casadi expression.') + if not casadi_length(model.cost_expr_ext_cost_0) == 1: + raise Exception('cost_expr_ext_cost_0 should be scalar-valued.') + if not is_empty(model.cost_expr_ext_cost_custom_hess_0): + if model.cost_expr_ext_cost_custom_hess_0.shape != (dims.nx+dims.nu, dims.nx+dims.nu): + raise Exception('cost_expr_ext_cost_custom_hess_0 should have shape (nx+nu, nx+nu).') + # GN check gn_warning_0 = (cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) gn_warning_path = (cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) @@ -307,6 +318,16 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + elif cost.cost_type == 'EXTERNAL': + if isinstance(model.cost_expr_ext_cost, (float, int)): + model.cost_expr_ext_cost = ca.DM(model.cost_expr_ext_cost) + if not isinstance(model.cost_expr_ext_cost, (ca.MX, ca.SX, ca.DM)): + raise Exception('cost_expr_ext_cost should be casadi expression.') + if not casadi_length(model.cost_expr_ext_cost) == 1: + raise Exception('cost_expr_ext_cost should be scalar-valued.') + if not is_empty(model.cost_expr_ext_cost_custom_hess): + if model.cost_expr_ext_cost_custom_hess.shape != (dims.nx+dims.nu, dims.nx+dims.nu): + raise Exception('cost_expr_ext_cost_custom_hess should have shape (nx+nu, nx+nu).') # terminal if cost.cost_type_e == 'AUTO': self.detect_cost_type(model, cost, dims, "terminal") @@ -348,6 +369,17 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + elif cost.cost_type_e == 'EXTERNAL': + if isinstance(model.cost_expr_ext_cost_e, (float, int)): + model.cost_expr_ext_cost_e = ca.DM(model.cost_expr_ext_cost_e) + if not isinstance(model.cost_expr_ext_cost_e, (ca.MX, ca.SX, ca.DM)): + raise Exception(f'cost_expr_ext_cost_e should be casadi expression, got {model.cost_expr_ext_cost_e}.') + if not casadi_length(model.cost_expr_ext_cost_e) == 1: + raise Exception('cost_expr_ext_cost_e should be scalar-valued.') + if not is_empty(model.cost_expr_ext_cost_custom_hess_e): + if model.cost_expr_ext_cost_custom_hess_e.shape != (dims.nx, dims.nx): + raise Exception('cost_expr_ext_cost_custom_hess_e should have shape (nx, nx).') + # cost integration supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] if opts.cost_discretization == 'INTEGRATOR' and \ diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 8b5b4ca707..12b540cbe2 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -65,6 +65,7 @@ '3.6.5', '3.6.6', '3.6.7', + '3.6.7+', ) TERA_VERSION = "0.0.34" From f901d03c0185fa89f02be5cbde0b67f804fb0f33 Mon Sep 17 00:00:00 2001 From: dirkpr <31849867+dirkpr@users.noreply.github.com> Date: Wed, 26 Mar 2025 17:00:12 +0100 Subject: [PATCH 008/164] Symbolic cost parameters (#1479) ## Enhancement: Symbolic Expressions for Cost Matrices in acados Python Interface This PR extends the Python interface to acados by enabling symbolic expressions for the following cost matrices and reference vectors: - `W_0` (initial stage cost weight matrix) - `W` (stage cost weight matrix) - `W_e` (terminal cost weight matrix) - `yref_0` (initial stage reference vector) - `yref` (stage reference vector) - `yref_e` (terminal reference vector) ### Benefits This enhancement allows for parameterized least-squares cost functions. The least-squares cost is translated to an external cost, and parameters can be dynamically modified at runtime. ### Use Case This is particularly valuable for advanced two-solver implementations where: 1. A forward solver leverages a Gauss-Newton Hessian approximation 2. A backward solver employs an exact Hessian for exact sensitivity computation --------- Co-authored-by: Jonathan Frey --- .github/workflows/full_build.yml | 2 ++ .../ocp/ocp_example_cost_formulations.py | 31 +++++++++++++++++++ interfaces/CMakeLists.txt | 4 --- .../acados_template/acados_ocp.py | 24 ++++++++++++++ .../acados_template/acados_ocp_cost.py | 14 ++++----- .../acados_template/acados_template/utils.py | 20 ++++++++++++ 6 files changed, 84 insertions(+), 11 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 9c10c5b9ef..5668734052 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -202,6 +202,8 @@ jobs: python value_gradient_example_linear.py python batch_adjoint_solution_sensitivity_example.py python non_ocp_example.py + cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + python ocp_example_cost_formulations.py - name: Python Furuta pendulum timeout test diff --git a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py index 305c0e9886..074c41b425 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py @@ -38,6 +38,7 @@ import scipy.linalg from utils import plot_pendulum import casadi as ca +from casadi.tools import entry, struct_symSX COST_VERSIONS = ['LS', 'EXTERNAL', 'EXTERNAL_Z', 'NLS', 'NLS_TO_EXTERNAL', 'NLS_Z', 'LS_Z', 'CONL', 'CONL_Z', 'AUTO'] HESSIAN_APPROXIMATION = 'GAUSS_NEWTON' # 'GAUSS_NEWTON @@ -199,6 +200,35 @@ def formulate_ocp(cost_version: str) -> AcadosOcp: ocp.model.cost_y_expr = ca.vertcat(x, u) ocp.model.cost_y_expr_e = x ocp.translate_cost_to_external_cost(cost_hessian='GAUSS_NEWTON') + elif cost_version == 'NLS_TO_EXTERNAL_P_GLOBAL': + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + p_global = struct_symSX([ + entry('W', shape=(ny, ny)), + entry('yref', shape=(ny, )), + entry('W_e', shape=(ny_e, ny_e)), + entry('yref_e', shape=(ny_e, )) + ]) + ocp.model.p_global = p_global.cat + + ocp.cost.W = p_global['W'] + ocp.cost.yref = p_global['yref'] + ocp.cost.W_e = p_global['W_e'] + ocp.cost.yref_e = p_global['yref_e'] + + ocp.model.cost_y_expr = ca.vertcat(x, u) + ocp.model.cost_y_expr_e = x + + ocp.translate_cost_to_external_cost(cost_hessian='GAUSS_NEWTON') + + p_global_values = p_global(0) + p_global_values['W'] = cost_W + p_global_values['yref'] = np.zeros((ny, )) + p_global_values['W_e'] = Q_mat + p_global_values['yref_e'] = np.zeros((ny_e, )) + + ocp.p_global_values = p_global_values.cat.full().flatten() else: raise Exception('Unknown cost_version.') @@ -345,6 +375,7 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', refor print(f"cost version: {cost_version}, formulation type: {formulation_type}") main(cost_version=cost_version, formulation_type=formulation_type, plot=False) + for cost_version in ["NLS_TO_EXTERNAL_P_GLOBAL"]: print(f"cost version: {cost_version} reformulated as EXTERNAL cost") main(cost_version=cost_version, formulation_type='ocp', plot=False, reformulate_to_external=True) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 859c987e17..bd17c3a10f 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -323,10 +323,6 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/mhe python closed_loop_mhe_ocp.py) - add_test(NAME python_pendulum_ocp_cost_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python ocp_example_cost_formulations.py) - add_test(NAME python_custom_update_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/custom_update python example_custom_rti_loop.py) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index b5f90b01d8..6780a4c18b 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -198,6 +198,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: if cost.cost_type_0 == 'AUTO': self.detect_cost_type(model, cost, dims, "initial") + if cost.cost_type_0 in ['LINEAR_LS', 'NONLINEAR_LS']: + if isinstance(cost.yref_0, (ca.SX, ca.MX, ca.DM)): + raise Exception("yref_0 should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + + if isinstance(cost.W_0, (ca.SX, ca.MX, ca.DM)): + raise Exception("W_0 should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + if cost.cost_type_0 == 'LINEAR_LS': check_if_square(cost.W_0, 'W_0') ny_0 = cost.W_0.shape[0] @@ -275,6 +282,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: if cost.cost_type == 'AUTO': self.detect_cost_type(model, cost, dims, "path") + if cost.cost_type in ['LINEAR_LS', 'NONLINEAR_LS']: + if isinstance(cost.yref, (ca.SX, ca.MX, ca.DM)): + raise Exception("yref should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + + if isinstance(cost.W, (ca.SX, ca.MX, ca.DM)): + raise Exception("W should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + if cost.cost_type == 'LINEAR_LS': ny = cost.W.shape[0] check_if_square(cost.W, 'W') @@ -332,6 +346,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: if cost.cost_type_e == 'AUTO': self.detect_cost_type(model, cost, dims, "terminal") + if cost.cost_type_e in ['LINEAR_LS', 'NONLINEAR_LS']: + if isinstance(cost.yref_e, (ca.SX, ca.MX, ca.DM)): + raise Exception("yref_e should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + + if isinstance(cost.W_e, (ca.SX, ca.MX, ca.DM)): + raise Exception("W_e should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") + if cost.cost_type_e == 'LINEAR_LS': ny_e = cost.W_e.shape[0] check_if_square(cost.W_e, 'W_e') @@ -1404,6 +1425,7 @@ def translate_cost_to_external_cost(self, if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess_0 = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_0, yref_0, W_0, self.model.x, self.model.u, self.model.z) + elif self.cost.cost_type_0 == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_0 = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0, @@ -1419,6 +1441,7 @@ def translate_cost_to_external_cost(self, self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, W) if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr, yref, W, self.model.x, self.model.u, self.model.z) + elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr, @@ -1434,6 +1457,7 @@ def translate_cost_to_external_cost(self, self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_e, yref_e, W_e) if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess_e = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_e, yref_e, W_e, self.model.x, [], self.model.z) + elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_e = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e, diff --git a/interfaces/acados_template/acados_template/acados_ocp_cost.py b/interfaces/acados_template/acados_template/acados_ocp_cost.py index 625272c014..b085ce5efe 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_cost.py +++ b/interfaces/acados_template/acados_template/acados_ocp_cost.py @@ -30,7 +30,7 @@ # import numpy as np -from .utils import check_if_nparray_and_flatten, check_if_2d_nparray +from .utils import check_if_nparray_and_flatten, check_if_2d_nparray, check_if_2d_nparray_or_casadi_symbolic, check_if_nparray_or_casadi_symbolic_and_flatten class AcadosOcpCost: r""" @@ -169,12 +169,12 @@ def cost_ext_fun_type_0(self): @yref_0.setter def yref_0(self, yref_0): - yref_0 = check_if_nparray_and_flatten(yref_0, "yref_0") + yref_0 = check_if_nparray_or_casadi_symbolic_and_flatten(yref_0, "yref_0") self.__yref_0 = yref_0 @W_0.setter def W_0(self, W_0): - check_if_2d_nparray(W_0, "W_0") + check_if_2d_nparray_or_casadi_symbolic(W_0, "W_0") self.__W_0 = W_0 @Vx_0.setter @@ -298,7 +298,7 @@ def cost_type_0(self, cost_type_0): @W.setter def W(self, W): - check_if_2d_nparray(W, "W") + check_if_2d_nparray_or_casadi_symbolic(W, "W") self.__W = W @@ -319,7 +319,7 @@ def Vz(self, Vz): @yref.setter def yref(self, yref): - yref = check_if_nparray_and_flatten(yref, "yref") + yref = check_if_nparray_or_casadi_symbolic_and_flatten(yref, "yref") self.__yref = yref @Zl.setter @@ -463,7 +463,7 @@ def cost_type_e(self, cost_type_e): @W_e.setter def W_e(self, W_e): - check_if_2d_nparray(W_e, "W_e") + check_if_2d_nparray_or_casadi_symbolic(W_e, "W_e") self.__W_e = W_e @Vx_e.setter @@ -473,7 +473,7 @@ def Vx_e(self, Vx_e): @yref_e.setter def yref_e(self, yref_e): - yref_e = check_if_nparray_and_flatten(yref_e, "yref_e") + yref_e = check_if_nparray_or_casadi_symbolic_and_flatten(yref_e, "yref_e") self.__yref_e = yref_e @Zl_e.setter diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 12b540cbe2..68888972ab 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -43,6 +43,7 @@ from ctypes import CDLL as DllLoader import numpy as np from casadi import DM, MX, SX, CasadiMeta, Function +import casadi as ca from contextlib import contextmanager @@ -411,6 +412,15 @@ def check_if_nparray_and_flatten(val, name) -> np.ndarray: raise Exception(f"{name} must be a numpy array, got {type(val)}") return val.reshape(-1) +def check_if_nparray_or_casadi_symbolic_and_flatten(val, name) -> np.ndarray: + if not isinstance(val, (np.ndarray, SX, MX)): + raise Exception(f"{name} must be array of type np.ndarray, casadi.SX, or casadi.MX, got {type(val)}") + + if isinstance(val, (SX, MX)): + return ca.reshape(val, val.numel(), 1) + else: + return val.reshape(-1) + def check_if_2d_nparray(val, name) -> None: if not isinstance(val, np.ndarray): @@ -419,6 +429,16 @@ def check_if_2d_nparray(val, name) -> None: raise Exception(f"{name} must be a 2D numpy array, got shape {val.shape}") return + +def check_if_2d_nparray_or_casadi_symbolic(val, name) -> None: + if isinstance(val, (SX, MX, DM)): + return + if not isinstance(val, np.ndarray): + raise Exception(f"{name} must be a array of type np.ndarray, casadi.SX, or casadi.MX, got {type(val)}") + if val.ndim != 2: + raise Exception(f"{name} must be a 2D array of type np.ndarray, casadi.SX, or casadi.MX, got shape {val.shape}") + + def print_J_to_idx_note(): print("NOTE: J* matrix is converted to zero based vector idx* vector, which is returned here.") From a1a39341441dce9a0aa7b7fbd2efde328e5e0b06 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 26 Mar 2025 17:25:52 +0100 Subject: [PATCH 009/164] Fix `get_path_cost_expression` for `CONVEX_OVER_NONLINEAR` (#1480) - fix get_path_cost_expression for `CONVEX_OVER_NONLINEAR`, broken in #1402 - fix also for terminal cost - removed comments --- .../acados_template/acados_template/mpc_utils.py | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/interfaces/acados_template/acados_template/mpc_utils.py b/interfaces/acados_template/acados_template/mpc_utils.py index 64d2b8f99a..1798fc3f8d 100644 --- a/interfaces/acados_template/acados_template/mpc_utils.py +++ b/interfaces/acados_template/acados_template/mpc_utils.py @@ -391,9 +391,6 @@ def evaluate_ocp_cost( def get_path_cost_expression(ocp: AcadosOcp): - # multiply path costs with td, nonuniform grid - # multiply with cost scaling - # cost scaling ueberschreibt tds, falls gesetzt model = ocp.model if ocp.cost.cost_type == "LINEAR_LS": y = ocp.cost.Vx @ model.x + ocp.cost.Vu @ model.u @@ -411,10 +408,8 @@ def get_path_cost_expression(ocp: AcadosOcp): cost_dot = model.cost_expr_ext_cost elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": - raise NotImplementedError( - "get_terminal_cost_expression: not implemented for CONVEX_OVER_NONLINEAR.") - #cost_dot = ca.substitute( - # model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) + cost_dot = ca.substitute( + model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) else: raise Exception("create_model_with_cost_state: Unknown cost type.") @@ -436,10 +431,8 @@ def get_terminal_cost_expression(ocp: AcadosOcp): cost_dot = model.cost_expr_ext_cost_e elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": - raise NotImplementedError( - "get_terminal_cost_expression: not implemented for CONVEX_OVER_NONLINEAR.") - #cost_dot = ca.substitute( - # model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) + cost_dot = ca.substitute( + model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) else: raise Exception("create_model_with_cost_state: Unknown terminal cost type.") From 1dfbfffa0633762e1706bc8fb979bc675689f73f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 27 Mar 2025 11:22:10 +0100 Subject: [PATCH 010/164] MATLAB: migrate example to new interface, add extensive timing evaluation, improve checks (#1482) --- .../pendulum_on_cart_model/example_ocp_reg.m | 274 ++++++------------ interfaces/acados_matlab_octave/AcadosOcp.m | 34 +++ .../acados_matlab_octave/AcadosSimSolver.m | 2 +- ...up_AcadosOcp_from_legacy_ocp_description.m | 9 +- .../acados_template/acados_ocp_options.py | 6 +- 5 files changed, 123 insertions(+), 202 deletions(-) diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_reg.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_reg.m index ebc7b7473c..4d3d52a4b2 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_reg.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_reg.m @@ -27,90 +27,40 @@ % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE.; -% -% NOTE: `acados` currently supports both an old MATLAB/Octave interface (< v0.4.0) -% as well as a new interface (>= v0.4.0). +clear all -% THIS EXAMPLE still uses the OLD interface. If you are new to `acados` please start -% with the examples that have been ported to the new interface already. -% see https://github.com/acados/acados/issues/1196#issuecomment-2311822122) +check_acados_requirements() -clear all - -% check that env.sh has been run -env_run = getenv('ENV_RUN'); -if (~strcmp(env_run, 'true')) - error('env.sh has not been sourced! Before executing this example, run: source env.sh'); -end +%% model dynamics +model = get_pendulum_on_cart_model(); +nx = length(model.x); % state size +nu = length(model.u); % input size -%% arguments -compile_interface = 'true'; %'auto'; -gnsf_detect_struct = 'true'; +%% OCP formulation object +ocp = AcadosOcp(); +ocp.model = model; % discretization N = 100; h = 0.01; -nlp_solver = 'sqp'; -%nlp_solver = 'sqp_rti'; nlp_solver_step_length = 1.0; -%nlp_solver_exact_hessian = 'false'; nlp_solver_exact_hessian = 'true'; -%regularize_method = 'no_regularize'; -%regularize_method = 'project'; -regularize_method = 'project_reduc_hess'; -%regularize_method = 'mirror'; -%regularize_method = 'convexify'; -nlp_solver_max_iter = 20; %100; +regularize_method = 'PROJECT'; % PROJECT_REDUC_HESS, PROJECT, GERSHGORIN_LEVENBERG_MARQUARDT +nlp_solver_max_iter = 100; %100; nlp_solver_tol_stat = 1e-8; nlp_solver_tol_eq = 1e-8; nlp_solver_tol_ineq = 1e-8; nlp_solver_tol_comp = 1e-8; -nlp_solver_ext_qp_res = 1; -%qp_solver = 'partial_condensing_hpipm'; -qp_solver = 'full_condensing_hpipm'; -%qp_solver = 'full_condensing_qpoases'; -qp_solver_cond_N = 5; -qp_solver_cond_ric_alg = 0; -qp_solver_ric_alg = 0; -qp_solver_warm_start = 0; -qp_solver_max_iter = 100; -%sim_method = 'erk'; -sim_method = 'irk'; -%sim_method = 'irk_gnsf'; -sim_method_num_stages = 4; -sim_method_num_steps = 3; -cost_type = 'linear_ls'; -%cost_type = 'ext_cost'; -model_name = 'ocp_pendulum'; - -%% create model entries -model = pendulum_on_cart_model(); +model_name = 'ocp_pendulum'; % dims T = N*h; % horizon length time -nx = model.nx; -nu = model.nu; -ny = nu+nx; % number of outputs in lagrange term -ny_e = nx; % number of outputs in mayer term -if 0 - nbx = 0; - nbu = nu; - ng = 0; - ng_e = 0; - nh = 0; - nh_e = 0; -else - nbx = 0; - nbu = 0; - ng = 0; - ng_e = 0; - nh = nu; - nh_e = 0; -end +ny = nu+nx; % number of outputs in path cost +ny_e = nx; % number of outputs in terminal cost term % cost Vu = zeros(ny, nu); for ii=1:nu Vu(ii,ii)=1.0; end % input-to-output matrix in lagrange term @@ -122,147 +72,79 @@ %for ii=nu+1:nu+nx/2 W(ii,ii)=1e1; end for ii=nu+nx/2+1:nu+nx W(ii,ii)=1e-2; end W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in mayer term -yr = zeros(ny, 1); % output reference in lagrange term -yr_e = zeros(ny_e, 1); % output reference in mayer term - -%yr(2:end) = [0; pi; 0; 0]; -%yr_e = [0; pi; 0; 0]; +yref = zeros(ny, 1); % output reference in lagrange term +yref_e = zeros(ny_e, 1); % output reference in mayer term + +ocp.cost.cost_type = 'LINEAR_LS'; +ocp.cost.Vx = Vx; +ocp.cost.Vu = Vu; +ocp.cost.Vx_e = Vx_e; +ocp.cost.W = W; +ocp.cost.W_e = W_e; +ocp.cost.yref = yref; +ocp.cost.yref_e = yref_e; -% constraints -x0 = [0; pi; 0; 0]; -%x0 = [0; pi; 0; 1]; -%Jbx = zeros(nbx, nx); for ii=1:nbx Jbx(ii,ii)=1.0; end -%lbx = -4*ones(nbx, 1); -%ubx = 4*ones(nbx, 1); -Jbu = zeros(nbu, nu); for ii=1:nbu Jbu(ii,ii)=1.0; end +% lbu = -80*ones(nu, 1); ubu = 80*ones(nu, 1); - -%% acados ocp model -ocp_model = acados_ocp_model(); -ocp_model.set('name', model_name); -ocp_model.set('T', T); - -% symbolics -ocp_model.set('sym_x', model.sym_x); -if isfield(model, 'sym_u') - ocp_model.set('sym_u', model.sym_u); -end -if isfield(model, 'sym_xdot') - ocp_model.set('sym_xdot', model.sym_xdot); -end -% cost -ocp_model.set('cost_type', cost_type); -ocp_model.set('cost_type_e', cost_type); -%if (strcmp(cost_type, 'linear_ls')) - ocp_model.set('cost_Vu', Vu); - ocp_model.set('cost_Vx', Vx); - ocp_model.set('cost_Vx_e', Vx_e); - ocp_model.set('cost_W', W); - ocp_model.set('cost_W_e', W_e); - ocp_model.set('cost_y_ref', yr); - ocp_model.set('cost_y_ref_e', yr_e); -%else % if (strcmp(cost_type, 'ext_cost')) -% ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); -% ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); -%end -% dynamics -if (strcmp(sim_method, 'erk')) - ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); -else % irk irk_gnsf - ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); -end % constraints -ocp_model.set('constr_x0', x0); -if (ng>0) - ocp_model.set('constr_C', C); - ocp_model.set('constr_D', D); - ocp_model.set('constr_lg', lg); - ocp_model.set('constr_ug', ug); - ocp_model.set('constr_C_e', C_e); - ocp_model.set('constr_lg_e', lg_e); - ocp_model.set('constr_ug_e', ug_e); -elseif (nh>0) - ocp_model.set('constr_expr_h_0', model.constr_expr_h); - ocp_model.set('constr_lh_0', lbu); - ocp_model.set('constr_uh_0', ubu); - ocp_model.set('constr_expr_h', model.constr_expr_h); - ocp_model.set('constr_lh', lbu); - ocp_model.set('constr_uh', ubu); -% ocp_model.set('constr_expr_h_e', model.expr_h_e); -% ocp_model.set('constr_lh_e', lh_e); -% ocp_model.set('constr_uh_e', uh_e); -else -% ocp_model.set('constr_Jbx', Jbx); -% ocp_model.set('constr_lbx', lbx); -% ocp_model.set('constr_ubx', ubx); - ocp_model.set('constr_Jbu', Jbu); - ocp_model.set('constr_lbu', lbu); - ocp_model.set('constr_ubu', ubu); -end - -%% acados ocp opts -ocp_opts = acados_ocp_opts(); -ocp_opts.set('compile_interface', compile_interface); -ocp_opts.set('param_scheme_N', N); -ocp_opts.set('nlp_solver', nlp_solver); -ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); -ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); -ocp_opts.set('nlp_solver_step_length', nlp_solver_step_length); -if (strcmp(nlp_solver, 'sqp')) - ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); - ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat); - ocp_opts.set('nlp_solver_tol_eq', nlp_solver_tol_eq); - ocp_opts.set('nlp_solver_tol_ineq', nlp_solver_tol_ineq); - ocp_opts.set('nlp_solver_tol_comp', nlp_solver_tol_comp); -end -ocp_opts.set('qp_solver', qp_solver); -if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); - ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); -end -ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); -ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); -ocp_opts.set('qp_solver_iter_max', qp_solver_max_iter); -ocp_opts.set('sim_method', sim_method); -ocp_opts.set('sim_method_num_stages', sim_method_num_stages); -ocp_opts.set('sim_method_num_steps', sim_method_num_steps); -if (strcmp(sim_method, 'irk_gnsf')) - ocp_opts.set('gnsf_detect_struct', gnsf_detect_struct); -end - - -%% acados ocp -% create ocp -ocp_solver = acados_ocp(ocp_model, ocp_opts); +x0 = [0; pi; 0; 0]; +constr_expr_h = model.u; + +% formulate as nonlinear constraint +ocp.constraints.x0 = x0; +ocp.model.con_h_expr_0 = constr_expr_h; +ocp.constraints.lh_0 = lbu; +ocp.constraints.uh_0 = ubu; +ocp.model.con_h_expr = constr_expr_h; +ocp.constraints.lh = lbu; +ocp.constraints.uh = ubu; + +% options +ocp.solver_options.tf = T; +ocp.solver_options.N_horizon = N; +ocp.solver_options.nlp_solver_type = 'SQP'; +ocp.solver_options.hessian_approx = 'EXACT'; +ocp.solver_options.integrator_type = 'IRK'; + +ocp.solver_options.regularize_method = regularize_method; +ocp.solver_options.nlp_solver_ext_qp_res = 1; +ocp.solver_options.nlp_solver_step_length = nlp_solver_step_length; +ocp.solver_options.nlp_solver_max_iter = nlp_solver_max_iter; +ocp.solver_options.nlp_solver_tol_stat = nlp_solver_tol_stat; +ocp.solver_options.nlp_solver_tol_eq = nlp_solver_tol_eq; +ocp.solver_options.nlp_solver_tol_ineq = nlp_solver_tol_ineq; +ocp.solver_options.nlp_solver_tol_comp = nlp_solver_tol_comp; +ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; +ocp.solver_options.qp_solver_cond_N = 5; +ocp.solver_options.qp_solver_ric_alg = 0; +ocp.solver_options.qp_solver_cond_ric_alg = 0; +ocp.solver_options.qp_solver_warm_start = 0; +ocp.solver_options.qp_solver_iter_max = 100; +ocp.solver_options.sim_method_num_stages = 2; +ocp.solver_options.sim_method_num_steps = 1; + +%% create solver +ocp_solver = AcadosOcpSolver(ocp); -% set trajectory initialization -%x_traj_init = zeros(nx, N+1); -%for ii=1:N x_traj_init(:,ii) = [0; pi; 0; 0]; end x_traj_init = [linspace(0, 0, N+1); linspace(pi, 0, N+1); linspace(0, 0, N+1); linspace(0, 0, N+1)]; - u_traj_init = zeros(nu, N); % if not set, the trajectory is initialized with the previous solution ocp_solver.set('init_x', x_traj_init); ocp_solver.set('init_u', u_traj_init); -% change number of sqp iterations -%ocp_solver.set('nlp_solver_max_iter', 20); - % solve tic; -if 0 +matlab_sqp_loop = 0; +if ~matlab_sqp_loop % solve ocp ocp_solver.solve(); else - % do one step at the time + % do one step at a time ocp_solver.set('nlp_solver_max_iter', 1); for ii=1:nlp_solver_max_iter @@ -288,7 +170,7 @@ end fprintf('A eig max %e\n', qp_A_eig_max); - % compute conditioning number and eigenvalues of hessian of (partial) cond qp + % compute condition number and eigenvalues of hessian of (partial) cond qp qp_cond_H = ocp_solver.get('qp_solver_cond_H'); if iscell(qp_cond_H) @@ -340,20 +222,26 @@ sqp_iter = ocp_solver.get('sqp_iter'); time_tot = ocp_solver.get('time_tot'); time_lin = ocp_solver.get('time_lin'); +time_sim = ocp_solver.get('time_sim'); +time_lin_remaining = time_lin - time_sim; % linearization excluding integrators time_reg = ocp_solver.get('time_reg'); +time_qp_solver_call = ocp_solver.get('time_qp_solver_call'); time_qp_sol = ocp_solver.get('time_qp_sol'); +time_qp_remaining = time_qp_sol - time_qp_solver_call; % time for QP solution in addition to QP solver call, mostly condensing +time_remaining_nlp = time_tot - time_lin - time_reg - time_qp_sol; fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); ocp_solver.print('stat'); - -%% figures - -for ii=1:N+1 - x_cur = x(:,ii); -%visualize; -end +%% +figure; +bar_data = 1e3*[time_sim, time_lin_remaining, time_reg, time_qp_solver_call, time_qp_remaining, time_remaining_nlp]'; +bar(0, bar_data, 'stacked') +legend('integrators', 'remaining linearization', 'regularization', 'QP solver call', 'QP processing: condensing, etc', 'remaining') +xlim([-.5, 1]) +ylabel('time in [ms]'); +xticks([]); %% plot trajectory figure; diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 515a08303d..88f889b970 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -122,6 +122,40 @@ function make_consistent(self, is_mocp_phase) end end + % sanity checks on options, which are done in setters in Python + qp_solvers = {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'}; + if ~ismember(opts.qp_solver, qp_solvers) + error(['Invalid qp_solver: ', opts.qp_solver, '. Available options are: ', strjoin(qp_solvers, ', ')]); + end + + regularize_methods = {'NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'}; + if ~ismember(opts.regularize_method, regularize_methods) + error(['Invalid regularize_method: ', opts.regularize_method, '. Available options are: ', strjoin(regularize_methods, ', ')]); + end + hpipm_modes = {'BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'}; + if ~ismember(opts.hpipm_mode, hpipm_modes) + error(['Invalid hpipm_mode: ', opts.hpipm_mode, '. Available options are: ', strjoin(hpipm_modes, ', ')]); + end + INTEGRATOR_TYPES = {'ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'}; + if ~ismember(opts.integrator_type, INTEGRATOR_TYPES) + error(['Invalid integrator_type: ', opts.integrator_type, '. Available options are: ', strjoin(INTEGRATOR_TYPES, ', ')]); + end + + COLLOCATION_TYPES = {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}; + if ~ismember(opts.collocation_type, COLLOCATION_TYPES) + error(['Invalid collocation_type: ', opts.collocation_type, '. Available options are: ', strjoin(COLLOCATION_TYPES, ', ')]); + end + + COST_DISCRETIZATION_TYPES = {'EULER', 'INTEGRATOR'}; + if ~ismember(opts.cost_discretization, COST_DISCRETIZATION_TYPES) + error(['Invalid cost_discretization: ', opts.cost_discretization, '. Available options are: ', strjoin(COST_DISCRETIZATION_TYPES, ', ')]); + end + + search_direction_modes = {'NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP'}; + if ~ismember(opts.search_direction_mode, search_direction_modes) + error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); + end + % OCP name self.name = model.name; diff --git a/interfaces/acados_matlab_octave/AcadosSimSolver.m b/interfaces/acados_matlab_octave/AcadosSimSolver.m index 7b139197be..88b42ce9b8 100644 --- a/interfaces/acados_matlab_octave/AcadosSimSolver.m +++ b/interfaces/acados_matlab_octave/AcadosSimSolver.m @@ -99,7 +99,7 @@ function compile_mex_sim_interface_if_needed(obj, output_dir) % check if path contains spaces if ~isempty(strfind(output_dir, ' ')) - error(strcat('acados_ocp: Path should not contain spaces, got: ',... + error(strcat('compile_mex_sim_interface_if_needed: Path should not contain spaces, got: ',... output_dir)); end diff --git a/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m b/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m index 8e78c15e03..40fa842fa1 100644 --- a/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m +++ b/interfaces/acados_matlab_octave/setup_AcadosOcp_from_legacy_ocp_description.m @@ -59,7 +59,6 @@ ocp.solver_options.integrator_type = upper(opts_struct.sim_method); end - N = opts_struct.param_scheme_N; % options ocp.solver_options.sim_method_num_steps = opts_struct.sim_method_num_steps; ocp.solver_options.sim_method_num_stages = opts_struct.sim_method_num_stages; @@ -129,10 +128,10 @@ ocp.solver_options.fixed_hess = opts_struct.fixed_hess; ocp.solver_options.ext_fun_compile_flags = opts_struct.ext_fun_compile_flags; - ocp.solver_options.ext_fun_expand_dyn = opts_struct.ext_fun_expand_dyn - ocp.solver_options.ext_fun_expand_cost = opts_struct.ext_fun_expand_cost - ocp.solver_options.ext_fun_expand_constr = opts_struct.ext_fun_expand_constr - ocp.solver_options.ext_fun_expand_precompute = opts_struct.ext_fun_expand_precompute + ocp.solver_options.ext_fun_expand_dyn = opts_struct.ext_fun_expand_dyn; + ocp.solver_options.ext_fun_expand_cost = opts_struct.ext_fun_expand_cost; + ocp.solver_options.ext_fun_expand_constr = opts_struct.ext_fun_expand_constr; + ocp.solver_options.ext_fun_expand_precompute = opts_struct.ext_fun_expand_precompute; ocp.solver_options.time_steps = opts_struct.time_steps; ocp.solver_options.shooting_nodes = opts_struct.shooting_nodes; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 68d3e67eca..0c19f6cd8d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -1536,12 +1536,12 @@ def use_constraint_hessian_in_feas_qp(self, use_constraint_hessian_in_feas_qp): @search_direction_mode.setter def search_direction_mode(self, search_direction_mode): - modes = ('NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP') + search_direction_modes = ('NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP') if isinstance(search_direction_mode, str): - if search_direction_mode in modes: + if search_direction_mode in search_direction_modes: self.__search_direction_mode = search_direction_mode else: - Exception(f'Invalid string for search_direction_mode. Possible modes are'+', '.join(modes) + f', got {search_direction_mode}') + raise Exception(f'Invalid string for search_direction_mode. Possible search_direction_modes are'+', '.join(search_direction_modes) + f', got {search_direction_mode}') else: raise Exception(f'Invalid datatype for search_direction_mode. Should be str, got {type(search_direction_mode)}') From d4215456f3ab5e91037e5709b65b6b281c33d028 Mon Sep 17 00:00:00 2001 From: dirkpr <31849867+dirkpr@users.noreply.github.com> Date: Thu, 27 Mar 2025 13:38:30 +0100 Subject: [PATCH 011/164] Stagewise translate (#1481) Refactor the external cost translation into three specialized methods: - `translate_cost_0_term_to_external` (initial) - `translate_cost_term_to_external` (intermediate) - `translate_cost_e_term_to_external` (terminal) This modular approach simplifies translation based on `yref_0`, `yref`, `yref_e` and `W_0`, `W`, `W_e` definitions without requiring direct handling of parameters (`p`, `p_values`, `p_global`, `p_global_values`). --------- Co-authored-by: Jonathan Frey --- .../acados_template/acados_ocp.py | 131 +++++++++++------- 1 file changed, 82 insertions(+), 49 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 6780a4c18b..8957c0b6b9 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1320,13 +1320,8 @@ def translate_cost_to_external_cost(self, :param yref_0, yref, yref_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, 1), (ny, 1), (ny_e, 1). cost_hessian: 'EXACT' or 'GAUSS_NEWTON', determines how the cost hessian is computed. """ - if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") - if cost_hessian == 'GAUSS_NEWTON': - for attr_name, cost_type in ([('cost_type', self.cost.cost_type), ('cost_type_0', self.cost.cost_type_0), ('cost_type_e', self.cost.cost_type_e)]): - if cost_type in ['EXTERNAL', 'AUTO', 'CONVEX_OVER_NONLINEAR']: - raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got {attr_name} = {cost_type}.") casadi_symbolics_type = type(self.model.x) @@ -1357,7 +1352,22 @@ def translate_cost_to_external_cost(self, self.model.p_global = ca.vertcat(self.model.p_global, p_global) self.p_global_values = np.concatenate((self.p_global_values, p_global_values)) - # references + self.translate_intial_cost_term_to_external(yref_0, W_0, cost_hessian) + self.translate_intermediate_cost_term_to_external(yref, W, cost_hessian) + self.translate_terminal_cost_term_to_external(yref_e, W_e, cost_hessian) + + + def translate_intial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + + if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: + raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + + if cost_hessian == 'GAUSS_NEWTON': + if self.cost.cost_type_0 not in ['LINEAR_LS', 'NONLINEAR_LS', None]: + raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_0 = {self.cost.cost_type_0}.") + + casadi_symbolics_type = type(self.model.x) + if yref_0 is None: yref_0 = self.cost.yref_0 else: @@ -1367,25 +1377,6 @@ def translate_cost_to_external_cost(self, if not isinstance(yref_0, casadi_symbolics_type): raise Exception(f"yref_0 has wrong type, got {type(yref_0)}, expected {casadi_symbolics_type}.") - if yref is None: - yref = self.cost.yref - else: - if yref.shape[0] != self.cost.yref.shape[0]: - raise Exception(f"yref has wrong shape, got {yref.shape}, expected {self.cost.yref.shape}.") - - if not isinstance(yref, casadi_symbolics_type): - raise Exception(f"yref has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") - - if yref_e is None: - yref_e = self.cost.yref_e - else: - if yref_e.shape[0] != self.cost.yref_e.shape[0]: - raise Exception(f"yref_e has wrong shape, got {yref_e.shape}, expected {self.cost.yref_e.shape}.") - - if not isinstance(yref_e, casadi_symbolics_type): - raise Exception(f"yref_e has wrong type, got {type(yref_e)}, expected {casadi_symbolics_type}.") - - # weighting matrices if W_0 is None: W_0 = self.cost.W_0 else: @@ -1395,25 +1386,6 @@ def translate_cost_to_external_cost(self, if not isinstance(W_0, casadi_symbolics_type): raise Exception(f"W_0 has wrong type, got {type(W_0)}, expected {casadi_symbolics_type}.") - if W is None: - W = self.cost.W - else: - if W.shape != self.cost.W.shape: - raise Exception(f"W has wrong shape, got {W.shape}, expected {self.cost.W.shape}.") - - if not isinstance(W, casadi_symbolics_type): - raise Exception(f"W has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") - - if W_e is None: - W_e = self.cost.W_e - else: - if W_e.shape != self.cost.W_e.shape: - raise Exception(f"W_e has wrong shape, got {W_e.shape}, expected {self.cost.W_e.shape}.") - - if not isinstance(W_e, casadi_symbolics_type): - raise Exception(f"W_e has wrong type, got {type(W_e)}, expected {casadi_symbolics_type}.") - - # initial stage if self.cost.cost_type_0 == "LINEAR_LS": self.model.cost_expr_ext_cost_0 = \ self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, @@ -1430,7 +1402,40 @@ def translate_cost_to_external_cost(self, self.model.cost_expr_ext_cost_0 = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0, self.model.cost_y_expr_0, yref_0) - # intermediate stages + + if self.cost.cost_type_0 is not None: + self.cost.cost_type_0 = 'EXTERNAL' + + + def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.SX, ca.MX]] = None, W: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + + if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: + raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + + if cost_hessian == 'GAUSS_NEWTON': + if self.cost.cost_type not in ['LINEAR_LS', 'NONLINEAR_LS']: + raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type = {self.cost.cost_type}.") + + casadi_symbolics_type = type(self.model.x) + + if yref is None: + yref = self.cost.yref + else: + if yref.shape[0] != self.cost.yref.shape[0]: + raise Exception(f"yref has wrong shape, got {yref.shape}, expected {self.cost.yref.shape}.") + + if not isinstance(yref, casadi_symbolics_type): + raise Exception(f"yref has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") + + if W is None: + W = self.cost.W + else: + if W.shape != self.cost.W.shape: + raise Exception(f"W has wrong shape, got {W.shape}, expected {self.cost.W.shape}.") + + if not isinstance(W, casadi_symbolics_type): + raise Exception(f"W has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") + if self.cost.cost_type == "LINEAR_LS": self.model.cost_expr_ext_cost = \ self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, @@ -1446,7 +1451,38 @@ def translate_cost_to_external_cost(self, self.model.cost_expr_ext_cost = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr, self.model.cost_y_expr, yref) - # terminal stage + + self.cost.cost_type = 'EXTERNAL' + + + def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, ca.MX]] = None, W_e: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: + raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + + if cost_hessian == 'GAUSS_NEWTON': + if self.cost.cost_type_e not in ['LINEAR_LS', 'NONLINEAR_LS']: + raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_e = {self.cost.cost_type_e}.") + + casadi_symbolics_type = type(self.model.x) + + if yref_e is None: + yref_e = self.cost.yref_e + else: + if yref_e.shape[0] != self.cost.yref_e.shape[0]: + raise Exception(f"yref_e has wrong shape, got {yref_e.shape}, expected {self.cost.yref_e.shape}.") + + if not isinstance(yref_e, casadi_symbolics_type): + raise Exception(f"yref_e has wrong type, got {type(yref_e)}, expected {casadi_symbolics_type}.") + + if W_e is None: + W_e = self.cost.W_e + else: + if W_e.shape != self.cost.W_e.shape: + raise Exception(f"W_e has wrong shape, got {W_e.shape}, expected {self.cost.W_e.shape}.") + + if not isinstance(W_e, casadi_symbolics_type): + raise Exception(f"W_e has wrong type, got {type(W_e)}, expected {casadi_symbolics_type}.") + if self.cost.cost_type_e == "LINEAR_LS": self.model.cost_expr_ext_cost_e = \ self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, @@ -1462,9 +1498,6 @@ def translate_cost_to_external_cost(self, self.model.cost_expr_ext_cost_e = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e, self.model.cost_y_expr_e, yref_e) - if self.cost.cost_type_0 is not None: - self.cost.cost_type_0 = 'EXTERNAL' - self.cost.cost_type = 'EXTERNAL' self.cost.cost_type_e = 'EXTERNAL' From 95f50efdbfadb020096346b6dd7cc3efec5e4c8c Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Fri, 28 Mar 2025 10:05:50 +0100 Subject: [PATCH 012/164] Python: Model with properties (#1484) Finally `AcadosModel` with properties. Next step: move type checks from `make_consistent` to setters --- .../acados_template/acados_model.py | 786 +++++++++++++++--- 1 file changed, 662 insertions(+), 124 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 1666d03b20..30712b062e 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -35,7 +35,7 @@ from casadi import MX, SX -from .utils import is_empty, casadi_length, is_column +from .utils import is_empty, casadi_length from .acados_dims import AcadosOcpDims, AcadosSimDims @@ -50,197 +50,735 @@ class AcadosModel(): """ def __init__(self): ## common for OCP and Integrator - self.name = None - """The model name is used for code generation. Type: string. Default: :code:`None`""" - self.x = [] - """CasADi variable describing the state of the system; Default: :code:`[]`""" - self.xdot = [] - """CasADi variable describing the derivative of the state wrt time; Default: :code:`[]`""" - self.u = [] - """CasADi variable describing the input of the system; Default: :code:`[]`""" - self.z = [] - """CasADi variable describing the algebraic variables of the DAE; Default: :code:`[]`""" - self.p = [] - """CasADi variable describing parameters of the DAE; Default: :code:`[]`""" - self.t = [] - """ - CasADi variable representing time t in functions; Default: :code:`[]` + self.__name = None + self.__x = [] + self.__xdot = [] + self.__u = [] + self.__z = [] + self.__p = [] + self.__t = [] + self.__p_global = [] + + ## dynamics + self.__f_impl_expr = [] + self.__f_expl_expr = [] + self.__disc_dyn_expr = [] + + self.__dyn_ext_fun_type = 'casadi' + self.__dyn_generic_source = None + self.__dyn_disc_fun_jac_hess = None + self.__dyn_disc_fun_jac = None + self.__dyn_disc_fun = None + + self.__dyn_impl_dae_fun_jac = None + self.__dyn_impl_dae_jac = None + self.__dyn_impl_dae_fun = None + + # for GNSF models + self.__gnsf_nontrivial_f_LO = 1 + self.__gnsf_purely_linear = 0 + + ### for OCP only. + # NOTE: These could be moved to cost / constraints + + # constraints at initial stage + self.__con_h_expr_0 = [] + self.__con_phi_expr_0 = [] + self.__con_r_expr_0 = [] + self.__con_r_in_phi_0 = [] + + + # path constraints + # BGH(default): lh <= h(x, u) <= uh + self.__con_h_expr = [] + # BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi + self.__con_phi_expr = [] + self.__con_r_expr = [] + self.__con_r_in_phi = [] + + # terminal + self.__con_h_expr_e = [] + self.__con_phi_expr_e = [] + self.__con_r_expr_e = [] + self.__con_r_in_phi_e = [] + + # cost + self.__cost_y_expr = [] + self.__cost_y_expr_e = [] + self.__cost_y_expr_0 = [] + self.__cost_expr_ext_cost = [] + self.__cost_expr_ext_cost_e = [] + self.__cost_expr_ext_cost_0 = [] + self.__cost_expr_ext_cost_custom_hess = [] + self.__cost_expr_ext_cost_custom_hess_e = [] + self.__cost_expr_ext_cost_custom_hess_0 = [] + + ## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p) + self.__cost_psi_expr_0 = [] + self.__cost_psi_expr = [] + self.__cost_psi_expr_e = [] + self.__cost_r_in_psi_expr_0 = [] + self.__cost_r_in_psi_expr = [] + self.__cost_r_in_psi_expr_e = [] + self.__cost_conl_custom_outer_hess_0 = [] + self.__cost_conl_custom_outer_hess = [] + self.__cost_conl_custom_outer_hess_e = [] + self.__nu_original = None # TODO: remove? only used by benchmark + + self.__t0 = None # TODO: remove? only used by benchmark + self.__x_labels = None + self.__u_labels = None + self.__t_label = "t" + + @property + def name(self): + """The model name is used for code generation. Type: string. + Default: :code:`None` + """ + return self.__name + + @name.setter + def name(self, name): + self.__name = name + + @property + def x(self): + """CasADi variable describing the state of the system; + Default: :code:`[]` + """ + return self.__x + + @x.setter + def x(self, x): + self.__x = x + + @property + def xdot(self): + """CasADi variable describing the derivative of the state wrt time; + Default: :code:`[]` + """ + return self.__xdot + + @xdot.setter + def xdot(self, xdot): + self.__xdot = xdot + + @property + def u(self): + """CasADi variable describing the derivative of the state wrt time; + Default: :code:`[]` + """ + return self.__u + + @u.setter + def u(self, u): + self.__u = u + + @property + def z(self): + """CasADi variable describing the algebraic variables of the DAE; + Default: :code:`[]` + """ + return self.__z + + @z.setter + def z(self, z): + self.__z = z + + @property + def p(self): + """CasADi variable describing stage-wise parameters; + Default: :code:`[]` + """ + return self.__p + + @p.setter + def p(self, p): + self.__p = p + + @property + def t(self): + """CasADi variable representing time t in functions; + Default: :code:`[]` NOTE: - For integrators, the start time has to be explicitly set via :py:attr:`acados_template.AcadosSimSolver.set`('t0'). - - For OCPs, the start time is set to 0. on each stage. + - For OCPs, the start time is set to 0 on each stage. The time dependency can be used within cost formulations and is relevant when cost integration is used. Start times of shooting intervals can be added using parameters. """ - self.p_global = [] + return self.__t + + @t.setter + def t(self, t): + self.__t = t + + @property + def p_global(self): """ - CasADi variable containing global parameters. + CasADi variable representing global parameters. This feature can be used to precompute expensive terms which only depend on these parameters, e.g. spline coefficients, when p_global are underlying data points. Only supported for OCP solvers. Updating these parameters can be done using :py:attr:`acados_template.acados_ocp_solver.AcadosOcpSolver.set_p_global_and_precompute_dependencies(values)`. NOTE: this is only supported with CasADi beta release https://github.com/casadi/casadi/releases/tag/nightly-se Default: :code:`[]` """ + return self.__p_global - ## dynamics - self.f_impl_expr = [] + @p_global.setter + def p_global(self, p_global): + self.__p_global = p_global + + @property + def f_impl_expr(self): r""" CasADi expression for the implicit dynamics :math:`f_\text{impl}(\dot{x}, x, u, z, p) = 0`. Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.integrator_type` == 'IRK'. Default: :code:`[]` """ - self.f_expl_expr = [] + return self.__f_impl_expr + + @f_impl_expr.setter + def f_impl_expr(self, f_impl_expr): + self.__f_impl_expr = f_impl_expr + + @property + def f_expl_expr(self): r""" CasADi expression for the explicit dynamics :math:`\dot{x} = f_\text{expl}(x, u, p)`. Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.integrator_type` == 'ERK'. Default: :code:`[]` """ - self.disc_dyn_expr = [] + return self.__f_expl_expr + + @f_expl_expr.setter + def f_expl_expr(self, f_expl_expr): + self.__f_expl_expr = f_expl_expr + + @property + def disc_dyn_expr(self): r""" CasADi expression for the discrete dynamics :math:`x_{+} = f_\text{disc}(x, u, p)`. Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.integrator_type` == 'DISCRETE'. Default: :code:`[]` """ + return self.__disc_dyn_expr - self.dyn_ext_fun_type = 'casadi' - """type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi'""" - self.dyn_generic_source = None - """name of source file for discrete dynamics, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" - self.dyn_disc_fun_jac_hess = None - """name of function discrete dynamics + jacobian and hessian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" - self.dyn_disc_fun_jac = None - """name of function discrete dynamics + jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" - self.dyn_disc_fun = None - """name of function discrete dynamics, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" + @disc_dyn_expr.setter + def disc_dyn_expr(self, disc_dyn_expr): + self.__disc_dyn_expr = disc_dyn_expr - self.dyn_impl_dae_fun_jac = None - """name of source files for implicit DAE function value and jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" - self.dyn_impl_dae_jac = None - """name of source files for implicit DAE jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" - self.dyn_impl_dae_fun = None - """name of source files for implicit DAE function value, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; Default: :code:`None`""" + @property + def dyn_ext_fun_type(self): + """ + Type of external functions for dynamics module; 'casadi' or 'generic'; + Default: 'casadi' + """ + return self.__dyn_ext_fun_type - # for GNSF models - self.gnsf_nontrivial_f_LO = 1 - """GNSF: Flag indicating whether GNSF stucture has nontrivial f.""" - self.gnsf_purely_linear = 0 - """GNSF: Flag indicating whether GNSF stucture is purely linear.""" + @dyn_ext_fun_type.setter + def dyn_ext_fun_type(self, dyn_ext_fun_type): + self.__dyn_ext_fun_type = dyn_ext_fun_type + + @property + def dyn_generic_source(self): + """ + Name of source file for discrete dynamics, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_generic_source + @dyn_generic_source.setter + def dyn_generic_source(self, dyn_generic_source): + self.__dyn_generic_source = dyn_generic_source - ### for OCP only. - # NOTE: These could be moved to cost / constraints + @property + def dyn_disc_fun_jac_hess(self): + """ + Name of function discrete dynamics + jacobian and hessian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_disc_fun_jac_hess - # constraints at initial stage - self.con_h_expr_0 = [] - """CasADi expression for the initial constraint :math:`h^0`; Default: :code:`[]`""" - self.con_phi_expr_0 = [] - r"""CasADi expression for the terminal constraint :math:`\phi_0`; Default: :code:`[]`""" - self.con_r_expr_0 = [] - r"""CasADi expression for the terminal constraint :math:`\phi_0(r)`, - dummy input for outer function; Default: :code:`[]`""" - self.con_r_in_phi_0 = [] - r"""CasADi expression for the terminal constraint :math:`\phi_0(r)`, input for outer function; Default: :code:`[]`""" + @dyn_disc_fun_jac_hess.setter + def dyn_disc_fun_jac_hess(self, dyn_disc_fun_jac_hess): + self.__dyn_disc_fun_jac_hess = dyn_disc_fun_jac_hess - # path constraints - # BGH(default): lh <= h(x, u) <= uh - self.con_h_expr = [] - """CasADi expression for the constraint :math:`h`; Default: :code:`[]`""" - # BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi - self.con_phi_expr = [] - """CasADi expression for the constraint phi; Default: :code:`[]`""" - self.con_r_expr = [] - """CasADi expression for the constraint phi(r), - dummy input for outer function; Default: :code:`[]`""" - self.con_r_in_phi = [] - r"""CasADi expression for the terminal constraint :math:`\phi(r)`, - input for outer function; Default: :code:`[]`""" + @property + def dyn_disc_fun_jac(self): + """ + Name of function discrete dynamics + jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_disc_fun_jac - # terminal - self.con_h_expr_e = [] - """CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`[]`""" - self.con_phi_expr_e = [] - r"""CasADi expression for the terminal constraint :math:`\phi_e`; Default: :code:`[]`""" - self.con_r_expr_e = [] - r"""CasADi expression for the terminal constraint :math:`\phi_e(r)`, - dummy input for outer function; Default: :code:`[]`""" - self.con_r_in_phi_e = [] - r"""CasADi expression for the terminal constraint :math:`\phi_e(r)`, input for outer function; Default: :code:`[]`""" + @dyn_disc_fun_jac.setter + def dyn_disc_fun_jac(self, dyn_disc_fun_jac): + self.__dyn_disc_fun_jac = dyn_disc_fun_jac - # cost - self.cost_y_expr = [] - """CasADi expression for nonlinear least squares; Default: :code:`[]`""" - self.cost_y_expr_e = [] - """CasADi expression for nonlinear least squares, terminal; Default: :code:`[]`""" - self.cost_y_expr_0 = [] - """CasADi expression for nonlinear least squares, initial; Default: :code:`[]`""" - self.cost_expr_ext_cost = [] - """CasADi expression for external cost; Default: :code:`[]`""" - self.cost_expr_ext_cost_e = [] - """CasADi expression for external cost, terminal; Default: :code:`[]`""" - self.cost_expr_ext_cost_0 = [] - """CasADi expression for external cost, initial; Default: :code:`[]`""" - self.cost_expr_ext_cost_custom_hess = [] - """CasADi expression for custom hessian (only for external cost); Default: :code:`[]`""" - self.cost_expr_ext_cost_custom_hess_e = [] - """CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`[]`""" - self.cost_expr_ext_cost_custom_hess_0 = [] - """CasADi expression for custom hessian (only for external cost), initial; Default: :code:`[]`""" - ## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p) - self.cost_psi_expr_0 = [] + + @property + def dyn_disc_fun(self): + """ + Name of function discrete dynamics, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_disc_fun + + @dyn_disc_fun.setter + def dyn_disc_fun(self, dyn_disc_fun): + self.__dyn_disc_fun = dyn_disc_fun + + @property + def dyn_impl_dae_fun_jac(self): + """ + Name of source files for implicit DAE function value and jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_impl_dae_fun_jac + + @dyn_impl_dae_fun_jac.setter + def dyn_impl_dae_fun_jac(self, dyn_impl_dae_fun_jac): + self.__dyn_impl_dae_fun_jac = dyn_impl_dae_fun_jac + + @property + def dyn_impl_dae_jac(self): + """ + Name of source files for implicit DAE jacobian, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_impl_dae_jac + + @dyn_impl_dae_jac.setter + def dyn_impl_dae_jac(self, dyn_impl_dae_jac): + self.__dyn_impl_dae_jac = dyn_impl_dae_jac + + @property + def dyn_impl_dae_fun(self): + """ + Name of source files for implicit DAE function value, only relevant if :code:`dyn_ext_fun_type` is :code:`'generic'`; + Default: :code:`None` + """ + return self.__dyn_impl_dae_fun + + @dyn_impl_dae_fun.setter + def dyn_impl_dae_fun(self, dyn_impl_dae_fun): + self.__dyn_impl_dae_fun = dyn_impl_dae_fun + + @property + def gnsf_nontrivial_f_LO(self): + """ + GNSF: Flag indicating whether GNSF stucture has nontrivial f. + """ + return self.__gnsf_nontrivial_f_LO + + @gnsf_nontrivial_f_LO.setter + def gnsf_nontrivial_f_LO(self, gnsf_nontrivial_f_LO): + self.__gnsf_nontrivial_f_LO = gnsf_nontrivial_f_LO + + @property + def gnsf_purely_linear(self): + """ + GNSF: Flag indicating whether GNSF stucture is purely linear. + """ + return self.__gnsf_purely_linear + + @gnsf_purely_linear.setter + def gnsf_purely_linear(self, gnsf_purely_linear): + self.__gnsf_purely_linear = gnsf_purely_linear + + @property + def con_h_expr_0(self): + r""" + CasADi expression for the initial constraint :math:`h^0`; + Default: :code:`[]` + """ + return self.__con_h_expr_0 + + @con_h_expr_0.setter + def con_h_expr_0(self, con_h_expr_0): + self.__con_h_expr_0 = con_h_expr_0 + + @property + def con_phi_expr_0(self): r""" - CasADi expression for the outer loss function :math:`\psi(r - yref, t, p)`, initial; Default: :code:`[]` + CasADi expression for the outer function of the initial constraint :math:`\phi_0(r_0)`; + Default: :code:`[]` + """ + return self.__con_phi_expr_0 + + @con_phi_expr_0.setter + def con_phi_expr_0(self, con_phi_expr_0): + self.__con_phi_expr_0 = con_phi_expr_0 + + @property + def con_r_expr_0(self): + r""" + CasADi expression for the inner function of the initial constraint :math:`\phi_0(r_0)`; + Default: :code:`[]` + """ + return self.__con_r_expr_0 + + @con_r_expr_0.setter + def con_r_expr_0(self, con_r_expr_0): + self.__con_r_expr_0 = con_r_expr_0 + + @property + def con_r_in_phi_0(self): + r""" + CasADi variable defining the input to the outer function of the initial constraint :math:`\phi_0(r_0)`; + Default: :code:`[]` + """ + return self.__con_r_in_phi_0 + + @con_r_in_phi_0.setter + def con_r_in_phi_0(self, con_r_in_phi_0): + self.__con_r_in_phi_0 = con_r_in_phi_0 + + @property + def con_h_expr(self): + """ + CasADi expression for the intermediate constraint :math:`h`; + Default: :code:`[]` + """ + return self.__con_h_expr + + @con_h_expr.setter + def con_h_expr(self, con_h_expr): + self.__con_h_expr = con_h_expr + + @property + def con_phi_expr(self): + r""" + CasADi expression for the outer function of the intermediate constraint :math:`\phi(r)`; + Default: :code:`[]` + """ + return self.__con_phi_expr + + @con_phi_expr.setter + def con_phi_expr(self, con_phi_expr): + self.__con_phi_expr = con_phi_expr + + @property + def con_r_expr(self): + r""" + CasADi expression for the inner function of the intermediate constraint :math:`\phi(r)`; + Default: :code:`[]` + """ + return self.__con_r_expr + + @con_r_expr.setter + def con_r_expr(self, con_r_expr): + self.__con_r_expr = con_r_expr + + @property + def con_r_in_phi(self): + r""" + CasADi variable defining the input to the outer function of the intermediate constraint :math:`\phi(r)`; + Default: :code:`[]` + """ + return self.__con_r_in_phi + + @con_r_in_phi.setter + def con_r_in_phi(self, con_r_in_phi): + self.__con_r_in_phi = con_r_in_phi + + @property + def con_h_expr_e(self): + """ + CasADi expression for the terminal constraint :math:`h_e`; + Default: :code:`[]` + """ + return self.__con_h_expr_e + + @con_h_expr_e.setter + def con_h_expr_e(self, con_h_expr_e): + self.__con_h_expr_e = con_h_expr_e + + @property + def con_phi_expr_e(self): + r""" + CasADi expression for the outer function of the terminal constraint :math:`\phi_e(r_e)`; + Default: :code:`[]` + """ + return self.__con_phi_expr_e + + @con_phi_expr_e.setter + def con_phi_expr_e(self, con_phi_expr_e): + self.__con_phi_expr_e = con_phi_expr_e + + @property + def con_r_expr_e(self): + r""" + CasADi expression for the inner function of the terminal constraint :math:`\phi_e(r_e)`; + Default: :code:`[]` + """ + return self.__con_r_expr_e + + @con_r_expr_e.setter + def con_r_expr_e(self, con_r_expr_e): + self.__con_r_expr_e = con_r_expr_e + + @property + def con_r_in_phi_e(self): + r""" + CasADi variable defining the input to the outer function of the terminal constraint :math:`\phi_e(r_e)`; + Default: :code:`[]` + """ + return self.__con_r_in_phi_e + + @con_r_in_phi_e.setter + def con_r_in_phi_e(self, con_r_in_phi_e): + self.__con_r_in_phi_e = con_r_in_phi_e + + @property + def cost_y_expr(self): + """CasADi expression for nonlinear least squares; + Default: :code:`[]` + """ + return self.__cost_y_expr + + @cost_y_expr.setter + def cost_y_expr(self, cost_y_expr): + self.__cost_y_expr = cost_y_expr + + + @property + def cost_y_expr_e(self): + """CasADi expression for nonlinear least squares, terminal; + Default: :code:`[]` + """ + return self.__cost_y_expr_e + + @cost_y_expr_e.setter + def cost_y_expr_e(self, cost_y_expr_e): + self.__cost_y_expr_e = cost_y_expr_e + + @property + def cost_y_expr_0(self): + """CasADi expression for nonlinear least squares, initial; + Default: :code:`[]` + """ + return self.__cost_y_expr_0 + + @cost_y_expr_0.setter + def cost_y_expr_0(self, cost_y_expr_0): + self.__cost_y_expr_0 = cost_y_expr_0 + + @property + def cost_expr_ext_cost(self): + """CasADi expression for external cost; + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost + + @cost_expr_ext_cost.setter + def cost_expr_ext_cost(self, cost_expr_ext_cost): + self.__cost_expr_ext_cost = cost_expr_ext_cost + + @property + def cost_expr_ext_cost_e(self): + """CasADi expression for external cost, terminal; + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost_e + + @cost_expr_ext_cost_e.setter + def cost_expr_ext_cost_e(self, cost_expr_ext_cost_e): + self.__cost_expr_ext_cost_e = cost_expr_ext_cost_e + + @property + def cost_expr_ext_cost_0(self): + """CasADi expression for external cost, initial; + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost_0 + + @cost_expr_ext_cost_0.setter + def cost_expr_ext_cost_0(self, cost_expr_ext_cost_0): + self.__cost_expr_ext_cost_0 = cost_expr_ext_cost_0 + + + @property + def cost_expr_ext_cost_custom_hess(self): + """CasADi expression for custom hessian (only for external cost); + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost_custom_hess + + @cost_expr_ext_cost_custom_hess.setter + def cost_expr_ext_cost_custom_hess(self, cost_expr_ext_cost_custom_hess): + self.__cost_expr_ext_cost_custom_hess = cost_expr_ext_cost_custom_hess + + @property + def cost_expr_ext_cost_custom_hess_e(self): + """CasADi expression for custom hessian (only for external cost), terminal; + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost_custom_hess_e + + @cost_expr_ext_cost_custom_hess_e.setter + def cost_expr_ext_cost_custom_hess_e(self, cost_expr_ext_cost_custom_hess_e): + self.__cost_expr_ext_cost_custom_hess_e = cost_expr_ext_cost_custom_hess_e + + @property + def cost_expr_ext_cost_custom_hess_0(self): + """CasADi expression for custom hessian (only for external cost), initial; + Default: :code:`[]` + """ + return self.__cost_expr_ext_cost_custom_hess_0 + + @cost_expr_ext_cost_custom_hess_0.setter + def cost_expr_ext_cost_custom_hess_0(self, cost_expr_ext_cost_custom_hess_0): + self.__cost_expr_ext_cost_custom_hess_0 = cost_expr_ext_cost_custom_hess_0 + + @property + def cost_psi_expr_0(self): + r""" + CasADi expression for the outer loss function :math:`\psi(r - yref, t, p)`, initial; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_0` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_psi_expr = [] + return self.__cost_psi_expr_0 + + @cost_psi_expr_0.setter + def cost_psi_expr_0(self, cost_psi_expr_0): + self.__cost_psi_expr_0 = cost_psi_expr_0 + + + @property + def cost_psi_expr(self): r""" - CasADi expression for the outer loss function :math:`\psi(r - yref, t, p)`; Default: :code:`[]` + CasADi expression for the outer loss function :math:`\psi(r - yref, t, p)`; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_psi_expr_e = [] + return self.__cost_psi_expr + + @cost_psi_expr.setter + def cost_psi_expr(self, cost_psi_expr): + self.__cost_psi_expr = cost_psi_expr + + @property + def cost_psi_expr_e(self): r""" - CasADi expression for the outer loss function :math:`\psi(r - yref, p)`, terminal; Default: :code:`[]` + CasADi expression for the outer loss function :math:`\psi(r - yref, t, p)`, terminal; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_e` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_r_in_psi_expr_0 = [] + return self.__cost_psi_expr_e + + @cost_psi_expr_e.setter + def cost_psi_expr_e(self, cost_psi_expr_e): + self.__cost_psi_expr_e = cost_psi_expr_e + + @property + def cost_r_in_psi_expr_0(self): r""" - CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, t, p)`, initial; Default: :code:`[]` + CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, t, p)`, initial; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_0` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_r_in_psi_expr = [] + return self.__cost_r_in_psi_expr_0 + + @cost_r_in_psi_expr_0.setter + def cost_r_in_psi_expr_0(self, cost_r_in_psi_expr_0): + self.__cost_r_in_psi_expr_0 = cost_r_in_psi_expr_0 + + @property + def cost_r_in_psi_expr(self): r""" - CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, t, p)`; Default: :code:`[]` + CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, t, p)`; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_r_in_psi_expr_e = [] + return self.__cost_r_in_psi_expr + + @cost_r_in_psi_expr.setter + def cost_r_in_psi_expr(self, cost_r_in_psi_expr): + self.__cost_r_in_psi_expr = cost_r_in_psi_expr + + + @property + def cost_r_in_psi_expr_e(self): r""" - CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`[]` + CasADi symbolic input variable for the argument :math:`r` to the outer loss function :math:`\psi(r, t, p)`, terminal; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_e` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_conl_custom_outer_hess_0 = [] + return self.__cost_r_in_psi_expr_e + + @cost_r_in_psi_expr_e.setter + def cost_r_in_psi_expr_e(self, cost_r_in_psi_expr_e): + self.__cost_r_in_psi_expr_e = cost_r_in_psi_expr_e + + + @property + def cost_conl_custom_outer_hess_0(self): """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`[]` + CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_0` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_conl_custom_outer_hess = [] + return self.__cost_conl_custom_outer_hess_0 + + @cost_conl_custom_outer_hess_0.setter + def cost_conl_custom_outer_hess_0(self, cost_conl_custom_outer_hess_0): + self.__cost_conl_custom_outer_hess_0 = cost_conl_custom_outer_hess_0 + + @property + def cost_conl_custom_outer_hess(self): """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`[]` + CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type` is 'CONVEX_OVER_NONLINEAR'. """ - self.cost_conl_custom_outer_hess_e = [] + return self.__cost_conl_custom_outer_hess + + @cost_conl_custom_outer_hess.setter + def cost_conl_custom_outer_hess(self, cost_conl_custom_outer_hess): + self.__cost_conl_custom_outer_hess = cost_conl_custom_outer_hess + + + @property + def cost_conl_custom_outer_hess_e(self): """ - CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`[]` + CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; + Default: :code:`[]` Used if :py:attr:`acados_template.acados_ocp_options.AcadosOcpOptions.cost_type_e` is 'CONVEX_OVER_NONLINEAR'. """ - self.nu_original = None # TODO: remove? only used by benchmark + return self.__cost_conl_custom_outer_hess_e + + @cost_conl_custom_outer_hess_e.setter + def cost_conl_custom_outer_hess_e(self, cost_conl_custom_outer_hess_e): + self.__cost_conl_custom_outer_hess_e = cost_conl_custom_outer_hess_e + + + @property + def nu_original(self): """ - Number of original control inputs (before polynomial control augmentation); Default: :code:`None` + Number of original control inputs (before polynomial control augmentation); + Default: :code:`None` """ - self.t0 = None # TODO: remove? only used by benchmark - """CasADi variable representing the start time of an interval; Default: :code:`None`""" - self.__x_labels = None - self.__u_labels = None - self.__t_label = "t" + return self.__nu_original + + @nu_original.setter + def nu_original(self, nu_original): + self.__nu_original = nu_original + + + @property + def t0(self): + """ + Only relevant for benchmark, TODO + """ + return self.__t0 + + @t0.setter + def t0(self, t0): + self.__t0 = t0 + @property def x_labels(self): From fb0485a8e948589425f15dc652b813d12b339c33 Mon Sep 17 00:00:00 2001 From: Confectio <30325218+Confectio@users.noreply.github.com> Date: Fri, 28 Mar 2025 12:58:58 +0100 Subject: [PATCH 013/164] Dynamic num threads in batch solvers (#1483) Previously, the number of threads to be used in the `AcadosOcpBatchSolver` or `AcadosSimBatchSolver` batched methods had to be set in the respective `AcadosOcpOptions` or `AcadosSimOptions` and could not be changed without rebuilding the solver. Now, it is a property that can be set dynamically, even after building the solver. The property `num_threads_in_batch_solve` in both `AcadosOcpOptions` and `AcadosSimOptions` is now deprecated, and warnings will be printed if it is used. Instead, the `AcadosOcpBatchSolver` and `AcadosSimBatchSolver` have a new (settable) property `num_threads_in_batch_solve` and take the initial value in their constructor via a new `num_threads_in_batch_solve` argument. The value of the new attribute is now used to determine how many threads the batched methods should use. The property `num_threads_in_batch_solve` in `AcadosOcpOptions` or `AcadosSimOptions` was previously used for determining whether the solvers should be build with openmp and how the C code of the batched methods should be templated (if `num_threads_in_batch_solve > 1`). Instead a new property `with_batch_functionality` is used as a flag to determine whether the solvers should be build with openmp and whether the C code of the batched methods should be created at all. This new property is not required to be set by the user himself, but is being set by the BatchSolvers themselves by default (a warning is printed though, to notify the user that the code is still build with the openmp flag). The matlab templates and the batch solver examples have been adjusted accordingly. Further, the minimal batch solver examples include new simple tests for the new `num_threads_in_batch_solve` property. --------- Co-authored-by: Jonathan Frey --- .../ocp/minimal_example_batch_ocp_solver.py | 15 ++- .../sim/minimal_example_batch_sim_solver.py | 15 ++- ...ch_adjoint_solution_sensitivity_example.py | 4 +- .../setup_parametric_ocp.py | 3 - .../acados_matlab_octave/AcadosOcpOptions.m | 4 +- .../acados_matlab_octave/AcadosSimOptions.m | 4 +- .../acados_ocp_batch_solver.py | 48 +++++-- .../acados_template/acados_ocp_options.py | 20 +++ .../acados_template/acados_sim.py | 20 ++- .../acados_sim_batch_solver.py | 30 ++++- .../c_templates_tera/CMakeLists.in.txt | 2 +- .../c_templates_tera/Makefile.in | 4 +- .../c_templates_tera/acados_sim_solver.in.c | 22 ++-- .../c_templates_tera/acados_sim_solver.in.h | 4 +- .../c_templates_tera/acados_solver.in.c | 120 ++++++++++-------- .../c_templates_tera/acados_solver.in.h | 13 +- .../c_templates_tera/multi_Makefile.in | 4 +- 17 files changed, 220 insertions(+), 112 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py index 34d7082e78..e23efe7170 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py @@ -44,10 +44,10 @@ If you want to use the batch solver, make sure to compile acados with openmp and num_threads set to 1, i.e. with the flags -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 -The number of threads for the batch solver is then set via the option `num_threads_in_batch_solve`, see below. +The number of threads for the batch solver is given in its constructor, see below. """ -def setup_ocp(num_threads_in_batch_solve=1, tol=1e-7): +def setup_ocp(tol=1e-7): ocp = AcadosOcp() @@ -89,7 +89,6 @@ def setup_ocp(num_threads_in_batch_solve=1, tol=1e-7): ocp.solver_options.nlp_solver_tol_eq = tol ocp.solver_options.nlp_solver_tol_ineq = tol ocp.solver_options.nlp_solver_tol_comp = tol - ocp.solver_options.num_threads_in_batch_solve = num_threads_in_batch_solve ocp.solver_options.tf = Tf @@ -122,8 +121,14 @@ def main_sequential(x0, N_sim, tol): def main_batch(Xinit, simU, tol, num_threads_in_batch_solve=1): N_batch = Xinit.shape[0] - 1 - ocp = setup_ocp(num_threads_in_batch_solve, tol) - batch_solver = AcadosOcpBatchSolver(ocp, N_batch, verbose=False) + ocp = setup_ocp(tol) + batch_solver = AcadosOcpBatchSolver(ocp, N_batch, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) + + assert batch_solver.num_threads_in_batch_solve == num_threads_in_batch_solve + batch_solver.num_threads_in_batch_solve = 1337 + assert batch_solver.num_threads_in_batch_solve == 1337 + batch_solver.num_threads_in_batch_solve = num_threads_in_batch_solve + assert batch_solver.num_threads_in_batch_solve == num_threads_in_batch_solve for n in range(N_batch): batch_solver.ocp_solvers[n].constraints_set(0, "lbx", Xinit[n]) diff --git a/examples/acados_python/pendulum_on_cart/sim/minimal_example_batch_sim_solver.py b/examples/acados_python/pendulum_on_cart/sim/minimal_example_batch_sim_solver.py index a172eb003b..b2404636ee 100644 --- a/examples/acados_python/pendulum_on_cart/sim/minimal_example_batch_sim_solver.py +++ b/examples/acados_python/pendulum_on_cart/sim/minimal_example_batch_sim_solver.py @@ -43,11 +43,11 @@ If you want to use the batch solver, make sure to compile acados with openmp and num_threads set to 1, i.e. with the flags -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 -The number of threads for the batch solver is then set via the option `num_threads_in_batch_solve`, see below. +The number of threads for the batch solver is then set in its constructor, see below. """ -def setup_integrator(num_threads_in_batch_solve=1): +def setup_integrator(): sim = AcadosSim() sim.model = export_pendulum_ode_model() @@ -57,7 +57,6 @@ def setup_integrator(num_threads_in_batch_solve=1): sim.solver_options.num_steps = 10 sim.solver_options.newton_iter = 10 # for implicit integrator sim.solver_options.collocation_type = "GAUSS_RADAU_IIA" - sim.solver_options.num_threads_in_batch_solve = num_threads_in_batch_solve return sim @@ -85,8 +84,14 @@ def main_sequential(x0, u0, N_sim): def main_batch(Xinit, u0, num_threads_in_batch_solve=1): N_batch = Xinit.shape[0] - 1 - sim = setup_integrator(num_threads_in_batch_solve) - batch_integrator = AcadosSimBatchSolver(sim, N_batch, verbose=False) + sim = setup_integrator() + batch_integrator = AcadosSimBatchSolver(sim, N_batch, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) + + assert batch_integrator.num_threads_in_batch_solve == num_threads_in_batch_solve + batch_integrator.num_threads_in_batch_solve = 1337 + assert batch_integrator.num_threads_in_batch_solve == 1337 + batch_integrator.num_threads_in_batch_solve = num_threads_in_batch_solve + assert batch_integrator.num_threads_in_batch_solve == num_threads_in_batch_solve for n in range(N_batch): batch_integrator.sim_solvers[n].set("u", u0) diff --git a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py index 2e97c07d36..e18d60c615 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py @@ -81,10 +81,10 @@ def main_batch(Xinit, simU, param_vals, adjoints_ref, tol, num_threads_in_batch_ N_batch = Xinit.shape[0] - 1 learnable_params = ["A", "Q", "b"] - ocp = export_parametric_ocp(PARAM_VALUE_DICT, learnable_params=learnable_params, num_threads_in_batch_solve = num_threads_in_batch_solve) + ocp = export_parametric_ocp(PARAM_VALUE_DICT, learnable_params=learnable_params) ocp.solver_options.with_solution_sens_wrt_params = True - batch_solver = AcadosOcpBatchSolver(ocp, N_batch, verbose=False) + batch_solver = AcadosOcpBatchSolver(ocp, N_batch, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) # reset, set bounds and p_global t0 = time.time() diff --git a/examples/acados_python/solution_sensitivities_convex_example/setup_parametric_ocp.py b/examples/acados_python/solution_sensitivities_convex_example/setup_parametric_ocp.py index 9a85c97812..ebb50c6e3d 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/setup_parametric_ocp.py +++ b/examples/acados_python/solution_sensitivities_convex_example/setup_parametric_ocp.py @@ -89,7 +89,6 @@ def export_parametric_ocp( param: dict[str, np.ndarray], name: str = "lti", learnable_params: Optional[list[str]] = None, - num_threads_in_batch_solve: int = 1, ) -> AcadosOcp: if learnable_params is None: @@ -108,8 +107,6 @@ def export_parametric_ocp( ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.nlp_solver_type = "SQP" - ocp.solver_options.num_threads_in_batch_solve = num_threads_in_batch_solve - # Add learnable parameters to p_global if len(learnable_params) != 0: ocp.model.p_global = struct_symSX( diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 82f9c4dc6d..6b6bf79b8d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -134,7 +134,7 @@ custom_update_header_filename custom_templates custom_update_copy - num_threads_in_batch_solve + with_batch_functionality compile_interface @@ -251,7 +251,7 @@ obj.custom_update_header_filename = ''; obj.custom_templates = []; obj.custom_update_copy = true; - obj.num_threads_in_batch_solve = 1; + obj.with_batch_functionality = false; obj.compile_interface = []; % corresponds to automatic detection, possible values: true, false, [] end diff --git a/interfaces/acados_matlab_octave/AcadosSimOptions.m b/interfaces/acados_matlab_octave/AcadosSimOptions.m index 1928e28aec..6c7602e35d 100644 --- a/interfaces/acados_matlab_octave/AcadosSimOptions.m +++ b/interfaces/acados_matlab_octave/AcadosSimOptions.m @@ -46,8 +46,8 @@ output_z ext_fun_compile_flags ext_fun_expand_dyn - num_threads_in_batch_solve compile_interface + with_batch_functionality end methods @@ -73,7 +73,7 @@ obj.ext_fun_compile_flags = env_var; end obj.ext_fun_expand_dyn = false; - obj.num_threads_in_batch_solve = 1; + obj.with_batch_functionality = false; obj.compile_interface = []; % corresponds to automatic detection, possible values: true, false, [] end diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index bab9bc80a1..85aa43a656 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -32,7 +32,7 @@ from .acados_ocp_solver import AcadosOcpSolver from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpFlattenedBatchIterate -from typing import Optional, List, Tuple, Sequence +from typing import Optional, List, Tuple, Sequence, Union from ctypes import (POINTER, c_int, c_void_p, cast, c_double, c_char_p) import numpy as np import time @@ -43,6 +43,7 @@ class AcadosOcpBatchSolver(): :param ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` :param N_batch: batch size, positive integer + :param num_threads_in_batch_solve: number of threads used for parallelizing the batch methods. Default: 1 :param json_file: Default: 'acados_ocp.json' :param build: Flag indicating whether solver should be (re)compiled. If False an attempt is made to load an already compiled shared library for the solver. Default: True :param generate: Flag indicating whether problem functions should be code generated. Default: True @@ -51,10 +52,22 @@ class AcadosOcpBatchSolver(): __ocp_solvers : List[AcadosOcpSolver] - def __init__(self, ocp: AcadosOcp, N_batch: int, json_file: str = 'acados_ocp.json', build: bool = True, generate: bool = True, verbose: bool=True): + def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None, json_file: str = 'acados_ocp.json', build: bool = True, generate: bool = True, verbose: bool=True): if not isinstance(N_batch, int) or N_batch <= 0: raise Exception("AcadosOcpBatchSolver: argument N_batch should be a positive integer.") + if num_threads_in_batch_solve is None: + num_threads_in_batch_solve = ocp.solver_options.num_threads_in_batch_solve + print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead.") + print("In the future, it should be passed explicitly in the AcadosOcpBatchSolver constructor.") + if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: + raise Exception("AcadosOcpBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") + if not ocp.solver_options.with_batch_functionality: + print("Warning: Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False.") + print("Attempting to compile with openmp nonetheless.") + ocp.solver_options.with_batch_functionality = True + + self.__num_threads_in_batch_solve = num_threads_in_batch_solve self.__N_batch = N_batch self.__ocp_solvers = [AcadosOcpSolver(ocp, @@ -76,19 +89,19 @@ def __init__(self, ocp: AcadosOcp, N_batch: int, json_file: str = 'acados_ocp.js self.__status = np.zeros((self.N_batch,), dtype=np.intc, order="C") self.__status_p = cast(self.__status.ctypes.data, POINTER(c_int)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve").argtypes = [POINTER(c_void_p), POINTER(c_int), c_int] + getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve").argtypes = [POINTER(c_void_p), POINTER(c_int), c_int, c_int] getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve").restype = c_void_p - getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac").argtypes = [POINTER(c_void_p), c_int] + getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac").argtypes = [POINTER(c_void_p), c_int, c_int] getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac").restype = c_void_p - getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p").argtypes = [POINTER(c_void_p), c_char_p, c_int, POINTER(c_double), c_int, c_int] + getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p").argtypes = [POINTER(c_void_p), c_char_p, c_int, POINTER(c_double), c_int, c_int, c_int] getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p").restype = c_void_p - getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat").argtypes = [POINTER(c_void_p), c_char_p, POINTER(c_double), c_int, c_int] + getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat").argtypes = [POINTER(c_void_p), c_char_p, POINTER(c_double), c_int, c_int, c_int] getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat").restype = c_void_p - getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat").argtypes = [POINTER(c_void_p), c_char_p, POINTER(c_double), c_int, c_int] + getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat").argtypes = [POINTER(c_void_p), c_char_p, POINTER(c_double), c_int, c_int, c_int] getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat").restype = c_void_p if self.ocp_solvers[0].acados_lib_uses_omp: @@ -111,6 +124,15 @@ def ocp_solvers(self): def N_batch(self): """Batch size.""" return self.__N_batch + + @property + def num_threads_in_batch_solve(self): + """Number of threads used for parallelizing the batch methods.""" + return self.__num_threads_in_batch_solve + + @num_threads_in_batch_solve.setter + def num_threads_in_batch_solve(self, num_threads_in_batch_solve): + self.__num_threads_in_batch_solve = num_threads_in_batch_solve def solve(self): @@ -118,7 +140,7 @@ def solve(self): Call solve for all `N_batch` solvers. """ - getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): @@ -130,7 +152,7 @@ def setup_qp_matrices_and_factorize(self): Call setup_qp_matrices_and_factorize for all `N_batch` solvers. """ - getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): @@ -212,7 +234,7 @@ def eval_adjoint_solution_sensitivity(self, # compute jacobian wrt params t0 = time.time() - getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, self.__N_batch) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, self.__N_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_lin = time.time() - t0 t1 = time.time() @@ -235,7 +257,7 @@ def eval_adjoint_solution_sensitivity(self, # solve adjoint sensitivities getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p")( - self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, self.__N_batch) + self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, self.__N_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_solve = time.time() - t1 @@ -274,7 +296,7 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: value_ = value_.astype(float) value_data = cast(value_.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, self.__N_batch) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, self.__N_batch, self.__num_threads_in_batch_solve) def get_flat(self, field_: str) -> np.ndarray: @@ -294,7 +316,7 @@ def get_flat(self, field_: str) -> np.ndarray: out = np.ascontiguousarray(np.zeros((self.N_batch, dim,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, self.N_batch*dim, self.__N_batch) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, self.N_batch*dim, self.__N_batch, self.__num_threads_in_batch_solve) return out diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 0c19f6cd8d..4e642995b3 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -143,6 +143,7 @@ def __init__(self): self.__custom_templates = [] self.__custom_update_copy = True self.__num_threads_in_batch_solve: int = 1 + self.__with_batch_functionality: bool = False @property def qp_solver(self): @@ -1215,11 +1216,21 @@ def with_value_sens_wrt_params(self): @property def num_threads_in_batch_solve(self): """ + DEPRECATED, use the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver. Integer indicating how many threads should be used within the batch solve. If more than one thread should be used, the solver is compiled with openmp. Default: 1. """ return self.__num_threads_in_batch_solve + + @property + def with_batch_functionality(self): + """ + Whether the AcadosOcpBatchSolver can be used. + In this case, the solver is compiled with openmp. + Default: False. + """ + return self.__with_batch_functionality @qp_solver.setter @@ -1972,10 +1983,19 @@ def ext_cost_num_hess(self, ext_cost_num_hess): @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): + print("Warning: num_threads_in_batch_solve is deprecated, set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: raise Exception('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') + @with_batch_functionality.setter + def with_batch_functionality(self, with_batch_functionality): + if isinstance(with_batch_functionality, bool): + self.__with_batch_functionality = with_batch_functionality + else: + raise Exception('Invalid with_batch_functionality value. Expected bool.') + + def set(self, attr, value): setattr(self, attr, value) diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 8c83e3dcdd..12348cb694 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -69,6 +69,7 @@ def __init__(self): self.__ext_fun_compile_flags = '-O2' if 'ACADOS_EXT_FUN_COMPILE_FLAGS' not in env else env['ACADOS_EXT_FUN_COMPILE_FLAGS'] self.__ext_fun_expand_dyn = False self.__num_threads_in_batch_solve: int = 1 + self.__with_batch_functionality: bool = False @property def integrator_type(self): @@ -164,12 +165,21 @@ def ext_fun_expand_dyn(self): @property def num_threads_in_batch_solve(self): """ + DEPRECATED, use the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver. Integer indicating how many threads should be used within the batch solve. If more than one thread should be used, the sim solver is compiled with openmp. Default: 1. """ return self.__num_threads_in_batch_solve - + + @property + def with_batch_functionality(self): + """ + Whether the AcadosSimBatchSolver can be used. + In this case, the sim solver is compiled with openmp. + Default: False. + """ + return self.__with_batch_functionality @ext_fun_compile_flags.setter def ext_fun_compile_flags(self, ext_fun_compile_flags): @@ -279,11 +289,19 @@ def sim_method_jac_reuse(self, sim_method_jac_reuse): @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): + print("Warning: num_threads_in_batch_solve is deprecated, set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: raise Exception('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') + @with_batch_functionality.setter + def with_batch_functionality(self, with_batch_functionality): + if isinstance(with_batch_functionality, bool): + self.__with_batch_functionality = with_batch_functionality + else: + raise Exception('Invalid with_batch_functionality value. Expected bool.') + class AcadosSim: """ The class has the following properties that can be modified to formulate a specific simulation problem, see below: diff --git a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py index c487ae6ed1..65bc55d624 100644 --- a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py @@ -31,7 +31,7 @@ from .acados_sim_solver import AcadosSimSolver from .acados_sim import AcadosSim -from typing import List +from typing import List, Union from ctypes import (POINTER, c_int, c_void_p) @@ -49,11 +49,22 @@ class AcadosSimBatchSolver(): __sim_solvers : List[AcadosSimSolver] - def __init__(self, sim: AcadosSim, N_batch: int, json_file: str = 'acados_sim.json', build: bool = True, generate: bool = True, verbose: bool=True): + def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None , json_file: str = 'acados_sim.json', build: bool = True, generate: bool = True, verbose: bool=True): if not isinstance(N_batch, int) or N_batch <= 0: raise Exception("AcadosSimBatchSolver: argument N_batch should be a positive integer.") - + if num_threads_in_batch_solve is None: + num_threads_in_batch_solve = sim.solver_options.num_threads_in_batch_solve + print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in sim.solver_options instead.") + print("In the future, it should be passed explicitly in the AcadosSimBatchSolver constructor.") + if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: + raise Exception("AcadosSimBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") + if not sim.solver_options.with_batch_functionality: + print("Warning: Using AcadosSimBatchSolver, but sim.solver_options.with_batch_functionality is False.") + print("Attempting to compile with openmp nonetheless.") + sim.solver_options.with_batch_functionality = True + + self.__num_threads_in_batch_solve = num_threads_in_batch_solve self.__N_batch = N_batch self.__sim_solvers = [AcadosSimSolver(sim, json_file=json_file, @@ -69,7 +80,7 @@ def __init__(self, sim: AcadosSim, N_batch: int, json_file: str = 'acados_sim.js for i in range(self.N_batch): self.__sim_solvers_pointer[i] = self.sim_solvers[i].capsule - getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve").argtypes = [POINTER(c_void_p), c_int] + getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve").argtypes = [POINTER(c_void_p), c_int, c_int] getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve").restype = c_void_p if not self.sim_solvers[0].acados_lib_uses_omp: @@ -80,7 +91,7 @@ def solve(self): """ Solve the simulation problem with current input for all `N_batch` integrators. """ - getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve")(self.__sim_solvers_pointer, self.__N_batch) + getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve")(self.__sim_solvers_pointer, self.__N_batch, self.__num_threads_in_batch_solve) @property @@ -92,4 +103,13 @@ def sim_solvers(self): def N_batch(self): """Batch size.""" return self.__N_batch + + @property + def num_threads_in_batch_solve(self): + """Number of threads used for parallelizing the batch methods.""" + return self.__num_threads_in_batch_solve + + @num_threads_in_batch_solve.setter + def num_threads_in_batch_solve(self, num_threads_in_batch_solve): + self.__num_threads_in_batch_solve = num_threads_in_batch_solve diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 1412ef384e..64923291dd 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -188,7 +188,7 @@ set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_co {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%} -DACADOS_WITH_QPDUNES {%- endif -%} -{%- if solver_options.num_threads_in_batch_solve > 1 -%} +{%- if solver_options.with_batch_functionality -%} -fopenmp {%- endif -%} ") diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index 7442ba37c3..fbfda16be8 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -172,7 +172,7 @@ CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include {# c-compiler flags #} # define the c-compiler flags for make's implicit rules CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g -{% if solver_options.num_threads_in_batch_solve > 1%} +{% if solver_options.with_batch_functionality %} CFLAGS += -fopenmp {%- endif %} # # Debugging @@ -180,7 +180,7 @@ CFLAGS += -fopenmp # linker flags LDFLAGS+= -L$(LIB_PATH) -{% if solver_options.num_threads_in_batch_solve > 1 %} +{% if solver_options.with_batch_functionality %} LDFLAGS += -fopenmp {%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.c index a41c3ad495..059078be66 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.c @@ -39,7 +39,7 @@ #include #include -{%- if solver_options.num_threads_in_batch_solve > 1 %} +{%- if solver_options.with_batch_functionality %} // openmp #include {%- endif %} @@ -417,25 +417,27 @@ int {{ model.name }}_acados_sim_solve({{ model.name }}_sim_solver_capsule *capsu } -void {{ model.name }}_acados_sim_batch_solve({{ model.name }}_sim_solver_capsule ** capsules, int N_batch) +{% if solver_options.with_batch_functionality %} +void {{ model.name }}_acados_sim_batch_solve({{ model.name }}_sim_solver_capsule ** capsules, int N_batch, int num_threads_in_batch_solve) { -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1){ + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { sim_solve(capsules[i]->acados_sim_solver, capsules[i]->acados_sim_in, capsules[i]->acados_sim_out); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1){ + omp_set_num_threads( num_threads_bkp ); + } return; } - +{%- endif %} int {{ model.name }}_acados_sim_free({{ model.name }}_sim_solver_capsule *capsule) { diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.h b/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.h index 5f6d48d4bb..0d358d8f0a 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.h +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -80,7 +80,9 @@ typedef struct {{ model.name }}_sim_solver_capsule ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_create({{ model.name }}_sim_solver_capsule *capsule); ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solve({{ model.name }}_sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_sim_batch_solve({{ model.name }}_sim_solver_capsule **capsules, int N_batch); +{% if solver_options.with_batch_functionality %} +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_sim_batch_solve({{ model.name }}_sim_solver_capsule **capsules, int N_batch, int num_threads_in_batch_solve); +{% endif %} ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_free({{ model.name }}_sim_solver_capsule *capsule); ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_update_params({{ model.name }}_sim_solver_capsule *capsule, double *value, int np); 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 448ea099e4..5723c23e47 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 @@ -37,10 +37,8 @@ #include "acados_c/ocp_nlp_interface.h" #include "acados_c/external_function_interface.h" -{%- if solver_options.num_threads_in_batch_solve > 1 %} // openmp #include -{%- endif %} // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" @@ -2817,89 +2815,101 @@ int {{ model.name }}_acados_setup_qp_matrices_and_factorize({{ model.name }}_sol -void {{ model.name }}_acados_batch_solve({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch) +{% if solver_options.with_batch_functionality %} +void {{ model.name }}_acados_batch_solve({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch, int num_threads_in_batch_solve) { -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { status_out[i] = ocp_nlp_solve(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->nlp_out); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } -void {{ model.name }}_acados_batch_setup_qp_matrices_and_factorize({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch) +void {{ model.name }}_acados_batch_setup_qp_matrices_and_factorize({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch, int num_threads_in_batch_solve) { -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { status_out[i] = ocp_nlp_setup_qp_matrices_and_factorize(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->nlp_out); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } -void {{ model.name }}_acados_batch_eval_params_jac({{ model.name }}_solver_capsule ** capsules, int N_batch) +void {{ model.name }}_acados_batch_eval_params_jac({{ model.name }}_solver_capsule ** capsules, int N_batch, int num_threads_in_batch_solve) { -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { ocp_nlp_eval_params_jac(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->nlp_out); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } -void {{ model.name }}_acados_batch_eval_solution_sens_adj_p({{ model.name }}_solver_capsule ** capsules, const char *field, int stage, double *out, int offset, int N_batch) +void {{ model.name }}_acados_batch_eval_solution_sens_adj_p({{ model.name }}_solver_capsule ** capsules, const char *field, int stage, double *out, int offset, int N_batch, int num_threads_in_batch_solve) { - -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { ocp_nlp_eval_solution_sens_adj_p(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->sens_out, field, stage, out + i*offset); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } -void {{ model.name }}_acados_batch_set_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch) +void {{ model.name }}_acados_batch_set_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch, int num_threads_in_batch_solve) { int offset = ocp_nlp_dims_get_total_from_attr(capsules[0]->nlp_solver->config, capsules[0]->nlp_solver->dims, capsules[0]->nlp_out, field); @@ -2909,26 +2919,29 @@ void {{ model.name }}_acados_batch_set_flat({{ model.name }}_solver_capsule ** c exit(1); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { ocp_nlp_set_all(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->nlp_out, field, data + i * offset); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } -void {{ model.name }}_acados_batch_get_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch) +void {{ model.name }}_acados_batch_get_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch, int num_threads_in_batch_solve) { int offset = ocp_nlp_dims_get_total_from_attr(capsules[0]->nlp_solver->config, capsules[0]->nlp_solver->dims, capsules[0]->nlp_out, field); @@ -2937,23 +2950,26 @@ void {{ model.name }}_acados_batch_get_flat({{ model.name }}_solver_capsule ** c printf("batch_get_flat: wrong input dimension, expected %d, got %d\n", N_batch*offset, N_data); exit(1); } - -{% if solver_options.num_threads_in_batch_solve > 1 %} - int num_threads_bkp = omp_get_num_threads(); - omp_set_num_threads({{ solver_options.num_threads_in_batch_solve }}); + int num_threads_bkp; + if (num_threads_in_batch_solve > 1) + { + num_threads_bkp = omp_get_num_threads(); + omp_set_num_threads(num_threads_in_batch_solve); + } #pragma omp parallel for -{%- endif %} for (int i = 0; i < N_batch; i++) { ocp_nlp_get_all(capsules[i]->nlp_solver, capsules[i]->nlp_in, capsules[i]->nlp_out, field, data + i * offset); } -{% if solver_options.num_threads_in_batch_solve > 1 %} - omp_set_num_threads( num_threads_bkp ); -{%- endif %} + if (num_threads_in_batch_solve > 1) + { + omp_set_num_threads( num_threads_bkp ); + } return; } +{% endif %} int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h index 6c29317adb..4ce6b8f2e2 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.h @@ -295,14 +295,15 @@ ACADOS_SYMBOL_EXPORT int {{ name }}_acados_set_p_global_and_precompute_dependenc ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_setup_qp_matrices_and_factorize({{ model.name }}_solver_capsule* capsule); -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_solve({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch); +{% if solver_options.with_batch_functionality %} +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_solve({{ model.name }}_solver_capsule ** capsules, int * status_out, int N_batch, int num_threads_in_batch_solve); -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_set_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch); -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_get_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch); - -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_eval_solution_sens_adj_p({{ model.name }}_solver_capsule ** capsules, const char *field, int stage, double *out, int offset, int N_batch); -ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_eval_params_jac({{ model.name }}_solver_capsule ** capsules, int N_batch); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_set_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch, int num_threads_in_batch_solve); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_get_flat({{ model.name }}_solver_capsule ** capsules, const char *field, double *data, int N_data, int N_batch, int num_threads_in_batch_solve); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_eval_solution_sens_adj_p({{ model.name }}_solver_capsule ** capsules, const char *field, int stage, double *out, int offset, int N_batch, int num_threads_in_batch_solve); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_batch_eval_params_jac({{ model.name }}_solver_capsule ** capsules, int N_batch, int num_threads_in_batch_solve); +{% endif %} ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule); ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule); diff --git a/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in index f6d80590a9..0656d547be 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in @@ -300,7 +300,7 @@ CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include # define the c-compiler flags for make's implicit rules CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g -{% if solver_options.num_threads_in_batch_solve > 1 %} +{% if solver_options.with_batch_functionality %} CFLAGS += -fopenmp {%- endif %} # # Debugging @@ -308,7 +308,7 @@ CFLAGS += -fopenmp # linker flags LDFLAGS+= -L$(LIB_PATH) -{% if solver_options.num_threads_in_batch_solve > 1 %} +{% if solver_options.with_batch_functionality %} LDFLAGS += -fopenmp {%- endif %} From 1ae06bd173c91e2fed3d6e19c5ca6354124e606c Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 3 Apr 2025 12:55:33 +0200 Subject: [PATCH 014/164] Fix Github Action errors (#1487) Our CI workflows were failing, since the ubuntu automatically installs CMake 4.0. This version throws an error when `cmake_minimum_required(VERSION 2.8.11)` is set. It can be mitigated by compiling with `-DCMAKE_POLICY_VERSION_MINIMUM=3.5`, but already prints a deprecation warning even then, saying we should directly go to 3.10. I am not sure if people actually use very old cmake versions, but updating the version requirement would also be an option at some point. An alternative fix is in https://github.com/acados/acados/commit/1e2640e457debf69e8efb39b12c5b627c5a78381 which changes `cmake_minimum_required(VERSION 2.8.11)` to `cmake_minimum_required(VERSION 2.8.11...4.0)` for the relevant submodules. However, it is not clear to me what the effects are. In particular, I think this would cause issues with cmake > 4.0. The documentation did not fully clarify directly: https://cmake.org/cmake/help/latest/command/cmake_minimum_required.html --------- Co-authored-by: sandmaennchen --- .../workflows/c_test_blasfeo_reference.yml | 2 +- .github/workflows/codeql-buildscript.sh | 2 +- .github/workflows/ext_dep_off.yml | 2 +- .github/workflows/full_build.yml | 34 +++++++++++++------ .../acados_install_windows.m | 11 ++++-- 5 files changed, 35 insertions(+), 16 deletions(-) diff --git a/.github/workflows/c_test_blasfeo_reference.yml b/.github/workflows/c_test_blasfeo_reference.yml index ed8622203f..5a485b71e9 100644 --- a/.github/workflows/c_test_blasfeo_reference.yml +++ b/.github/workflows/c_test_blasfeo_reference.yml @@ -43,7 +43,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DCMAKE_POLICY_VERSION_MINIMUM=3.5 - name: Build & Install working-directory: ${{runner.workspace}}/build diff --git a/.github/workflows/codeql-buildscript.sh b/.github/workflows/codeql-buildscript.sh index 5cb79ed318..128071571d 100644 --- a/.github/workflows/codeql-buildscript.sh +++ b/.github/workflows/codeql-buildscript.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash mkdir build && cd build -cmake ../ +cmake ../ -DCMAKE_POLICY_VERSION_MINIMUM=3.5 make diff --git a/.github/workflows/ext_dep_off.yml b/.github/workflows/ext_dep_off.yml index 43573b9dc9..5c553df5a3 100644 --- a/.github/workflows/ext_dep_off.yml +++ b/.github/workflows/ext_dep_off.yml @@ -43,7 +43,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DEXT_DEP=OFF -DBLASFEO_EXAMPLES=OFF + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DEXT_DEP=OFF -DBLASFEO_EXAMPLES=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 - name: Build & Install working-directory: ${{runner.workspace}}/build diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 5668734052..59b63508d1 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -35,7 +35,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 -DCMAKE_POLICY_VERSION_MINIMUM=3.5 - name: Build & Install working-directory: ${{runner.workspace}}/acados/build @@ -144,10 +146,10 @@ jobs: shell: bash run: ${{runner.workspace}}/acados/.github/linux/install_simde.sh'' - - name: Install new CasADi Python - working-directory: ${{runner.workspace}}/acados - shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_new_casadi_python.sh'' + # - name: Install new CasADi Python + # working-directory: ${{runner.workspace}}/acados + # shell: bash + # run: ${{runner.workspace}}/acados/.github/linux/install_new_casadi_python.sh'' - name: Prepare Octave working-directory: ${{runner.workspace}}/acados/external @@ -162,7 +164,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Export Paths for octave working-directory: ${{runner.workspace}}/acados @@ -257,7 +261,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -314,7 +320,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -373,7 +381,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -426,7 +436,9 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + run: | + cmake --version + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Run Simulink closed-loop test uses: matlab-actions/run-command@v2 @@ -502,7 +514,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE - name: Run CMake Octave tests (ctest) working-directory: ${{runner.workspace}}/acados/build diff --git a/interfaces/acados_matlab_octave/acados_install_windows.m b/interfaces/acados_matlab_octave/acados_install_windows.m index 71fb7900a9..0019e6fe8c 100644 --- a/interfaces/acados_matlab_octave/acados_install_windows.m +++ b/interfaces/acados_matlab_octave/acados_install_windows.m @@ -6,7 +6,7 @@ function acados_install_windows(varargin) switch(nargin) case 0 - cmakeConfigString='-DBUILD_SHARED_LIBS=OFF -DACADOS_WITH_OSQP=OFF'; + cmakeConfigString='-DBUILD_SHARED_LIBS=OFF -DACADOS_WITH_OSQP=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5'; case 1 cmakeConfigString=varargin{1}; otherwise @@ -21,6 +21,8 @@ function acados_install_windows(varargin) [folderPath,~,~]=fileparts(folderPath); [acadosPath,~,~]=fileparts(folderPath); + % backward to forward slashes + acadosPath = strrep(acadosPath, '\', '/'); acadosBuildPath=fullfile(acadosPath,'build'); %% Acados installation instructions: @@ -46,6 +48,8 @@ function acados_install_windows(varargin) mkdir(acadosBuildPath); cd(acadosBuildPath); + disp(['ACADOS_INSTALL_DIR is' acadosPath]); + %% Acados installation instructions: % cmake.exe -G "MinGW Makefiles" -DACADOS_INSTALL_DIR="$ACADOS_INSTALL_DIR" -DBUILD_SHARED_LIBS=OFF -DACADOS_WITH_OSQP=ON .. % # useful options to add above: @@ -53,10 +57,13 @@ function acados_install_windows(varargin) % # -DBLASFEO_TARGET=GENERIC -DHPIPM_TARGET=GENERIC % # NOTE: check the output of cmake: -- Installation directory: should be , % # if this is not the case, set -DACADOS_INSTALL_DIR= explicitly above. - fprintf('Executing cmake configuration\n'); % Command slightly modified to work with CMD instead of PowerShell cmake_cmd = sprintf('cmake.exe -G "MinGW Makefiles" -DACADOS_INSTALL_DIR=%%ACADOS_INSTALL_DIR%% %s ..',cmakeConfigString); + % print cmake command + fprintf('Executing cmake configuration\n'); + disp(cmake_cmd); + % Execute cmake command status=system(cmake_cmd); if (status~=0) error('cmake command failed. Command was:\n%s\n', cmake_cmd); From 0bc28c2753a0fd62d00b91dbca5b4f1429c15eb6 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 3 Apr 2025 14:08:45 +0200 Subject: [PATCH 015/164] Update CasADi version to 3.7 (#1485) We are excited about the latest CasADi 3.7 release :tada: A few of the latest features are already interfaced in acados, which makes CasADi >= 3.7 a strict requirement for some acados features. Thus, this PR: - Updates the version checks, warning on CasADi < 3.7, as not all acados features are supported then. - Updates the install scripts to CasADi 3.7, MATLAB install script, (Python automatically picks the latest CasADi) --- .github/linux/install_new_casadi_matlab.sh | 2 +- .github/linux/install_new_casadi_octave.sh | 2 +- .github/linux/install_new_casadi_python.sh | 4 +- .../check_casadi_version.m | 8 +++- .../acados_template/acados_template/utils.py | 43 ++++++------------- 5 files changed, 22 insertions(+), 37 deletions(-) diff --git a/.github/linux/install_new_casadi_matlab.sh b/.github/linux/install_new_casadi_matlab.sh index 33fbc21478..936253935b 100755 --- a/.github/linux/install_new_casadi_matlab.sh +++ b/.github/linux/install_new_casadi_matlab.sh @@ -30,7 +30,7 @@ # -CASADI_MATLAB_URL="https://github.com/casadi/casadi/releases/download/nightly-main/casadi-main-linux64-matlab2018b.zip"; +CASADI_MATLAB_URL="https://github.com/casadi/casadi/releases/download/3.7.0/casadi-3.7.0-linux64-matlab2018b.zip"; wget -O casadi-linux-matlab.zip "${CASADI_MATLAB_URL}"; mkdir -p casadi-matlab; diff --git a/.github/linux/install_new_casadi_octave.sh b/.github/linux/install_new_casadi_octave.sh index f43412ba62..69251e028b 100755 --- a/.github/linux/install_new_casadi_octave.sh +++ b/.github/linux/install_new_casadi_octave.sh @@ -30,7 +30,7 @@ # echo "Installing CasADi for Octave"; -CASADI_OCTAVE_URL="https://github.com/casadi/casadi/releases/download/nightly-main/casadi-main-linux64-octave7.3.0.zip"; +CASADI_OCTAVE_URL="https://github.com/casadi/casadi/releases/download/3.7.0/casadi-3.7.0-linux64-octave7.3.0.zip"; wget -O casadi-linux-octave.zip "${CASADI_OCTAVE_URL}"; mkdir -p casadi-octave; diff --git a/.github/linux/install_new_casadi_python.sh b/.github/linux/install_new_casadi_python.sh index e0c0e79076..69f6c1fa72 100755 --- a/.github/linux/install_new_casadi_python.sh +++ b/.github/linux/install_new_casadi_python.sh @@ -33,8 +33,8 @@ source acadosenv/bin/activate; which python; -curl -L --remote-name https://github.com/casadi/casadi/releases/download/nightly-main/casadi-3.6.7.dev+main-cp310-none-manylinux2014_x86_64.whl; -pip install casadi-3.6.7.dev+main-cp310-none-manylinux2014_x86_64.whl; +curl -L --remote-name https://github.com/casadi/casadi/releases/download/3.7.0/casadi-3.7.0-cp310-none-manylinux2014_x86_64.whl; +pip install casadi-3.7.0-cp310-none-manylinux2014_x86_64.whl; diff --git a/interfaces/acados_matlab_octave/check_casadi_version.m b/interfaces/acados_matlab_octave/check_casadi_version.m index 92fdb3e5f6..61c98b4730 100644 --- a/interfaces/acados_matlab_octave/check_casadi_version.m +++ b/interfaces/acados_matlab_octave/check_casadi_version.m @@ -31,7 +31,11 @@ function check_casadi_version() import casadi.* casadi_version = CasadiMeta.version(); - if ~(strcmp(casadi_version(1:3),'3.4') || strcmp(casadi_version(1:3),'3.5') || strcmp(casadi_version(1:3),'3.6')) - warning('Tested CasADi versions are 3.4, 3.5 and 3.6 you are using: %s.', casadi_version); + if ~(strcmp(casadi_version(1:3),'3.7')) + if ~(strcmp(casadi_version(1:3),'3.4') || strcmp(casadi_version(1:3),'3.5') || strcmp(casadi_version(1:3),'3.6')) + warning('Tested CasADi versions are 3.4, 3.5, 3.6, 3.7 you are using: %s.', casadi_version); + else + warning('Recommended CasADi version to be used in acados is 3.7 for a full featured experience. Versions 3.4 to 3.6 work for most problem formulations. You are using %s.', casadi_version); + end end end diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 68888972ab..cc4d9b7a31 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -47,28 +47,6 @@ from contextlib import contextmanager - - -ALLOWED_CASADI_VERSIONS = ( - '3.4.0', - '3.4.5', - '3.5.1', - '3.5.2', - '3.5.3', - '3.5.4', - '3.5.6', - '3.5.5', - '3.6.0', - '3.6.1', - '3.6.2', - '3.6.3', - '3.6.4', - '3.6.5', - '3.6.6', - '3.6.7', - '3.6.7+', -) - TERA_VERSION = "0.0.34" PLATFORM2TERA = { @@ -150,15 +128,18 @@ def get_shared_lib(shared_lib_name: str, winmode = None) -> DllLoader: def check_casadi_version(): casadi_version = CasadiMeta.version() - if casadi_version in ALLOWED_CASADI_VERSIONS: - return - else: - msg = 'Warning: Please note that the following versions of CasADi are ' - msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) - msg += 'If there is an incompatibility with the CasADi generated code, ' - msg += 'please consider changing your CasADi version.\n' - msg += 'Version {} currently in use.'.format(casadi_version) - print(msg) + major_minor = casadi_version.split('.') + major = int(major_minor[0]) + minor = int(major_minor[1]) + if major < 3 or (major == 3 and minor < 4): # < 3.4 + raise Exception(f'CasADi version {casadi_version} is not supported. ' + 'Please use a version >= 3.4.0.') + + if major > 3 or (major == 3 and minor > 7): # >= 3.7 + print(f"Warning: CasADi version {casadi_version} is not tested with acados yet.") + elif major == 3 and minor < 7: + print(f"Warning: Full featured acados requires CasADi version >= 3.7, got {casadi_version}.") + def check_casadi_version_supports_p_global(): try: From d3d7f472a8bcbe1c841706f796888f7a103e7edd Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 3 Apr 2025 14:32:27 +0200 Subject: [PATCH 016/164] Update automatic CasADi matlab install (#1488) Forgot to push this commit in https://github.com/acados/acados/pull/1485 --- interfaces/acados_matlab_octave/check_acados_requirements.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_matlab_octave/check_acados_requirements.m b/interfaces/acados_matlab_octave/check_acados_requirements.m index 0fbadf24f7..9320aa536c 100644 --- a/interfaces/acados_matlab_octave/check_acados_requirements.m +++ b/interfaces/acados_matlab_octave/check_acados_requirements.m @@ -36,10 +36,10 @@ function check_acados_requirements(varargin) error('Please set up CasADi yourself and try again.'); end % download CasADi - CasADi_version = 'main'; % NOTE: this needs to be set/updated manually - CasADi_release = 'nightly-main'; % NOTE: this needs to be set/updated manually - later_than_36 = true; % NOTE: this needs to be set/updated manually + CasADi_version = '3.7.0'; % NOTE: this needs to be set/updated manually + CasADi_release = '3.7.0'; % NOTE: this needs to be set/updated manually + later_than_36 = true; % NOTE: this needs to be set/updated manually url = strcat('https://github.com/casadi/casadi/releases/download/',... CasADi_release, '/'); external_folder = fullfile(acados_dir, 'external'); From 563c27835f0244273e6122c5b402c7d193dff339 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 4 Apr 2025 13:03:19 +0200 Subject: [PATCH 017/164] Update solution sensitivity examples (#1489) Co-authored-by: sandmaennchen --- .gitignore | 3 + .../acados_python/chain_mass/plot_utils.py | 58 +++++++++++++------ .../solution_sensitivity_example.py | 17 +++--- examples/acados_python/chain_mass/utils.py | 1 + .../sensitivity_utils.py | 33 +++++++---- .../smooth_policy_gradients.py | 35 +++++++---- .../non_ocp_example.py | 16 +++-- .../acados_template/acados_ocp_solver.py | 16 +++-- 8 files changed, 122 insertions(+), 57 deletions(-) diff --git a/.gitignore b/.gitignore index f23866c3b9..eebbf8b88a 100644 --- a/.gitignore +++ b/.gitignore @@ -53,6 +53,7 @@ Thumbs.db *.synctex *.fdb_latexmk *.fls +*.pdf # dSpace # ########## @@ -131,3 +132,5 @@ qp_in*.txt qp_out*.txt figures +examples/**/*.pdf +examples/**/*.png diff --git a/examples/acados_python/chain_mass/plot_utils.py b/examples/acados_python/chain_mass/plot_utils.py index 67965db9f2..27726b0849 100644 --- a/examples/acados_python/chain_mass/plot_utils.py +++ b/examples/acados_python/chain_mass/plot_utils.py @@ -36,34 +36,56 @@ latexify_plot() -def plot_timings(results_list, labels, figure_filename=None, t_max=None): +def plot_timings(results_list, labels, figure_filename=None, t_max=None, horizontal=False, figsize=None, with_patterns=False): num_entries = len(labels) if num_entries != len(results_list): raise ValueError("Number of labels and result files do not match") - width = 0.8 - fig, ax = plt.subplots(figsize=(7.5, 6)) + if figsize is None: + figsize = (7.5, 6) + bottom = np.zeros(num_entries) colors = ["C0", "C1", "C4", "C3", "C6", "C5", "C2", "C7"] + patterns = [ "/" , "\\" , "o" , "|" , "-" , "x", None, "o", "O", ".", "*" ] + fig, ax = plt.subplots(figsize=figsize) + if not horizontal: + width = 0.8 + + for i, k in enumerate(results_list[0].keys()): + vals = [np.mean(res_dict[k]) for res_dict in results_list] + plt.bar(labels, vals, width, label=k, bottom=bottom, color=colors[i], hatch=patterns[i] if with_patterns else None) + bottom += vals + if t_max is not None: + plt.ylim(0, t_max) + for i in range(bottom.size): + if bottom[i] > t_max: + plt.text(i, 0.95 * t_max, f"{bottom[i]:.1f}", ha="center", va="bottom") + + plt.xticks(rotation=10) + plt.grid(axis="y") + plt.ylabel("mean computation time [ms]") + ax.legend() + else: + width = 0.8 + for i, k in enumerate(results_list[0].keys()): + vals = [np.mean(res_dict[k]) for res_dict in results_list] + plt.barh(labels, vals, width, label=k, left=bottom, color=colors[i], hatch=patterns[i] if with_patterns else None) + bottom += vals + if t_max is not None: + plt.xlim(0, t_max) + for i in range(bottom.size): + if bottom[i] > t_max: + plt.text(0.95 * t_max, i, f"{bottom[i]:.1f}", ha="right", va="center") + + plt.grid(axis="x") + plt.xlabel("mean computation time [ms]") + ax.legend(ncol=2) + - for i, k in enumerate(results_list[0].keys()): - vals = [np.mean(res_dict[k]) for res_dict in results_list] - plt.bar(labels, vals, width, label=k, bottom=bottom, color=colors[i]) - bottom += vals - if t_max is not None: - plt.ylim(0, t_max) - for i in range(bottom.size): - if bottom[i] > t_max: - plt.text(i, 0.95 * t_max, f"{bottom[i]:.1f}", ha="center", va="bottom") - - plt.xticks(rotation=10) - plt.grid(axis="y") - plt.ylabel("mean computation time [ms]") # tight layout plt.tight_layout() - ax.legend() if figure_filename is not None: - plt.savefig(figure_filename) + plt.savefig(figure_filename, dpi=600) print(f"Saved figure to {figure_filename}") plt.show() diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index f273d241b4..b1198a147f 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -503,30 +503,30 @@ def main_parametric(qp_solver_ric_alg: int = 0, chain_params_: dict = get_chain_ assert np.allclose(sens_adj, out_dict['sens_u']) timings_common = { - "NLP solve": timings_solve_ocp_solver * 1e3, + "NLP solve (S1)": timings_solve_ocp_solver * 1e3, "store \& load iterates": timings_store_load * 1e3, "parameter update": timings_parameter_update * 1e3, - "setup exact Lagrange Hessian": timings_lin_exact_hessian_qp * 1e3, - "factorize exact Lagrange Hessian": timings_lin_and_factorize * 1e3, - r"evaluate $J$": timings_lin_params * 1e3, + "setup exact Lagrange Hessian (S2)": timings_lin_exact_hessian_qp * 1e3, + "factorize exact Lagrange Hessian (S3)": timings_lin_and_factorize * 1e3, + r"evaluate $J_\star$ (S4)": timings_lin_params * 1e3, } timing_results_forward = timings_common.copy() timing_results_adjoint = timings_common.copy() timing_results_adj_uforw = timings_common.copy() timing_results_adj_all_primals = timings_common.copy() - backsolve_label = "sensitivity solve given factorization" + backsolve_label = "sensitivity solve given factorization (S5)" timing_results_forward[backsolve_label] = timings_solve_params * 1e3 timing_results_adjoint[backsolve_label] = timings_solve_params_adj * 1e3 timings_list = [timing_results_forward, timing_results_adjoint] - labels = ['forward', 'adjoint'] + labels = [r'$\frac{\partial w^\star}{\partial \theta}$ via forward', r'$\nu^\top \frac{\partial w^\star}{\partial \theta}$ via adjoint'] if with_more_adjoints: timing_results_adj_uforw[backsolve_label] = timings_solve_params_adj_uforw * 1e3 timing_results_adj_all_primals[backsolve_label] = timings_solve_params_adj_all_primals * 1e3 timings_list += [timing_results_adj_uforw, timing_results_adj_all_primals] - labels += [r'$\frac{\partial u_0}{\partial \theta}$ via adjoints', r'$\frac{\partial z}{\partial \theta} $ via adjoints'] + labels += [r'$\frac{\partial u_0^\star}{\partial \theta}$ via adjoints', r'$\frac{\partial z^\star}{\partial \theta} $ via adjoints'] print_timings(timing_results_forward, metric="median") @@ -543,6 +543,7 @@ def main_parametric(qp_solver_ric_alg: int = 0, chain_params_: dict = get_chain_ u_opt_reconstructed_acados = np.cumsum(sens_u, axis=0) * delta_p + u_opt[0, :] u_opt_reconstructed_acados += u_opt[0, :] - u_opt_reconstructed_acados[0, :] + # TODO move to plot utils plt.figure(figsize=(7, 7)) for col in range(3): plt.subplot(4, 1, col + 1) @@ -569,7 +570,7 @@ def main_parametric(qp_solver_ric_alg: int = 0, chain_params_: dict = get_chain_ plt.tight_layout() plt.savefig("chain_adj_fwd_sens.pdf") - plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.pdf", t_max=10) + plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.png", t_max=10, horizontal=True, figsize=(12, 3), with_patterns=True) plt.show() diff --git a/examples/acados_python/chain_mass/utils.py b/examples/acados_python/chain_mass/utils.py index c4dbb477d9..5d1b6cda11 100755 --- a/examples/acados_python/chain_mass/utils.py +++ b/examples/acados_python/chain_mass/utils.py @@ -32,6 +32,7 @@ from export_chain_mass_model import export_chain_mass_model +# TODO this should be a dataclass def get_chain_params(): params = dict() diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py index 33a3015589..a8b8da621a 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py @@ -173,10 +173,10 @@ def export_parametric_ocp( ocp.solver_options.tol = 1e-8 # ocp.solver_options.globalization = "MERIT_BACKTRACKING" - if hessian_approx == 'EXACT': + # if hessian_approx == 'EXACT': # sensitivity solver settings! - ocp.solver_options.with_solution_sens_wrt_params = True - ocp.solver_options.with_value_sens_wrt_params = True + ocp.solver_options.with_solution_sens_wrt_params = True + ocp.solver_options.with_value_sens_wrt_params = True return ocp @@ -325,6 +325,7 @@ def plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi multipliers_bu=None, multipliers_h=None, figsize=None, fig_filename=None, + horizontal_plot=False, ): nsub = 2 @@ -335,9 +336,10 @@ def plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi if figsize is None: figsize = (9, 9) - _, ax = plt.subplots(nrows=nsub, ncols=1, sharex=True, figsize=figsize) - ax[0].set_xlim([p_test[0], p_test[-1]]) - + if not horizontal_plot: + _, ax = plt.subplots(nrows=nsub, ncols=1, sharex=True, figsize=figsize) + else: + _, ax = plt.subplots(nrows=1, ncols=nsub, sharex=False, figsize=figsize) linestyles = ["-", "--", "-.", ":", "-", "--", "-.", ":"] @@ -347,16 +349,20 @@ def plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi ax[isub].set_ylabel(r"$u_0$") if title is not None: ax[isub].set_title(title) + ax[isub].legend() + ax[isub].legend(handlelength=1.2) isub += 1 for i, (sens_pi, label) in enumerate(sens_pi_label_pairs): ax[isub].plot(p_test, sens_pi, label=label, linestyle=linestyles[i]) ax[isub].set_ylabel(r"$\partial_\theta u_0$") + if horizontal_plot: + ax[isub].legend(loc = 'upper left', handlelength=1.2, ncol=2, columnspacing=0.5, labelspacing=0.2) + else: + ax[isub].legend(loc = 'upper left', handlelength=1.2) if with_multiplier_subplot: isub += 1 - isub_multipliers = isub - legend_elements = [] if multipliers_bu is not None: for lam in multipliers_bu: @@ -366,14 +372,19 @@ def plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi for lam in multipliers_h: ax[isub].plot(p_test, lam, linestyle='--', color='C1', alpha=.6) legend_elements += [plt.Line2D([0], [0], color='C1', linestyle='--', label='multipliers $h$')] - ax[isub].legend(handles=legend_elements, ncol=2) + if horizontal_plot: + ax[isub].legend(handles=legend_elements, ncol=1, handlelength=1.2) + else: + ax[isub].legend(handles=legend_elements, ncol=2) ax[isub].set_ylim([0, 14]) ax[isub].set_ylabel("multipliers") for isub in range(nsub): ax[isub].grid() - if isub != isub_multipliers: - ax[isub].legend() + ax[isub].set_xlim([p_test[0], p_test[-1]]) + + if horizontal_plot: + ax[isub].set_xlabel(f"{parameter_name}") ax[-1].set_xlabel(f"{parameter_name}") diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py index 9ff71be9ac..e44bc8acd3 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py @@ -42,7 +42,7 @@ Fmax = 80.0 -def solve_ocp_and_compute_sens(ocp_solver: AcadosOcpSolver, sensitivity_solver: AcadosOcpSolver, p_test, x0, tau_min): +def solve_ocp_and_compute_sens(ocp_solver: AcadosOcpSolver, sensitivity_solver: AcadosOcpSolver, p_test, x0, tau_min, sanity_checks=True): ocp_solver.options_set('tau_min', tau_min) sensitivity_solver.options_set('tau_min', tau_min) @@ -62,7 +62,7 @@ def solve_ocp_and_compute_sens(ocp_solver: AcadosOcpSolver, sensitivity_solver: sensitivity_solver.set_p_global_and_precompute_dependencies(p_val) u_opt[i] = ocp_solver.solve_for_x0(x0, fail_on_nonzero_status=False)[0] status = ocp_solver.get_status() - ocp_solver.print_statistics() + # ocp_solver.print_statistics() if status != 0: ocp_solver.print_statistics() print(f"Solver failed with status {status} for {i}th parameter value {p} and {tau_min=}.") @@ -84,7 +84,7 @@ def solve_ocp_and_compute_sens(ocp_solver: AcadosOcpSolver, sensitivity_solver: print(f"sensitivity solver returned status {sensitivity_solver.get_status()}.") # breakpoint() # Calculate the policy gradient - out_dict = sensitivity_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=False) + out_dict = sensitivity_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=False, sanity_checks=sanity_checks) sens_u[i] = out_dict['sens_u'].item() return u_opt, sens_u, lambda_flat @@ -121,12 +121,12 @@ def main_parametric(qp_solver_ric_alg: int, use_cython=False, plot_trajectory=Fa """ x0 = np.array([0.0, np.pi / 2, 0.0, 0.0]) - delta_p = 0.0002 + delta_p = 0.001 # p_nominal = 1.0 # p_test = np.arange(p_nominal + 0.1, p_nominal + 0.5, delta_p) - p_test = np.arange(1.0, 1.4, delta_p) + p_test = np.arange(1.05, 1.4+delta_p, delta_p) - ocp_solver, sensitivity_solver = create_solvers(x0, use_cython=use_cython, qp_solver_ric_alg=qp_solver_ric_alg) + ocp_solver, sensitivity_solver = create_solvers(x0, use_cython=use_cython, qp_solver_ric_alg=qp_solver_ric_alg,) # verbose=False, build=False, generate=False) ocp = ocp_solver.acados_ocp # compute policy and its gradient @@ -182,14 +182,27 @@ def main_parametric(qp_solver_ric_alg: int, use_cython=False, plot_trajectory=Fa pi_label_pairs.append((u_opt, label)) sens_pi_label_pairs.append((sens_u, label)) - sens_pi_label_pairs.append((sens_u_fd, 'finite differences')) + sens_pi_label_pairs.append((sens_u_fd, 'finite diff.')) + + # without 2-solver approach + tau_min = 1e-6 + u_opt, sens_u, _ = solve_ocp_and_compute_sens(ocp_solver, ocp_solver, p_test, x0, tau_min=tau_min, sanity_checks=False) + label = r"IFT approx. Hess." + # pi_label_pairs.append((u_opt, label)) + sens_pi_label_pairs.append((sens_u, label)) # plot + # plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi_label_pairs, title=None, parameter_name=r"$\theta$", + # multipliers_bu=multipliers_bu, multipliers_h=multipliers_h, + # figsize=(7, 9), + # fig_filename="smoothed_solution_sensitivities.pdf", + # ) plot_smoothed_solution_sensitivities_results(p_test, pi_label_pairs, sens_pi_label_pairs, title=None, parameter_name=r"$\theta$", - multipliers_bu=multipliers_bu, multipliers_h=multipliers_h, - figsize=(7, 9), - fig_filename="smoothed_solution_sensitivities.pdf", - ) + multipliers_bu=multipliers_bu, multipliers_h=multipliers_h, + figsize=(12, 3.2), + fig_filename="smoothed_solution_sensitivities_horizontal.pdf", + horizontal_plot=True, + ) if plot_trajectory: plot_pendulum_traj_from_ocp_iterate(ocp_solver) diff --git a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py index fdef066892..f208fff186 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py @@ -83,7 +83,7 @@ def solve_and_compute_sens(p_test, tau): ocp_solver.set_p_global_and_precompute_dependencies(p_val) status = ocp_solver.solve() - solution[i] = ocp_solver.get(0, "x") + solution[i] = ocp_solver.get(0, "x")[0] if status != 0: ocp_solver.print_statistics() @@ -129,15 +129,19 @@ def main(): plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, title=None, parameter_name=r"$\theta$", fig_filename="solution_sens_non_ocp.pdf") + plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, + title=None, parameter_name=r"$\theta$", fig_filename="solution_sens_non_ocp_transposed.pdf", horizontal_plot=True) - -def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, title=None, parameter_name="", fig_filename=None): +def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, title=None, parameter_name="", fig_filename=None, horizontal_plot=False): p_min = p_test[0] p_max = p_test[-1] linestyles = ["--", "-.", "--", ":", "-.", ":"] nsub = 2 - _, ax = plt.subplots(nrows=nsub, ncols=1, sharex=True, figsize=(6.5,5)) + if horizontal_plot: + _, ax = plt.subplots(nrows=1, ncols=nsub, sharex=False, figsize=(12, 3.0)) + else: + _, ax = plt.subplots(nrows=nsub, ncols=1, sharex=True, figsize=(6.5,5)) isub = 0 # plot analytic solution @@ -171,6 +175,8 @@ def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list for i in range(nsub): ax[i].grid(True) + if horizontal_plot: + ax[i].set_xlabel(f"{parameter_name}") ax[-1].set_xlabel(f"{parameter_name}") plt.tight_layout() @@ -182,4 +188,6 @@ def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list if __name__ == "__main__": main() + + # to plot only analytic solution # plot_solution_sensitivities_results([-2, 2], [], [], [], parameter_name=r"$\theta$", fig_filename="solution_sens_non_ocp_analytic.pdf") diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 5e1af14374..1b6bcad28d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -715,6 +715,7 @@ def eval_solution_sensitivity(self, return_sens_lam: bool = False, return_sens_su: bool = False, return_sens_sl: bool = False, + sanity_checks: bool = True, ) \ -> Dict: """ @@ -728,6 +729,8 @@ def eval_solution_sensitivity(self, :param return_sens_lam: Flag indicating whether sensitivities of lam should be returned. Default: False. :param return_sens_su: Flag indicating whether sensitivities of su should be returned. Default: False. :param return_sens_sl: Flag indicating whether sensitivities of sl should be returned. Default: False. + :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True + :returns: A dictionary with the solution sensitivities with fields sens_x, sens_u, sens_pi, sens_lam, sens_su, sens_sl if corresponding flags were set. If stages is a list, sens_x, sens_lam, sens_su, sens_sl is a list of the same length. For sens_u, sens_pi, the list has length len(stages) or len(stages)-1 depending on whether N is included or not. @@ -763,21 +766,24 @@ def eval_solution_sensitivity(self, sens_sl = [] sens_su = [] - for s in stages_: - if not isinstance(s, int) or s < 0 or s > self.N: - raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or list[int] and in [0, N], got stages = {stages_}.") + if sanity_checks: + for s in stages_: + if not isinstance(s, int) or s < 0 or s > self.N: + raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or list[int] and in [0, N], got stages = {stages_}.") if with_respect_to == "initial_state": nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) ngrad = nx field = "ex" - self._sanity_check_solution_sensitivities(parametric=False) + if sanity_checks: + self._sanity_check_solution_sensitivities(parametric=False) elif with_respect_to == "p_global": np_global = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "p_global".encode('utf-8')) ngrad = np_global field = "p_global" - self._sanity_check_solution_sensitivities() + if sanity_checks: + self._sanity_check_solution_sensitivities() # compute jacobians wrt params in all modules t0 = time.time() From f135546a222c4d9052f6f68e79894d282bec0b72 Mon Sep 17 00:00:00 2001 From: "LI, Jinjie" <45286479+Li-Jinjie@users.noreply.github.com> Date: Sun, 6 Apr 2025 22:09:47 +0900 Subject: [PATCH 018/164] Improving Exception Handling in acados Python Template (#1478) Raise specific errors instead of general Exceptions. --- .../acados_template/acados_dims.py | 279 ++++++--------- .../acados_template/acados_model.py | 24 +- .../acados_template/acados_multiphase_ocp.py | 32 +- .../acados_template/acados_ocp.py | 336 +++++++++--------- .../acados_ocp_batch_solver.py | 32 +- .../acados_template/acados_ocp_constraints.py | 36 +- .../acados_template/acados_ocp_cost.py | 20 +- .../acados_template/acados_ocp_iterate.py | 4 +- .../acados_template/acados_ocp_options.py | 194 +++++----- .../acados_template/acados_ocp_solver.py | 202 +++++------ .../acados_template/acados_ocp_solver_pyx.pyx | 58 +-- .../acados_template/acados_sim.py | 38 +- .../acados_sim_batch_solver.py | 4 +- .../acados_template/acados_sim_solver.py | 12 +- .../acados_template/acados_sim_solver_pyx.pyx | 10 +- .../casadi_function_generation.py | 8 +- .../gnsf/detect_gnsf_structure.py | 2 +- .../gnsf/reformulate_with_invertible_E_mat.py | 2 +- .../acados_template/mpc_utils.py | 4 +- .../acados_template/penalty_utils.py | 8 +- .../acados_template/acados_template/utils.py | 32 +- .../acados_template/zoro_description.py | 2 +- 22 files changed, 629 insertions(+), 710 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_dims.py b/interfaces/acados_template/acados_template/acados_dims.py index 038078f0e3..1ca68b8103 100644 --- a/interfaces/acados_template/acados_template/acados_dims.py +++ b/interfaces/acados_template/acados_template/acados_dims.py @@ -29,6 +29,14 @@ # POSSIBILITY OF SUCH DAMAGE.; # +def check_int_value(name, value, *, positive=False, nonnegative=False): + if not isinstance(value, int): + raise TypeError(f"Invalid {name} value: expected an integer, got {type(value).__name__}.") + if positive and value <= 0: + raise ValueError(f"Invalid {name} value: expected a positive integer, got {value}.") + if nonnegative and value < 0: + raise ValueError(f"Invalid {name} value: expected a nonnegative integer, got {value}.") + class AcadosSimDims: """ @@ -62,31 +70,24 @@ def np(self): @nx.setter def nx(self, nx): - if isinstance(nx, int) and nx > 0: - self.__nx = nx - else: - raise Exception('Invalid nx value, expected positive integer.') + check_int_value("nx", nx, positive=True) + self.__nx = nx @nz.setter def nz(self, nz): - if isinstance(nz, int) and nz > -1: - self.__nz = nz - else: - raise Exception('Invalid nz value, expected nonnegative integer.') + check_int_value("nz", nz, nonnegative=True) + self.__nz = nz @nu.setter def nu(self, nu): - if isinstance(nu, int) and nu > -1: - self.__nu = nu - else: - raise Exception('Invalid nu value, expected nonnegative integer.') + check_int_value("nu", nu, nonnegative=True) + self.__nu = nu @np.setter def np(self, np): - if isinstance(np, int) and np > -1: - self.__np = np - else: - raise Exception('Invalid np value, expected nonnegative integer.') + check_int_value("np", np, nonnegative=True) + self.__np = np + class AcadosOcpDims: @@ -396,287 +397,205 @@ def N(self): @nx.setter def nx(self, nx): - if isinstance(nx, int) and nx > 0: - self.__nx = nx - else: - raise Exception('Invalid nx value, expected positive integer.') + check_int_value("nx", nx, positive=True) + self.__nx = nx @nz.setter def nz(self, nz): - if isinstance(nz, int) and nz > -1: - self.__nz = nz - else: - raise Exception('Invalid nz value, expected nonnegative integer.') + check_int_value("nz", nz, nonnegative=True) + self.__nz = nz @nu.setter def nu(self, nu): - if isinstance(nu, int) and nu > -1: - self.__nu = nu - else: - raise Exception('Invalid nu value, expected nonnegative integer.') + check_int_value("nu", nu, nonnegative=True) + self.__nu = nu @np.setter def np(self, np): - if isinstance(np, int) and np > -1: - self.__np = np - else: - raise Exception('Invalid np value, expected nonnegative integer.') + check_int_value("np", np, nonnegative=True) + self.__np = np @nx_next.setter def nx_next(self, nx_next): - if isinstance(nx_next, int) and nx_next > 0: - self.__nx_next = nx_next - else: - raise Exception('Invalid nx_next value, expected positive integer.') + check_int_value("nx_next", nx_next, positive=True) + self.__nx_next = nx_next @np_global.setter def np_global(self, np_global): - if isinstance(np_global, int) and np_global > -1: - self.__np_global = np_global - else: - raise Exception('Invalid np_global value, expected nonnegative integer.') + check_int_value("np_global", np_global, nonnegative=True) + self.__np_global = np_global @n_global_data.setter def n_global_data(self, n_global_data): - if isinstance(n_global_data, int) and n_global_data > -1: - self.__n_global_data = n_global_data - else: - raise Exception('Invalid n_global_data value, expected nonnegative integer.') + check_int_value("n_global_data", n_global_data, nonnegative=True) + self.__n_global_data = n_global_data @ny_0.setter def ny_0(self, ny_0): - if isinstance(ny_0, int) and ny_0 > -1: - self.__ny_0 = ny_0 - else: - raise Exception('Invalid ny_0 value, expected nonnegative integer.') + check_int_value("ny_0", ny_0, nonnegative=True) + self.__ny_0 = ny_0 @ny.setter def ny(self, ny): - if isinstance(ny, int) and ny > -1: - self.__ny = ny - else: - raise Exception('Invalid ny value, expected nonnegative integer.') + check_int_value("ny", ny, nonnegative=True) + self.__ny = ny @ny_e.setter def ny_e(self, ny_e): - if isinstance(ny_e, int) and ny_e > -1: - self.__ny_e = ny_e - else: - raise Exception('Invalid ny_e value, expected nonnegative integer.') + check_int_value("ny_e", ny_e, nonnegative=True) + self.__ny_e = ny_e @nh.setter def nh(self, nh): - if isinstance(nh, int) and nh > -1: - self.__nh = nh - else: - raise Exception('Invalid nh value, expected nonnegative integer.') + check_int_value("nh", nh, nonnegative=True) + self.__nh = nh @nh_0.setter def nh_0(self, nh_0): - if isinstance(nh_0, int) and nh_0 > -1: - self.__nh_0 = nh_0 - else: - raise Exception('Invalid nh_0 value, expected nonnegative integer.') + check_int_value("nh_0", nh_0, nonnegative=True) + self.__nh_0 = nh_0 @nh_e.setter def nh_e(self, nh_e): - if isinstance(nh_e, int) and nh_e > -1: - self.__nh_e = nh_e - else: - raise Exception('Invalid nh_e value, expected nonnegative integer.') + check_int_value("nh_e", nh_e, nonnegative=True) + self.__nh_e = nh_e @nphi_0.setter def nphi_0(self, nphi_0): - if isinstance(nphi_0, int) and nphi_0 > -1: - self.__nphi_0 = nphi_0 - else: - raise Exception('Invalid nphi_0 value, expected nonnegative integer.') + check_int_value("nphi_0", nphi_0, nonnegative=True) + self.__nphi_0 = nphi_0 @nphi.setter def nphi(self, nphi): - if isinstance(nphi, int) and nphi > -1: - self.__nphi = nphi - else: - raise Exception('Invalid nphi value, expected nonnegative integer.') + check_int_value("nphi", nphi, nonnegative=True) + self.__nphi = nphi @nphi_e.setter def nphi_e(self, nphi_e): - if isinstance(nphi_e, int) and nphi_e > -1: - self.__nphi_e = nphi_e - else: - raise Exception('Invalid nphi_e value, expected nonnegative integer.') + check_int_value("nphi_e", nphi_e, nonnegative=True) + self.__nphi_e = nphi_e @nr_0.setter def nr_0(self, nr_0): - if isinstance(nr_0, int) and nr_0 > -1: - self.__nr_0 = nr_0 - else: - raise Exception('Invalid nr_0 value, expected nonnegative integer.') + check_int_value("nr_0", nr_0, nonnegative=True) + self.__nr_0 = nr_0 @nr.setter def nr(self, nr): - if isinstance(nr, int) and nr > -1: - self.__nr = nr - else: - raise Exception('Invalid nr value, expected nonnegative integer.') + check_int_value("nr", nr, nonnegative=True) + self.__nr = nr @nr_e.setter def nr_e(self, nr_e): - if isinstance(nr_e, int) and nr_e > -1: - self.__nr_e = nr_e - else: - raise Exception('Invalid nr_e value, expected nonnegative integer.') + check_int_value("nr_e", nr_e, nonnegative=True) + self.__nr_e = nr_e @nbx.setter def nbx(self, nbx): - if isinstance(nbx, int) and nbx > -1: - self.__nbx = nbx - else: - raise Exception('Invalid nbx value, expected nonnegative integer.') + check_int_value("nbx", nbx, nonnegative=True) + self.__nbx = nbx @nbxe_0.setter def nbxe_0(self, nbxe_0): - if isinstance(nbxe_0, int) and nbxe_0 > -1: - self.__nbxe_0 = nbxe_0 - else: - raise Exception('Invalid nbxe_0 value, expected nonnegative integer.') + check_int_value("nbxe_0", nbxe_0, nonnegative=True) + self.__nbxe_0 = nbxe_0 @nbx_0.setter def nbx_0(self, nbx_0): - if isinstance(nbx_0, int) and nbx_0 > -1: - self.__nbx_0 = nbx_0 - else: - raise Exception('Invalid nbx_0 value, expected nonnegative integer.') + check_int_value("nbx_0", nbx_0, nonnegative=True) + self.__nbx_0 = nbx_0 @nbx_e.setter def nbx_e(self, nbx_e): - if isinstance(nbx_e, int) and nbx_e > -1: - self.__nbx_e = nbx_e - else: - raise Exception('Invalid nbx_e value, expected nonnegative integer.') + check_int_value("nbx_e", nbx_e, nonnegative=True) + self.__nbx_e = nbx_e @nbu.setter def nbu(self, nbu): - if isinstance(nbu, int) and nbu > -1: - self.__nbu = nbu - else: - raise Exception('Invalid nbu value, expected nonnegative integer.') + check_int_value("nbu", nbu, nonnegative=True) + self.__nbu = nbu @nsbx.setter def nsbx(self, nsbx): - if isinstance(nsbx, int) and nsbx > -1: - self.__nsbx = nsbx - else: - raise Exception('Invalid nsbx value, expected nonnegative integer.') + check_int_value("nsbx", nsbx, nonnegative=True) + self.__nsbx = nsbx @nsbx_e.setter def nsbx_e(self, nsbx_e): - if isinstance(nsbx_e, int) and nsbx_e > -1: - self.__nsbx_e = nsbx_e - else: - raise Exception('Invalid nsbx_e value, expected nonnegative integer.') + check_int_value("nsbx_e", nsbx_e, nonnegative=True) + self.__nsbx_e = nsbx_e @nsbu.setter def nsbu(self, nsbu): - if isinstance(nsbu, int) and nsbu > -1: - self.__nsbu = nsbu - else: - raise Exception('Invalid nsbu value, expected nonnegative integer.') + check_int_value("nsbu", nsbu, nonnegative=True) + self.__nsbu = nsbu @nsg.setter def nsg(self, nsg): - if isinstance(nsg, int) and nsg > -1: - self.__nsg = nsg - else: - raise Exception('Invalid nsg value, expected nonnegative integer.') + check_int_value("nsg", nsg, nonnegative=True) + self.__nsg = nsg @nsg_e.setter def nsg_e(self, nsg_e): - if isinstance(nsg_e, int) and nsg_e > -1: - self.__nsg_e = nsg_e - else: - raise Exception('Invalid nsg_e value, expected nonnegative integer.') + check_int_value("nsg_e", nsg_e, nonnegative=True) + self.__nsg_e = nsg_e @nsh_0.setter def nsh_0(self, nsh_0): - if isinstance(nsh_0, int) and nsh_0 > -1: - self.__nsh_0 = nsh_0 - else: - raise Exception('Invalid nsh_0 value, expected nonnegative integer.') + check_int_value("nsh_0", nsh_0, nonnegative=True) + self.__nsh_0 = nsh_0 @nsh.setter def nsh(self, nsh): - if isinstance(nsh, int) and nsh > -1: - self.__nsh = nsh - else: - raise Exception('Invalid nsh value, expected nonnegative integer.') + check_int_value("nsh", nsh, nonnegative=True) + self.__nsh = nsh @nsh_e.setter def nsh_e(self, nsh_e): - if isinstance(nsh_e, int) and nsh_e > -1: - self.__nsh_e = nsh_e - else: - raise Exception('Invalid nsh_e value, expected nonnegative integer.') + check_int_value("nsh_e", nsh_e, nonnegative=True) + self.__nsh_e = nsh_e @nsphi_0.setter def nsphi_0(self, nsphi_0): - if isinstance(nsphi_0, int) and nsphi_0 > -1: - self.__nsphi_0 = nsphi_0 - else: - raise Exception('Invalid nsphi_0 value, expected nonnegative integer.') + check_int_value("nsphi_0", nsphi_0, nonnegative=True) + self.__nsphi_0 = nsphi_0 @nsphi.setter def nsphi(self, nsphi): - if isinstance(nsphi, int) and nsphi > -1: - self.__nsphi = nsphi - else: - raise Exception('Invalid nsphi value, expected nonnegative integer.') + check_int_value("nsphi", nsphi, nonnegative=True) + self.__nsphi = nsphi @nsphi_e.setter def nsphi_e(self, nsphi_e): - if isinstance(nsphi_e, int) and nsphi_e > -1: - self.__nsphi_e = nsphi_e - else: - raise Exception('Invalid nsphi_e value, expected nonnegative integer.') + check_int_value("nsphi_e", nsphi_e, nonnegative=True) + self.__nsphi_e = nsphi_e @ns_0.setter def ns_0(self, ns_0): - if isinstance(ns_0, int) and ns_0 > -1: - self.__ns_0 = ns_0 - else: - raise Exception('Invalid ns_0 value, expected nonnegative integer.') + check_int_value("ns_0", ns_0, nonnegative=True) + self.__ns_0 = ns_0 @ns.setter def ns(self, ns): - if isinstance(ns, int) and ns > -1: - self.__ns = ns - else: - raise Exception('Invalid ns value, expected nonnegative integer.') + check_int_value("ns", ns, nonnegative=True) + self.__ns = ns @ns_e.setter def ns_e(self, ns_e): - if isinstance(ns_e, int) and ns_e > -1: - self.__ns_e = ns_e - else: - raise Exception('Invalid ns_e value, expected nonnegative integer.') + check_int_value("ns_e", ns_e, nonnegative=True) + self.__ns_e = ns_e @ng.setter def ng(self, ng): - if isinstance(ng, int) and ng > -1: - self.__ng = ng - else: - raise Exception('Invalid ng value, expected nonnegative integer.') + check_int_value("ng", ng, nonnegative=True) + self.__ng = ng @ng_e.setter def ng_e(self, ng_e): - if isinstance(ng_e, int) and ng_e > -1: - self.__ng_e = ng_e - else: - raise Exception('Invalid ng_e value, expected nonnegative integer.') + check_int_value("ng_e", ng_e, nonnegative=True) + self.__ng_e = ng_e @N.setter def N(self, N): - if isinstance(N, int) and N > 0: - self.__N = N - else: - raise Exception('Invalid N value, expected positive integer.') + check_int_value("N", N, positive=True) + self.__N = N diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 30712b062e..0089c73354 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -821,7 +821,7 @@ def get_casadi_symbol(self): elif isinstance(self.x, SX): return SX.sym else: - raise Exception(f"model.x must be casadi.SX or casadi.MX, got {type(self.x)}") + raise TypeError(f"model.x must be casadi.SX or casadi.MX, got {type(self.x)}") def get_casadi_zeros(self): if isinstance(self.x, MX): @@ -829,7 +829,7 @@ def get_casadi_zeros(self): elif isinstance(self.x, SX): return SX.zeros else: - raise Exception(f"model.x must be casadi.SX or casadi.MX, got {type(self.x)}") + raise TypeError(f"model.x must be casadi.SX or casadi.MX, got {type(self.x)}") def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: @@ -838,7 +838,7 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: # nx if is_empty(self.x): - raise Exception("model.x must be defined") + raise ValueError("model.x must be defined") else: dims.nx = casadi_length(self.x) @@ -846,7 +846,7 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: self.xdot = casadi_symbol('xdot', dims.nx, 1) else: if casadi_length(self.xdot) != dims.nx: - raise Exception(f"model.xdot must have length nx = {dims.nx}, got {casadi_length(self.xdot)}") + raise ValueError(f"model.xdot must have length nx = {dims.nx}, got {casadi_length(self.xdot)}") # nu if is_empty(self.u): @@ -879,16 +879,16 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: # sanity checks for symbol, name in [(self.x, 'x'), (self.xdot, 'xdot'), (self.u, 'u'), (self.z, 'z'), (self.p, 'p'), (self.p_global, 'p_global')]: if not isinstance(symbol, (ca.MX, ca.SX)): - raise Exception(f"model.{name} must be casadi.MX, casadi.SX got {type(symbol)}") + raise TypeError(f"model.{name} must be casadi.MX, casadi.SX got {type(symbol)}") if not symbol.is_valid_input(): - raise Exception(f"model.{name} must be valid CasADi symbol, got {symbol}") + raise ValueError(f"model.{name} must be valid CasADi symbol, got {symbol}") # p_global if not is_empty(self.p_global): if isinstance(dims, AcadosSimDims): - raise Exception("model.p_global is only supported for OCPs") + raise NotImplementedError("model.p_global is only supported for OCPs") if any(ca.which_depends(self.p_global, self.p)): - raise Exception(f"model.p_global must not depend on model.p, got p_global ={self.p_global}, p = {self.p}") + raise ValueError(f"model.p_global must not depend on model.p, got p_global ={self.p_global}, p = {self.p}") # model output dimension nx_next: dimension of the next state if isinstance(dims, AcadosOcpDims): @@ -899,10 +899,10 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: if not is_empty(self.f_impl_expr): if casadi_length(self.f_impl_expr) != (dims.nx + dims.nz): - raise Exception(f"model.f_impl_expr must have length nx + nz = {dims.nx} + {dims.nz}, got {casadi_length(self.f_impl_expr)}") + raise ValueError(f"model.f_impl_expr must have length nx + nz = {dims.nx} + {dims.nz}, got {casadi_length(self.f_impl_expr)}") if not is_empty(self.f_expl_expr): if casadi_length(self.f_expl_expr) != dims.nx: - raise Exception(f"model.f_expl_expr must have length nx = {dims.nx}, got {casadi_length(self.f_expl_expr)}") + raise ValueError(f"model.f_expl_expr must have length nx = {dims.nx}, got {casadi_length(self.f_expl_expr)}") return @@ -936,9 +936,9 @@ def reformulate_with_polynomial_control(self, degree: int) -> None: :type degree: int """ if self.u is None: - raise Exception('model.u must be defined') + raise ValueError('model.u must be defined') if self.nu_original is not None: - raise Exception('model.u has already been augmented') + raise ValueError('model.u has already been augmented') casadi_symbol = self.get_casadi_symbol() diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index 253c463229..6df5df8176 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -68,7 +68,7 @@ def find_non_default_fields_of_obj(obj: Union[AcadosOcpCost, AcadosOcpConstraint elif stage_type == 'terminal': all_fields = [field for field in all_fields if field.endswith("_e")] else: - raise Exception(f"stage_type {stage_type} not supported.") + raise ValueError(f"stage_type {stage_type} not supported.") obj_type = type(obj) dummy_obj = obj_type() @@ -113,11 +113,11 @@ def make_consistent(self, opts: AcadosOcpOptions, n_phases: int) -> None: # non varying field, use value from ocp opts setattr(self, field, [getattr(opts, field) for _ in range(n_phases)]) if not isinstance(getattr(self, field), list): - raise Exception(f'AcadosMultiphaseOptions.{field} must be a list, got {getattr(self, field)}.') + raise TypeError(f'AcadosMultiphaseOptions.{field} must be a list, got {getattr(self, field)}.') if len(getattr(self, field)) != n_phases: - raise Exception(f'AcadosMultiphaseOptions.{field} must be a list of length n_phases, got {getattr(self, field)}.') + raise ValueError(f'AcadosMultiphaseOptions.{field} must be a list of length n_phases, got {getattr(self, field)}.') if not all([item in variants for item in getattr(self, field)]): - raise Exception(f'AcadosMultiphaseOptions.{field} must be a list of strings in {variants}, got {getattr(self, field)}.') + raise ValueError(f'AcadosMultiphaseOptions.{field} must be a list of strings in {variants}, got {getattr(self, field)}.') class AcadosMultiphaseOcp: @@ -135,9 +135,9 @@ class AcadosMultiphaseOcp: def __init__(self, N_list: list): if not isinstance(N_list, list) or len(N_list) < 1: - raise Exception("N_list must be a list of integers.") + raise TypeError("N_list must be a list of integers.") if any([not isinstance(N, int) for N in N_list]): - raise Exception("N_list must be a list of integers.") + raise TypeError("N_list must be a list of integers.") n_phases = len(N_list) @@ -196,9 +196,9 @@ def parameter_values(self): @parameter_values.setter def parameter_values(self, parameter_values): if not isinstance(parameter_values, list): - raise Exception('parameter_values must be a list of numpy.ndarrays.') + raise TypeError('parameter_values must be a list of numpy.ndarrays.') elif len(parameter_values) != self.n_phases: - raise Exception('parameter_values must be a list of length n_phases.') + raise ValueError('parameter_values must be a list of length n_phases.') self.__parameter_values = parameter_values @@ -213,7 +213,7 @@ def p_global_values(self): @p_global_values.setter def p_global_values(self, p_global_values): if not isinstance(p_global_values, np.ndarray): - raise Exception('p_global_values must be a single numpy.ndarrays.') + raise TypeError('p_global_values must be a single numpy.ndarrays.') self.__p_global_values = p_global_values @@ -237,7 +237,7 @@ def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None: :param phase_idx: index of the phase, must be in [0, n_phases-1] """ if phase_idx >= self.n_phases: - raise Exception(f"phase_idx {phase_idx} out of bounds, must be in [0, {self.n_phases-1}].") + raise IndexError(f"phase_idx {phase_idx} out of bounds, must be in [0, {self.n_phases-1}].") # check options non_default_opts = find_non_default_fields_of_obj(ocp.solver_options) @@ -268,15 +268,15 @@ def make_consistent(self) -> None: warning = "\nNOTE: this can happen if set_phase() is called with the same ocp object for multiple phases." for field in ['model', 'cost', 'constraints']: if len(set(getattr(self, field))) != self.n_phases: - raise Exception(f"AcadosMultiphaseOcp: make_consistent: {field} objects are not distinct.{warning}") + raise ValueError(f"AcadosMultiphaseOcp: make_consistent: {field} objects are not distinct.{warning}") # p_global check: p_global = self.model[0].p_global for i in range(self.n_phases): if is_empty(p_global) and not is_empty(self.model[i].p_global): - raise Exception(f"p_global is empty for phase 0, but not for phase {i}. Should be the same for all phases.") + raise ValueError(f"p_global is empty for phase 0, but not for phase {i}. Should be the same for all phases.") if not is_empty(p_global) and not ca.is_equal(p_global, self.model[i].p_global): - raise Exception(f"p_global is different for phase 0 and phase {i}. Should be the same for all phases.") + raise ValueError(f"p_global is different for phase 0 and phase {i}. Should be the same for all phases.") # compute phase indices phase_idx = np.cumsum([0] + self.N_list).tolist() @@ -339,9 +339,9 @@ def make_consistent(self) -> None: for i in range(1, self.n_phases): if nx_list[i] != nx_list[i-1]: if self.phases_dims[i].nx != self.phases_dims[i-1].nx_next: - raise Exception(f"detected stage transition with different nx from phase {i-1} to {i}, nx_next at phase {i-1} = {self.phases_dims[i-1].nx_next} should match nx at phase {i} = {nx_list[i]}.") + raise ValueError(f"detected stage transition with different nx from phase {i-1} to {i}, nx_next at phase {i-1} = {self.phases_dims[i-1].nx_next} should match nx at phase {i} = {nx_list[i]}.") if self.N_list[i-1] != 1 or self.mocp_opts.integrator_type[i-1] != 'DISCRETE': - raise Exception(f"detected stage transition with different nx from phase {i-1} to {i}, which is only supported for integrator_type='DISCRETE' and N_list[i] == 1.") + raise ValueError(f"detected stage transition with different nx from phase {i-1} to {i}, which is only supported for integrator_type='DISCRETE' and N_list[i] == 1.") return @@ -428,7 +428,7 @@ def render_templates(self, cmake_builder=None): # check json file json_path = os.path.abspath(self.json_file) if not os.path.exists(json_path): - raise Exception(f'Path "{json_path}" not found!') + raise FileNotFoundError(f'Path "{json_path}" not found!') # solver templates template_list = self.__get_template_list(cmake_builder=cmake_builder) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 8957c0b6b9..e14b70188b 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -130,10 +130,10 @@ def parameter_values(self): def parameter_values(self, parameter_values): if isinstance(parameter_values, np.ndarray): if not is_column(parameter_values): - raise Exception("parameter_values should be column vector.") + raise ValueError("parameter_values should be column vector.") self.__parameter_values = parameter_values else: - raise Exception('Invalid parameter_values value. ' + + raise ValueError('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') @property @@ -147,11 +147,11 @@ def p_global_values(self): def p_global_values(self, p_global_values): if isinstance(p_global_values, np.ndarray): if not is_column(p_global_values): - raise Exception("p_global_values should be column vector.") + raise ValueError("p_global_values should be column vector.") self.__p_global_values = p_global_values else: - raise Exception('Invalid p_global_values value. ' + + raise ValueError('Invalid p_global_values value. ' + f'Expected numpy array, got {type(p_global_values)}.') @property @@ -178,16 +178,16 @@ def make_consistent(self, is_mocp_phase=False) -> None: # check if nx != nx_next if not is_mocp_phase and dims.nx != dims.nx_next and opts.N_horizon > 1: - raise Exception('nx_next should be equal to nx if more than one shooting interval is used.') + raise ValueError('nx_next should be equal to nx if more than one shooting interval is used.') # parameters if self.parameter_values.shape[0] != dims.np: - raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ + raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n') # p_global_values if self.p_global_values.shape[0] != dims.np_global: - raise Exception('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ + raise ValueError('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n') ## cost @@ -209,17 +209,17 @@ def make_consistent(self, is_mocp_phase=False) -> None: check_if_square(cost.W_0, 'W_0') ny_0 = cost.W_0.shape[0] if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0: - raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \ + raise ValueError('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \ f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}]\n') if dims.nz != 0 and cost.Vz_0.shape[0] != ny_0: - raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + \ + raise ValueError('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}], Vz_0[{cost.Vz_0.shape}]\n') if cost.Vx_0.shape[1] != dims.nx and ny_0 != 0: - raise Exception('inconsistent dimension: Vx_0 should have nx columns.') + raise ValueError('inconsistent dimension: Vx_0 should have nx columns.') if cost.Vu_0.shape[1] != dims.nu and ny_0 != 0: - raise Exception('inconsistent dimension: Vu_0 should have nu columns.') + raise ValueError('inconsistent dimension: Vu_0 should have nu columns.') if cost.yref_0.shape[0] != ny_0: - raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \ + raise ValueError('inconsistent dimension: regarding W_0, yref_0.' + \ f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}]\n') dims.ny_0 = ny_0 @@ -227,37 +227,37 @@ def make_consistent(self, is_mocp_phase=False) -> None: ny_0 = cost.W_0.shape[0] check_if_square(cost.W_0, 'W_0') if (is_empty(model.cost_y_expr_0) and ny_0 != 0) or casadi_length(model.cost_y_expr_0) != ny_0 or cost.yref_0.shape[0] != ny_0: - raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.' + + raise ValueError('inconsistent dimension ny_0: regarding W_0, cost_y_expr.' + f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}], ', f'cost_y_expr_0 [{casadi_length(model.cost_y_expr_0)}]\n') dims.ny_0 = ny_0 elif cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR': if is_empty(model.cost_y_expr_0): - raise Exception('cost_y_expr_0 and/or cost_y_expr not provided.') + raise ValueError('cost_y_expr_0 and/or cost_y_expr not provided.') ny_0 = casadi_length(model.cost_y_expr_0) if is_empty(model.cost_r_in_psi_expr_0) or casadi_length(model.cost_r_in_psi_expr_0) != ny_0: - raise Exception('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_0.') + raise ValueError('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_0.') if is_empty(model.cost_psi_expr_0) or casadi_length(model.cost_psi_expr_0) != 1: - raise Exception('cost_psi_expr_0 not provided or not scalar-valued.') + raise ValueError('cost_psi_expr_0 not provided or not scalar-valued.') if cost.yref_0.shape[0] != ny_0: - raise Exception('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_0.') + raise ValueError('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_0.') dims.ny_0 = ny_0 if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + raise ValueError("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") elif cost.cost_type_0 == 'EXTERNAL': if isinstance(model.cost_expr_ext_cost_0, (float, int)): model.cost_expr_ext_cost_0 = ca.DM(model.cost_expr_ext_cost_0) if not isinstance(model.cost_expr_ext_cost_0, (ca.MX, ca.SX, ca.DM)): - raise Exception('cost_expr_ext_cost_0 should be casadi expression.') + raise TypeError('cost_expr_ext_cost_0 should be casadi expression.') if not casadi_length(model.cost_expr_ext_cost_0) == 1: - raise Exception('cost_expr_ext_cost_0 should be scalar-valued.') + raise ValueError('cost_expr_ext_cost_0 should be scalar-valued.') if not is_empty(model.cost_expr_ext_cost_custom_hess_0): if model.cost_expr_ext_cost_custom_hess_0.shape != (dims.nx+dims.nu, dims.nx+dims.nu): - raise Exception('cost_expr_ext_cost_custom_hess_0 should have shape (nx+nu, nx+nu).') + raise ValueError('cost_expr_ext_cost_custom_hess_0 should have shape (nx+nu, nx+nu).') # GN check gn_warning_0 = (cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) @@ -293,17 +293,17 @@ def make_consistent(self, is_mocp_phase=False) -> None: ny = cost.W.shape[0] check_if_square(cost.W, 'W') if cost.Vx.shape[0] != ny or cost.Vu.shape[0] != ny: - raise Exception('inconsistent dimension ny, regarding W, Vx, Vu.' + \ + raise ValueError('inconsistent dimension ny, regarding W, Vx, Vu.' + \ f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}]\n') if dims.nz != 0 and cost.Vz.shape[0] != ny: - raise Exception('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \ + raise ValueError('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \ f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}], Vz[{cost.Vz.shape}]\n') if cost.Vx.shape[1] != dims.nx and ny != 0: - raise Exception('inconsistent dimension: Vx should have nx columns.') + raise ValueError('inconsistent dimension: Vx should have nx columns.') if cost.Vu.shape[1] != dims.nu and ny != 0: - raise Exception('inconsistent dimension: Vu should have nu columns.') + raise ValueError('inconsistent dimension: Vu should have nu columns.') if cost.yref.shape[0] != ny: - raise Exception('inconsistent dimension: regarding W, yref.' + \ + raise ValueError('inconsistent dimension: regarding W, yref.' + \ f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') dims.ny = ny @@ -311,37 +311,37 @@ def make_consistent(self, is_mocp_phase=False) -> None: ny = cost.W.shape[0] check_if_square(cost.W, 'W') if (is_empty(model.cost_y_expr) and ny != 0) or casadi_length(model.cost_y_expr) != ny or cost.yref.shape[0] != ny: - raise Exception('inconsistent dimension: regarding W, yref.' + \ + raise ValueError('inconsistent dimension: regarding W, yref.' + \ f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}],', f'cost_y_expr[{casadi_length(model.cost_y_expr)}]\n') dims.ny = ny elif cost.cost_type == 'CONVEX_OVER_NONLINEAR': if is_empty(model.cost_y_expr): - raise Exception('cost_y_expr and/or cost_y_expr not provided.') + raise ValueError('cost_y_expr and/or cost_y_expr not provided.') ny = casadi_length(model.cost_y_expr) if is_empty(model.cost_r_in_psi_expr) or casadi_length(model.cost_r_in_psi_expr) != ny: - raise Exception('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi.') + raise ValueError('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi.') if is_empty(model.cost_psi_expr) or casadi_length(model.cost_psi_expr) != 1: - raise Exception('cost_psi_expr not provided or not scalar-valued.') + raise ValueError('cost_psi_expr not provided or not scalar-valued.') if cost.yref.shape[0] != ny: - raise Exception('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi.') + raise ValueError('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi.') dims.ny = ny if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + raise ValueError("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") elif cost.cost_type == 'EXTERNAL': if isinstance(model.cost_expr_ext_cost, (float, int)): model.cost_expr_ext_cost = ca.DM(model.cost_expr_ext_cost) if not isinstance(model.cost_expr_ext_cost, (ca.MX, ca.SX, ca.DM)): - raise Exception('cost_expr_ext_cost should be casadi expression.') + raise TypeError('cost_expr_ext_cost should be casadi expression.') if not casadi_length(model.cost_expr_ext_cost) == 1: - raise Exception('cost_expr_ext_cost should be scalar-valued.') + raise ValueError('cost_expr_ext_cost should be scalar-valued.') if not is_empty(model.cost_expr_ext_cost_custom_hess): if model.cost_expr_ext_cost_custom_hess.shape != (dims.nx+dims.nu, dims.nx+dims.nu): - raise Exception('cost_expr_ext_cost_custom_hess should have shape (nx+nu, nx+nu).') + raise ValueError('cost_expr_ext_cost_custom_hess should have shape (nx+nu, nx+nu).') # terminal if cost.cost_type_e == 'AUTO': self.detect_cost_type(model, cost, dims, "terminal") @@ -357,74 +357,74 @@ def make_consistent(self, is_mocp_phase=False) -> None: ny_e = cost.W_e.shape[0] check_if_square(cost.W_e, 'W_e') if cost.Vx_e.shape[0] != ny_e: - raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: - raise Exception('inconsistent dimension: Vx_e should have nx columns.') + raise ValueError('inconsistent dimension: Vx_e should have nx columns.') if cost.yref_e.shape[0] != ny_e: - raise Exception('inconsistent dimension: regarding W_e, yref_e.') + raise ValueError('inconsistent dimension: regarding W_e, yref_e.') dims.ny_e = ny_e elif cost.cost_type_e == 'NONLINEAR_LS': ny_e = cost.W_e.shape[0] check_if_square(cost.W_e, 'W_e') if (is_empty(model.cost_y_expr_e) and ny_e != 0) or casadi_length(model.cost_y_expr_e) != ny_e or cost.yref_e.shape[0] != ny_e: - raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + f'\nGot W_e[{cost.W_e.shape}], yref_e[{cost.yref_e.shape}], ', f'cost_y_expr_e [{casadi_length(model.cost_y_expr_e)}]\n') dims.ny_e = ny_e elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': if is_empty(model.cost_y_expr_e): - raise Exception('cost_y_expr_e not provided.') + raise ValueError('cost_y_expr_e not provided.') ny_e = casadi_length(model.cost_y_expr_e) if is_empty(model.cost_r_in_psi_expr_e) or casadi_length(model.cost_r_in_psi_expr_e) != ny_e: - raise Exception('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_e.') + raise ValueError('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_e.') if is_empty(model.cost_psi_expr_e) or casadi_length(model.cost_psi_expr_e) != 1: - raise Exception('cost_psi_expr_e not provided or not scalar-valued.') + raise ValueError('cost_psi_expr_e not provided or not scalar-valued.') if cost.yref_e.shape[0] != ny_e: - raise Exception('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_e.') + raise ValueError('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_e.') dims.ny_e = ny_e if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': - raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + raise ValueError("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") elif cost.cost_type_e == 'EXTERNAL': if isinstance(model.cost_expr_ext_cost_e, (float, int)): model.cost_expr_ext_cost_e = ca.DM(model.cost_expr_ext_cost_e) if not isinstance(model.cost_expr_ext_cost_e, (ca.MX, ca.SX, ca.DM)): - raise Exception(f'cost_expr_ext_cost_e should be casadi expression, got {model.cost_expr_ext_cost_e}.') + raise TypeError(f'cost_expr_ext_cost_e should be casadi expression, got {model.cost_expr_ext_cost_e}.') if not casadi_length(model.cost_expr_ext_cost_e) == 1: - raise Exception('cost_expr_ext_cost_e should be scalar-valued.') + raise ValueError('cost_expr_ext_cost_e should be scalar-valued.') if not is_empty(model.cost_expr_ext_cost_custom_hess_e): if model.cost_expr_ext_cost_custom_hess_e.shape != (dims.nx, dims.nx): - raise Exception('cost_expr_ext_cost_custom_hess_e should have shape (nx, nx).') + raise ValueError('cost_expr_ext_cost_custom_hess_e should have shape (nx, nx).') # cost integration supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] if opts.cost_discretization == 'INTEGRATOR' and \ any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): - raise Exception('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') + raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') ## constraints # initial nbx_0 = constraints.idxbx_0.shape[0] if constraints.ubx_0.shape[0] != nbx_0 or constraints.lbx_0.shape[0] != nbx_0: - raise Exception('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') + raise ValueError('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') dims.nbx_0 = nbx_0 if any(constraints.idxbx_0 >= dims.nx): - raise Exception(f'idxbx_0 = {constraints.idxbx_0} contains value >= nx = {dims.nx}.') + raise ValueError(f'idxbx_0 = {constraints.idxbx_0} contains value >= nx = {dims.nx}.') if constraints.has_x0 and dims.nbx_0 != dims.nx: - raise Exception(f"x0 should have shape nx = {dims.nx}.") + raise ValueError(f"x0 should have shape nx = {dims.nx}.") if constraints.has_x0 and not np.all(constraints.idxbxe_0 == np.arange(dims.nx)): - raise Exception(f"idxbxe_0 should be 0:{dims.nx} if x0 is set.") + raise ValueError(f"idxbxe_0 should be 0:{dims.nx} if x0 is set.") dims.nbxe_0 = constraints.idxbxe_0.shape[0] if any(constraints.idxbxe_0 >= dims.nbx_0): - raise Exception(f'idxbxe_0 = {constraints.idxbxe_0} contains value >= nbx_0 = {dims.nbx_0}.') + raise ValueError(f'idxbxe_0 = {constraints.idxbxe_0} contains value >= nbx_0 = {dims.nbx_0}.') if not is_empty(model.con_h_expr_0): nh_0 = casadi_length(model.con_h_expr_0) @@ -432,7 +432,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: nh_0 = 0 if constraints.uh_0.shape[0] != nh_0 or constraints.lh_0.shape[0] != nh_0: - raise Exception('inconsistent dimension nh_0, regarding lh_0, uh_0, con_h_expr_0.') + raise ValueError('inconsistent dimension nh_0, regarding lh_0, uh_0, con_h_expr_0.') else: dims.nh_0 = nh_0 @@ -443,40 +443,40 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.nphi_0 = casadi_length(model.con_phi_expr_0) constraints.constr_type_0 = "BGP" if is_empty(model.con_r_expr_0): - raise Exception('convex over nonlinear constraints: con_r_expr_0 but con_phi_expr_0 is nonempty') + raise ValueError('convex over nonlinear constraints: con_r_expr_0 but con_phi_expr_0 is nonempty') else: dims.nr_0 = casadi_length(model.con_r_expr_0) # path nbx = constraints.idxbx.shape[0] if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx: - raise Exception('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') + raise ValueError('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') else: dims.nbx = nbx if any(constraints.idxbx >= dims.nx): - raise Exception(f'idxbx = {constraints.idxbx} contains value >= nx = {dims.nx}.') + raise ValueError(f'idxbx = {constraints.idxbx} contains value >= nx = {dims.nx}.') nbu = constraints.idxbu.shape[0] if constraints.ubu.shape[0] != nbu or constraints.lbu.shape[0] != nbu: - raise Exception('inconsistent dimension nbu, regarding idxbu, ubu, lbu.') + raise ValueError('inconsistent dimension nbu, regarding idxbu, ubu, lbu.') else: dims.nbu = nbu if any(constraints.idxbu >= dims.nu): - raise Exception(f'idxbu = {constraints.idxbu} contains value >= nu = {dims.nu}.') + raise ValueError(f'idxbu = {constraints.idxbu} contains value >= nu = {dims.nu}.') # lg <= C * x + D * u <= ug ng = constraints.lg.shape[0] if constraints.ug.shape[0] != ng or constraints.C.shape[0] != ng \ or constraints.D.shape[0] != ng: - raise Exception('inconsistent dimension ng, regarding lg, ug, C, D.') + raise ValueError('inconsistent dimension ng, regarding lg, ug, C, D.') else: dims.ng = ng if ng > 0: if constraints.C.shape[1] != dims.nx: - raise Exception(f'inconsistent dimension nx, regarding C, got C.shape[1] = {constraints.C.shape[1]}.') + raise ValueError(f'inconsistent dimension nx, regarding C, got C.shape[1] = {constraints.C.shape[1]}.') if constraints.D.shape[1] != dims.nu: - raise Exception(f'inconsistent dimension nu, regarding D, got D.shape[1] = {constraints.D.shape[1]}.') + raise ValueError(f'inconsistent dimension nu, regarding D, got D.shape[1] = {constraints.D.shape[1]}.') if not is_empty(model.con_h_expr): nh = casadi_length(model.con_h_expr) @@ -484,7 +484,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: nh = 0 if constraints.uh.shape[0] != nh or constraints.lh.shape[0] != nh: - raise Exception('inconsistent dimension nh, regarding lh, uh, con_h_expr.') + raise ValueError('inconsistent dimension nh, regarding lh, uh, con_h_expr.') else: dims.nh = nh @@ -495,7 +495,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.nphi = casadi_length(model.con_phi_expr) constraints.constr_type = "BGP" if is_empty(model.con_r_expr): - raise Exception('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty') + raise ValueError('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty') else: dims.nr = casadi_length(model.con_r_expr) @@ -503,15 +503,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: # terminal nbx_e = constraints.idxbx_e.shape[0] if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e: - raise Exception('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') + raise ValueError('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') else: dims.nbx_e = nbx_e if any(constraints.idxbx_e >= dims.nx): - raise Exception(f'idxbx_e = {constraints.idxbx_e} contains value >= nx = {dims.nx}.') + raise ValueError(f'idxbx_e = {constraints.idxbx_e} contains value >= nx = {dims.nx}.') ng_e = constraints.lg_e.shape[0] if constraints.ug_e.shape[0] != ng_e or constraints.C_e.shape[0] != ng_e: - raise Exception('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.') + raise ValueError('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.') else: dims.ng_e = ng_e @@ -521,7 +521,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: nh_e = 0 if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e: - raise Exception('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') + raise ValueError('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') else: dims.nh_e = nh_e @@ -532,84 +532,84 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.nphi_e = casadi_length(model.con_phi_expr_e) constraints.constr_type_e = "BGP" if is_empty(model.con_r_expr_e): - raise Exception('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty') + raise ValueError('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty') else: dims.nr_e = casadi_length(model.con_r_expr_e) # Slack dimensions nsbx = constraints.idxsbx.shape[0] if nsbx > nbx: - raise Exception(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') + raise ValueError(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') if any(constraints.idxsbx >= nbx): - raise Exception(f'idxsbx = {constraints.idxsbx} contains value >= nbx = {nbx}.') + raise ValueError(f'idxsbx = {constraints.idxsbx} contains value >= nbx = {nbx}.') if is_empty(constraints.lsbx): constraints.lsbx = np.zeros((nsbx,)) elif constraints.lsbx.shape[0] != nsbx: - raise Exception('inconsistent dimension nsbx, regarding idxsbx, lsbx.') + raise ValueError('inconsistent dimension nsbx, regarding idxsbx, lsbx.') if is_empty(constraints.usbx): constraints.usbx = np.zeros((nsbx,)) elif constraints.usbx.shape[0] != nsbx: - raise Exception('inconsistent dimension nsbx, regarding idxsbx, usbx.') + raise ValueError('inconsistent dimension nsbx, regarding idxsbx, usbx.') dims.nsbx = nsbx nsbu = constraints.idxsbu.shape[0] if nsbu > nbu: - raise Exception(f'inconsistent dimension nsbu = {nsbu}. Is greater than nbu = {nbu}.') + raise ValueError(f'inconsistent dimension nsbu = {nsbu}. Is greater than nbu = {nbu}.') if any(constraints.idxsbu >= nbu): - raise Exception(f'idxsbu = {constraints.idxsbu} contains value >= nbu = {nbu}.') + raise ValueError(f'idxsbu = {constraints.idxsbu} contains value >= nbu = {nbu}.') if is_empty(constraints.lsbu): constraints.lsbu = np.zeros((nsbu,)) elif constraints.lsbu.shape[0] != nsbu: - raise Exception('inconsistent dimension nsbu, regarding idxsbu, lsbu.') + raise ValueError('inconsistent dimension nsbu, regarding idxsbu, lsbu.') if is_empty(constraints.usbu): constraints.usbu = np.zeros((nsbu,)) elif constraints.usbu.shape[0] != nsbu: - raise Exception('inconsistent dimension nsbu, regarding idxsbu, usbu.') + raise ValueError('inconsistent dimension nsbu, regarding idxsbu, usbu.') dims.nsbu = nsbu nsh = constraints.idxsh.shape[0] if nsh > nh: - raise Exception(f'inconsistent dimension nsh = {nsh}. Is greater than nh = {nh}.') + raise ValueError(f'inconsistent dimension nsh = {nsh}. Is greater than nh = {nh}.') if any(constraints.idxsh >= nh): - raise Exception(f'idxsh = {constraints.idxsh} contains value >= nh = {nh}.') + raise ValueError(f'idxsh = {constraints.idxsh} contains value >= nh = {nh}.') if is_empty(constraints.lsh): constraints.lsh = np.zeros((nsh,)) elif constraints.lsh.shape[0] != nsh: - raise Exception('inconsistent dimension nsh, regarding idxsh, lsh.') + raise ValueError('inconsistent dimension nsh, regarding idxsh, lsh.') if is_empty(constraints.ush): constraints.ush = np.zeros((nsh,)) elif constraints.ush.shape[0] != nsh: - raise Exception('inconsistent dimension nsh, regarding idxsh, ush.') + raise ValueError('inconsistent dimension nsh, regarding idxsh, ush.') dims.nsh = nsh nsphi = constraints.idxsphi.shape[0] if nsphi > dims.nphi: - raise Exception(f'inconsistent dimension nsphi = {nsphi}. Is greater than nphi = {dims.nphi}.') + raise ValueError(f'inconsistent dimension nsphi = {nsphi}. Is greater than nphi = {dims.nphi}.') if any(constraints.idxsphi >= dims.nphi): - raise Exception(f'idxsphi = {constraints.idxsphi} contains value >= nphi = {dims.nphi}.') + raise ValueError(f'idxsphi = {constraints.idxsphi} contains value >= nphi = {dims.nphi}.') if is_empty(constraints.lsphi): constraints.lsphi = np.zeros((nsphi,)) elif constraints.lsphi.shape[0] != nsphi: - raise Exception('inconsistent dimension nsphi, regarding idxsphi, lsphi.') + raise ValueError('inconsistent dimension nsphi, regarding idxsphi, lsphi.') if is_empty(constraints.usphi): constraints.usphi = np.zeros((nsphi,)) elif constraints.usphi.shape[0] != nsphi: - raise Exception('inconsistent dimension nsphi, regarding idxsphi, usphi.') + raise ValueError('inconsistent dimension nsphi, regarding idxsphi, usphi.') dims.nsphi = nsphi nsg = constraints.idxsg.shape[0] if nsg > ng: - raise Exception(f'inconsistent dimension nsg = {nsg}. Is greater than ng = {ng}.') + raise ValueError(f'inconsistent dimension nsg = {nsg}. Is greater than ng = {ng}.') if any(constraints.idxsg >= ng): - raise Exception(f'idxsg = {constraints.idxsg} contains value >= ng = {ng}.') + raise ValueError(f'idxsg = {constraints.idxsg} contains value >= ng = {ng}.') if is_empty(constraints.lsg): constraints.lsg = np.zeros((nsg,)) elif constraints.lsg.shape[0] != nsg: - raise Exception('inconsistent dimension nsg, regarding idxsg, lsg.') + raise ValueError('inconsistent dimension nsg, regarding idxsg, lsg.') if is_empty(constraints.usg): constraints.usg = np.zeros((nsg,)) elif constraints.usg.shape[0] != nsg: - raise Exception('inconsistent dimension nsg, regarding idxsg, usg.') + raise ValueError('inconsistent dimension nsg, regarding idxsg, usg.') dims.nsg = nsg ns = nsbx + nsbu + nsh + nsg + nsphi @@ -628,7 +628,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: dim = cost.zu.shape[0] if wrong_fields != []: - raise Exception(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t'\ + raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns @@ -636,32 +636,32 @@ def make_consistent(self, is_mocp_phase=False) -> None: # slack dimensions at initial node nsh_0 = constraints.idxsh_0.shape[0] if nsh_0 > nh_0: - raise Exception(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') + raise ValueError(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') if any(constraints.idxsh_0 >= nh_0): - raise Exception(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') + raise ValueError(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') if is_empty(constraints.lsh_0): constraints.lsh_0 = np.zeros((nsh_0,)) elif constraints.lsh_0.shape[0] != nsh_0: - raise Exception('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') if is_empty(constraints.ush_0): constraints.ush_0 = np.zeros((nsh_0,)) elif constraints.ush_0.shape[0] != nsh_0: - raise Exception('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') dims.nsh_0 = nsh_0 nsphi_0 = constraints.idxsphi_0.shape[0] if nsphi_0 > dims.nphi_0: - raise Exception(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') + raise ValueError(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') if any(constraints.idxsphi_0 >= dims.nphi_0): - raise Exception(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') + raise ValueError(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') if is_empty(constraints.lsphi_0): constraints.lsphi_0 = np.zeros((nsphi_0,)) elif constraints.lsphi_0.shape[0] != nsphi_0: - raise Exception('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') if is_empty(constraints.usphi_0): constraints.usphi_0 = np.zeros((nsphi_0,)) elif constraints.usphi_0.shape[0] != nsphi_0: - raise Exception('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') dims.nsphi_0 = nsphi_0 # Note: at stage 0 bounds on x are not slacked! @@ -698,7 +698,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: dim = cost.zu_0.shape[0] if wrong_fields != []: - raise Exception(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t'\ + raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') dims.ns_0 = ns_0 @@ -706,62 +706,62 @@ def make_consistent(self, is_mocp_phase=False) -> None: # slacks at terminal node nsbx_e = constraints.idxsbx_e.shape[0] if nsbx_e > nbx_e: - raise Exception(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') + raise ValueError(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') if any(constraints.idxsbx_e >= nbx_e): - raise Exception(f'idxsbx_e = {constraints.idxsbx_e} contains value >= nbx_e = {nbx_e}.') + raise ValueError(f'idxsbx_e = {constraints.idxsbx_e} contains value >= nbx_e = {nbx_e}.') if is_empty(constraints.lsbx_e): constraints.lsbx_e = np.zeros((nsbx_e,)) elif constraints.lsbx_e.shape[0] != nsbx_e: - raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.') + raise ValueError('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.') if is_empty(constraints.usbx_e): constraints.usbx_e = np.zeros((nsbx_e,)) elif constraints.usbx_e.shape[0] != nsbx_e: - raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.') + raise ValueError('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.') dims.nsbx_e = nsbx_e nsh_e = constraints.idxsh_e.shape[0] if nsh_e > nh_e: - raise Exception(f'inconsistent dimension nsh_e = {nsh_e}. Is greater than nh_e = {nh_e}.') + raise ValueError(f'inconsistent dimension nsh_e = {nsh_e}. Is greater than nh_e = {nh_e}.') if any(constraints.idxsh_e >= nh_e): - raise Exception(f'idxsh_e = {constraints.idxsh_e} contains value >= nh_e = {nh_e}.') + raise ValueError(f'idxsh_e = {constraints.idxsh_e} contains value >= nh_e = {nh_e}.') if is_empty(constraints.lsh_e): constraints.lsh_e = np.zeros((nsh_e,)) elif constraints.lsh_e.shape[0] != nsh_e: - raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.') + raise ValueError('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.') if is_empty(constraints.ush_e): constraints.ush_e = np.zeros((nsh_e,)) elif constraints.ush_e.shape[0] != nsh_e: - raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.') + raise ValueError('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.') dims.nsh_e = nsh_e nsphi_e = constraints.idxsphi_e.shape[0] if nsphi_e > dims.nphi_e: - raise Exception(f'inconsistent dimension nsphi_e = {nsphi_e}. Is greater than nphi_e = {dims.nphi_e}.') + raise ValueError(f'inconsistent dimension nsphi_e = {nsphi_e}. Is greater than nphi_e = {dims.nphi_e}.') if any(constraints.idxsphi_e >= dims.nphi_e): - raise Exception(f'idxsphi_e = {constraints.idxsphi_e} contains value >= nphi_e = {dims.nphi_e}.') + raise ValueError(f'idxsphi_e = {constraints.idxsphi_e} contains value >= nphi_e = {dims.nphi_e}.') if is_empty(constraints.lsphi_e): constraints.lsphi_e = np.zeros((nsphi_e,)) elif constraints.lsphi_e.shape[0] != nsphi_e: - raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.') + raise ValueError('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.') if is_empty(constraints.usphi_e): constraints.usphi_e = np.zeros((nsphi_e,)) elif constraints.usphi_e.shape[0] != nsphi_e: - raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.') + raise ValueError('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.') dims.nsphi_e = nsphi_e nsg_e = constraints.idxsg_e.shape[0] if nsg_e > ng_e: - raise Exception(f'inconsistent dimension nsg_e = {nsg_e}. Is greater than ng_e = {ng_e}.') + raise ValueError(f'inconsistent dimension nsg_e = {nsg_e}. Is greater than ng_e = {ng_e}.') if any(constraints.idxsg_e >= ng_e): - raise Exception(f'idxsg_e = {constraints.idxsg_e} contains value >= ng_e = {ng_e}.') + raise ValueError(f'idxsg_e = {constraints.idxsg_e} contains value >= ng_e = {ng_e}.') if is_empty(constraints.lsg_e): constraints.lsg_e = np.zeros((nsg_e,)) elif constraints.lsg_e.shape[0] != nsg_e: - raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.') + raise ValueError('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.') if is_empty(constraints.usg_e): constraints.usg_e = np.zeros((nsg_e,)) elif constraints.usg_e.shape[0] != nsg_e: - raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.') + raise ValueError('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.') dims.nsg_e = nsg_e # terminal @@ -781,7 +781,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: dim = cost.zu_e.shape[0] if wrong_field != "": - raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\ + raise ValueError(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t' + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}.') @@ -793,21 +793,21 @@ def make_consistent(self, is_mocp_phase=False) -> None: for field in ['lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e']: bound = getattr(constraints, field) if any(bound >= ACADOS_INFTY) or any(bound <= -ACADOS_INFTY): - raise Exception(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") + raise ValueError(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") # discretization if opts.N_horizon is None and dims.N is None: - raise Exception('N_horizon not provided.') + raise ValueError('N_horizon not provided.') elif opts.N_horizon is None and dims.N is not None: opts.N_horizon = dims.N print("field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon. setting AcadosOcpOptions.N_horizon = N. For future comppatibility, please use AcadosOcpOptions.N_horizon directly.") elif opts.N_horizon is not None and dims.N is not None and opts.N_horizon != dims.N: - raise Exception(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') + raise ValueError(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') else: dims.N = opts.N_horizon if not isinstance(opts.tf, (float, int)): - raise Exception(f'Time horizon tf should be float provided, got tf = {opts.tf}.') + raise TypeError(f'Time horizon tf should be float provided, got tf = {opts.tf}.') if is_empty(opts.time_steps) and is_empty(opts.shooting_nodes): # uniform discretization @@ -816,7 +816,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: elif not is_empty(opts.shooting_nodes): if np.shape(opts.shooting_nodes)[0] != opts.N_horizon+1: - raise Exception('inconsistent dimension N, regarding shooting_nodes.') + raise ValueError('inconsistent dimension N, regarding shooting_nodes.') time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1] # identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit @@ -834,18 +834,18 @@ def make_consistent(self, is_mocp_phase=False) -> None: opts.shooting_nodes = np.concatenate((np.array([0.]), np.cumsum(opts.time_steps))) elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)): - Exception('Please provide either time_steps or shooting_nodes for nonuniform discretization') + ValueError('Please provide either time_steps or shooting_nodes for nonuniform discretization') tf = np.sum(opts.time_steps) if (tf - opts.tf) / tf > 1e-13: - raise Exception(f'Inconsistent discretization: {opts.tf}'\ + raise ValueError(f'Inconsistent discretization: {opts.tf}' f' = tf != sum(opts.time_steps) = {tf}.') # cost scaling if opts.cost_scaling is None: opts.cost_scaling = np.append(opts.time_steps, 1.0) if opts.cost_scaling.shape[0] != opts.N_horizon + 1: - raise Exception(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') + raise ValueError(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') # set integrator time automatically opts.Tsim = opts.time_steps[0] @@ -860,7 +860,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: and np.all(np.equal(np.mod(opts.sim_method_num_steps, 1), 0)): opts.sim_method_num_steps = np.reshape(opts.sim_method_num_steps, (opts.N_horizon,)).astype(np.int64) else: - raise Exception("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).") + raise TypeError("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).") # num_stages if isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == 1: @@ -872,7 +872,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: and np.all(np.equal(np.mod(opts.sim_method_num_stages, 1), 0)): opts.sim_method_num_stages = np.reshape(opts.sim_method_num_stages, (opts.N_horizon,)).astype(np.int64) else: - raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).") + raise ValueError("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).") # jac_reuse if isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == 1: @@ -884,18 +884,18 @@ def make_consistent(self, is_mocp_phase=False) -> None: and np.all(np.equal(np.mod(opts.sim_method_jac_reuse, 1), 0)): opts.sim_method_jac_reuse = np.reshape(opts.sim_method_jac_reuse, (opts.N_horizon,)).astype(np.int64) else: - raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") + raise ValueError("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") # fixed hessian if opts.fixed_hess: if opts.hessian_approx == 'EXACT': - raise Exception('fixed_hess is not compatible with hessian_approx == EXACT.') + raise ValueError('fixed_hess is not compatible with hessian_approx == EXACT.') if cost.cost_type != "LINEAR_LS": - raise Exception('fixed_hess is only compatible LINEAR_LS cost_type.') + raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type.') if cost.cost_type_0 != "LINEAR_LS": - raise Exception('fixed_hess is only compatible LINEAR_LS cost_type_0.') + raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_0.') if cost.cost_type_e != "LINEAR_LS": - raise Exception('fixed_hess is only compatible LINEAR_LS cost_type_e.') + raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_e.') # solution sensitivities bgp_type_constraint_pairs = [ @@ -908,45 +908,45 @@ def make_consistent(self, is_mocp_phase=False) -> None: if opts.with_solution_sens_wrt_params: if dims.np_global == 0: - raise Exception('with_solution_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') + raise ValueError('with_solution_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise Exception(f'with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type, got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}.') + raise ValueError(f'with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type, got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}.') if opts.integrator_type != "DISCRETE": - raise Exception('with_solution_sens_wrt_params is only compatible with DISCRETE dynamics.') + raise NotImplementedError('with_solution_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): - raise Exception(f"with_solution_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + raise NotImplementedError(f"with_solution_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") if opts.with_value_sens_wrt_params: if dims.np_global == 0: - raise Exception('with_value_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') + raise ValueError('with_value_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise Exception('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type.') + raise ValueError('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type.') if opts.integrator_type != "DISCRETE": - raise Exception('with_value_sens_wrt_params is only compatible with DISCRETE dynamics.') + raise NotImplementedError('with_value_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): - raise Exception(f"with_value_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + raise NotImplementedError(f"with_value_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") if opts.qp_solver_cond_N is None: opts.qp_solver_cond_N = opts.N_horizon if opts.tau_min > 0 and not "HPIPM" in opts.qp_solver: - raise Exception('tau_min > 0 is only compatible with HPIPM.') + raise ValueError('tau_min > 0 is only compatible with HPIPM.') if opts.qp_solver_cond_block_size is not None: if sum(opts.qp_solver_cond_block_size) != opts.N_horizon: - raise Exception(f'sum(qp_solver_cond_block_size) = {sum(opts.qp_solver_cond_block_size)} != N = {opts.N_horizon}.') + raise ValueError(f'sum(qp_solver_cond_block_size) = {sum(opts.qp_solver_cond_block_size)} != N = {opts.N_horizon}.') if len(opts.qp_solver_cond_block_size) != opts.qp_solver_cond_N+1: - raise Exception(f'qp_solver_cond_block_size = {opts.qp_solver_cond_block_size} should have length qp_solver_cond_N+1 = {opts.qp_solver_cond_N+1}.') + raise ValueError(f'qp_solver_cond_block_size = {opts.qp_solver_cond_block_size} should have length qp_solver_cond_N+1 = {opts.qp_solver_cond_N+1}.') if opts.nlp_solver_type == "DDP": if opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon: - raise Exception(f'DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N, got qp solver {opts.qp_solver} and qp_solver_cond_N {opts.qp_solver_cond_N}, N {opts.N_horizon}.') + raise ValueError(f'DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N, got qp solver {opts.qp_solver} and qp_solver_cond_N {opts.qp_solver_cond_N}, N {opts.N_horizon}.') if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]): - raise Exception(f'DDP only supports initial state constraints, got path constraints. Dimensions: dims.nbu = {dims.nbu}, dims.nbx = {dims.nbx}, dims.ng = {dims.ng}, dims.nh = {dims.nh}, dims.nphi = {dims.nphi}') + raise ValueError(f'DDP only supports initial state constraints, got path constraints. Dimensions: dims.nbu = {dims.nbu}, dims.nbx = {dims.nbx}, dims.ng = {dims.ng}, dims.nh = {dims.nh}, dims.nphi = {dims.nphi}') if any([dims.ng_e, dims.nphi_e, dims.nh_e]): - raise Exception('DDP only supports initial state constraints, got terminal constraints.') + raise ValueError('DDP only supports initial state constraints, got terminal constraints.') ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') # Set default parameters for globalization @@ -982,7 +982,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: # sanity check for Funnel globalization and SQP if opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' and opts.nlp_solver_type not in ['SQP', 'SQP_WITH_FEASIBLE_QP']: - raise Exception('FUNNEL_L1PEN_LINESEARCH only supports SQP.') + raise NotImplementedError('FUNNEL_L1PEN_LINESEARCH only supports SQP.') # termination if opts.nlp_solver_tol_min_step_norm == None: @@ -994,13 +994,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: # zoRO if self.zoro_description is not None: if not isinstance(self.zoro_description, ZoroDescription): - raise Exception('zoro_description should be of type ZoroDescription or None') + raise TypeError('zoro_description should be of type ZoroDescription or None') else: self.zoro_description = process_zoro_description(self.zoro_description) # nlp_solver_warm_start_first_qp_from_nlp if opts.nlp_solver_warm_start_first_qp_from_nlp and (opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon): - raise Exception('nlp_solver_warm_start_first_qp_from_nlp only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.') + raise NotImplementedError('nlp_solver_warm_start_first_qp_from_nlp only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.') return @@ -1100,7 +1100,7 @@ def render_templates(self, cmake_builder=None): # check json file json_path = os.path.abspath(self.json_file) if not os.path.exists(json_path): - raise Exception(f'Path "{json_path}" not found!') + raise FileNotFoundError(f'Path "{json_path}" not found!') template_list = self.__get_template_list(cmake_builder=cmake_builder) @@ -1175,7 +1175,7 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia elif self.solver_options.integrator_type == 'DISCRETE': generate_c_code_discrete_dynamics(context, model, model_dir) else: - raise Exception("ocp_generate_external_functions: unknown integrator type.") + raise ValueError("ocp_generate_external_functions: unknown integrator type.") else: target_dir = os.path.join(code_gen_opts.code_export_directory, model_dir) target_location = os.path.join(target_dir, model.dyn_generic_source) @@ -1266,7 +1266,7 @@ def translate_nls_cost_to_conl(self): self.model.cost_r_in_psi_expr_0 = conl_res_0 self.model.cost_psi_expr_0 = .5 * conl_res_0.T @ ca.sparsify(ca.DM(self.cost.W_0)) @ conl_res_0 else: - raise Exception(f"Terminal cost type must be NONLINEAR_LS, got cost_type_0 {self.cost.cost_type_0}.") + raise TypeError(f"Terminal cost type must be NONLINEAR_LS, got cost_type_0 {self.cost.cost_type_0}.") # path cost if self.cost.cost_type == "CONVEX_OVER_NONLINEAR": @@ -1279,7 +1279,7 @@ def translate_nls_cost_to_conl(self): self.model.cost_r_in_psi_expr = conl_res self.model.cost_psi_expr = .5 * conl_res.T @ ca.sparsify(ca.DM(self.cost.W)) @ conl_res else: - raise Exception(f"Path cost type must be NONLINEAR_LS, got cost_type {self.cost.cost_type}.") + raise TypeError(f"Path cost type must be NONLINEAR_LS, got cost_type {self.cost.cost_type}.") # terminal cost if self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": @@ -1292,7 +1292,7 @@ def translate_nls_cost_to_conl(self): self.model.cost_r_in_psi_expr_e = conl_res_e self.model.cost_psi_expr_e = .5 * conl_res_e.T @ ca.sparsify(ca.DM(self.cost.W_e)) @ conl_res_e else: - raise Exception(f"Initial cost type must be NONLINEAR_LS, got cost_type_e {self.cost.cost_type_e}.") + raise ValueError(f"Initial cost type must be NONLINEAR_LS, got cost_type_e {self.cost.cost_type_e}.") return @@ -1321,33 +1321,33 @@ def translate_cost_to_external_cost(self, cost_hessian: 'EXACT' or 'GAUSS_NEWTON', determines how the cost hessian is computed. """ if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: - raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") casadi_symbolics_type = type(self.model.x) # check p, p_values and append if p is not None: if p_values is None: - raise Exception("If p is not None, also p_values need to be provided.") + raise ValueError("If p is not None, also p_values need to be provided.") if not (is_column(p) and is_column(p_values)): - raise Exception("p, p_values need to be column vectors.") + raise ValueError("p, p_values need to be column vectors.") if p.shape[0] != p_values.shape[0]: - raise Exception(f"Mismatching shapes regarding p, p_values: p has shape {p.shape}, p_values has shape {p_values.shape}.") + raise ValueError(f"Mismatching shapes regarding p, p_values: p has shape {p.shape}, p_values has shape {p_values.shape}.") if not isinstance(p, casadi_symbolics_type): - raise Exception(f"p has wrong type, got {type(p)}, expected {casadi_symbolics_type}.") + raise TypeError(f"p has wrong type, got {type(p)}, expected {casadi_symbolics_type}.") self.model.p = ca.vertcat(self.model.p, p) self.parameter_values = np.concatenate((self.parameter_values, p_values)) if p_global is not None: if p_global_values is None: - raise Exception("If p_global is not None, also p_global_values need to be provided.") + raise ValueError("If p_global is not None, also p_global_values need to be provided.") if not (is_column(p_global) and is_column(p_global_values)): - raise Exception("p_global, p_global_values need to be column vectors.") + raise ValueError("p_global, p_global_values need to be column vectors.") if p_global.shape[0] != p_global_values.shape[0]: - raise Exception(f"Mismatching shapes regarding p_global, p_global_values: p_global has shape {p_global.shape}, p_global_values has shape {p_global_values.shape}.") + raise ValueError(f"Mismatching shapes regarding p_global, p_global_values: p_global has shape {p_global.shape}, p_global_values has shape {p_global_values.shape}.") if not isinstance(p_global, casadi_symbolics_type): - raise Exception(f"p_global has wrong type, got {type(p_global)}, expected {casadi_symbolics_type}.") + raise TypeError(f"p_global has wrong type, got {type(p_global)}, expected {casadi_symbolics_type}.") self.model.p_global = ca.vertcat(self.model.p_global, p_global) self.p_global_values = np.concatenate((self.p_global_values, p_global_values)) @@ -1372,19 +1372,19 @@ def translate_intial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, c yref_0 = self.cost.yref_0 else: if yref_0.shape[0] != self.cost.yref_0.shape[0]: - raise Exception(f"yref_0 has wrong shape, got {yref_0.shape}, expected {self.cost.yref_0.shape}.") + raise ValueError(f"yref_0 has wrong shape, got {yref_0.shape}, expected {self.cost.yref_0.shape}.") if not isinstance(yref_0, casadi_symbolics_type): - raise Exception(f"yref_0 has wrong type, got {type(yref_0)}, expected {casadi_symbolics_type}.") + raise TypeError(f"yref_0 has wrong type, got {type(yref_0)}, expected {casadi_symbolics_type}.") if W_0 is None: W_0 = self.cost.W_0 else: if W_0.shape != self.cost.W_0.shape: - raise Exception(f"W_0 has wrong shape, got {W_0.shape}, expected {self.cost.W_0.shape}.") + raise ValueError(f"W_0 has wrong shape, got {W_0.shape}, expected {self.cost.W_0.shape}.") if not isinstance(W_0, casadi_symbolics_type): - raise Exception(f"W_0 has wrong type, got {type(W_0)}, expected {casadi_symbolics_type}.") + raise TypeError(f"W_0 has wrong type, got {type(W_0)}, expected {casadi_symbolics_type}.") if self.cost.cost_type_0 == "LINEAR_LS": self.model.cost_expr_ext_cost_0 = \ @@ -1631,7 +1631,7 @@ def formulate_constraint_as_Huber_penalty( raise ValueError("Either upper or lower bound must be provided.") if self.cost.cost_type != "CONVEX_OVER_NONLINEAR": - raise Exception("Huber penalty is only supported for CONVEX_OVER_NONLINEAR cost type.") + raise ValueError("Huber penalty is only supported for CONVEX_OVER_NONLINEAR cost type.") if use_xgn and is_empty(self.model.cost_conl_custom_outer_hess): # switch to XGN Hessian start with exact Hessian of previously defined cost @@ -1835,7 +1835,7 @@ def augment_with_t0_param(self) -> None: # TODO only needed in benchmark for problems with time-varying references. # maybe remove this function and model.t0 from acados (and move to benchmark) if self.model.t0 is not None: - raise Exception("Parameter t0 is already present in the model.") + raise ValueError("Parameter t0 is already present in the model.") self.model.t0 = ca.SX.sym("t0") self.model.p = ca.vertcat(self.model.p, self.model.t0) self.parameter_values = np.append(self.parameter_values, [0.0]) diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 85aa43a656..e4855ff3f9 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -55,13 +55,13 @@ class AcadosOcpBatchSolver(): def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None, json_file: str = 'acados_ocp.json', build: bool = True, generate: bool = True, verbose: bool=True): if not isinstance(N_batch, int) or N_batch <= 0: - raise Exception("AcadosOcpBatchSolver: argument N_batch should be a positive integer.") + raise ValueError("AcadosOcpBatchSolver: argument N_batch should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = ocp.solver_options.num_threads_in_batch_solve print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead.") print("In the future, it should be passed explicitly in the AcadosOcpBatchSolver constructor.") if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: - raise Exception("AcadosOcpBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") + raise ValueError("AcadosOcpBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") if not ocp.solver_options.with_batch_functionality: print("Warning: Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False.") print("Attempting to compile with openmp nonetheless.") @@ -179,30 +179,30 @@ def eval_adjoint_solution_sensitivity(self, if seed_x is None: seed_x = [] elif not isinstance(seed_x, Sequence): - raise Exception(f"seed_x should be a Sequence, got {type(seed_x)}") + raise TypeError(f"seed_x should be a Sequence, got {type(seed_x)}") if seed_u is None: seed_u = [] elif not isinstance(seed_u, Sequence): - raise Exception(f"seed_u should be a Sequence, got {type(seed_u)}") + raise TypeError(f"seed_u should be a Sequence, got {type(seed_u)}") if len(seed_x) == 0 and len(seed_u) == 0: - raise Exception("seed_x and seed_u cannot both be empty.") + raise ValueError("seed_x and seed_u cannot both be empty.") if len(seed_x) > 0: if not isinstance(seed_x[0], tuple) or len(seed_x[0]) != 2: - raise Exception(f"seed_x[0] should be tuple of length 2, got seed_x[0] {seed_x[0]}") + raise TypeError(f"seed_x[0] should be tuple of length 2, got seed_x[0] {seed_x[0]}") s = seed_x[0][1] if not isinstance(s, np.ndarray): - raise Exception(f"seed_x[0][1] should be np.ndarray, got {type(s)}") + raise TypeError(f"seed_x[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_x[0][1].shape[2] if len(seed_u) > 0: if not isinstance(seed_u[0], tuple) or len(seed_u[0]) != 2: - raise Exception(f"seed_u[0] should be tuple of length 2, got seed_u[0] {seed_u[0]}") + raise ValueError(f"seed_u[0] should be tuple of length 2, got seed_u[0] {seed_u[0]}") s = seed_u[0][1] if not isinstance(s, np.ndarray): - raise Exception(f"seed_u[0][1] should be np.ndarray, got {type(s)}") + raise TypeError(f"seed_u[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_u[0][1].shape[2] if sanity_checks: @@ -220,11 +220,11 @@ def eval_adjoint_solution_sensitivity(self, for seed, name, dim in [(seed_x, "seed_x", nx,), (seed_u, "seed_u", nu)]: for stage, seed_stage in seed: if not isinstance(stage, int) or stage < 0 or stage > N_horizon: - raise Exception(f"AcadosOcpBatchSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") + raise ValueError(f"AcadosOcpBatchSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") if not isinstance(seed_stage, np.ndarray): - raise Exception(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") + raise TypeError(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") if seed_stage.shape != (self.N_batch, dim, n_seeds): - raise Exception(f"{name} for stage {stage} should have shape (N_batch, dim, n_seeds) = ({self.N_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") + raise ValueError(f"{name} for stage {stage} should have shape (N_batch, dim, n_seeds) = ({self.N_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") if with_respect_to == "p_global": field = "p_global".encode('utf-8') @@ -283,12 +283,12 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: field = field_.encode('utf-8') if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: - raise Exception(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') dim = self.ocp_solvers[0].get_dim_flat(field_) if value_.shape != (self.N_batch, dim): - raise Exception(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({self.N_batch}, {dim}), got {value_.shape}.') + raise ValueError(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({self.N_batch}, {dim}), got {value_.shape}.') value_ = value_.reshape((-1,), order='C') N_data = value_.shape[0] @@ -307,7 +307,7 @@ def get_flat(self, field_: str) -> np.ndarray: :returns: numpy array of shape (N_batch, n_field_total) """ if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: - raise Exception(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') field = field_.encode('utf-8') @@ -341,7 +341,7 @@ def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedBatchIterate) -> """ if self.N_batch != iterate.N_batch: - raise Exception(f"Wrong batch dimension. Expected {self.N_batch}, got {iterate.N_batch}") + raise ValueError(f"Wrong batch dimension. Expected {self.N_batch}, got {iterate.N_batch}") self.set_flat("x", iterate.x) self.set_flat("u", iterate.u) diff --git a/interfaces/acados_template/acados_template/acados_ocp_constraints.py b/interfaces/acados_template/acados_template/acados_ocp_constraints.py index 58ac37ca74..f04845b45f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_constraints.py +++ b/interfaces/acados_template/acados_template/acados_ocp_constraints.py @@ -779,7 +779,7 @@ def constr_type(self, constr_type): if constr_type in constr_types: self.__constr_type = constr_type else: - raise Exception('Invalid constr_type value. Possible values are:\n\n' \ + raise ValueError('Invalid constr_type value. Possible values are:\n\n' \ + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n') @constr_type_0.setter @@ -788,7 +788,7 @@ def constr_type_0(self, constr_type_0): if constr_type_0 in constr_types: self.__constr_type_0 = constr_type_0 else: - raise Exception('Invalid constr_type_0 value. Possible values are:\n\n' \ + raise ValueError('Invalid constr_type_0 value. Possible values are:\n\n' \ + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_0 + '.\n\n') @constr_type_e.setter @@ -797,7 +797,7 @@ def constr_type_e(self, constr_type_e): if constr_type_e in constr_types: self.__constr_type_e = constr_type_e else: - raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \ + raise ValueError('Invalid constr_type_e value. Possible values are:\n\n' \ + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n') # initial x @@ -821,7 +821,7 @@ def Jbx_0(self, Jbx_0): if isinstance(Jbx_0, np.ndarray): self.__idxbx_0 = J_to_idx(Jbx_0) else: - raise Exception('Invalid Jbx_0 value.') + raise ValueError('Invalid Jbx_0 value.') @idxbxe_0.setter def idxbxe_0(self, idxbxe_0): @@ -865,7 +865,7 @@ def Jbx(self, Jbx): if isinstance(Jbx, np.ndarray): self.__idxbx = J_to_idx(Jbx) else: - raise Exception('Invalid Jbx value.') + raise ValueError('Invalid Jbx value.') # bounds on u @lbu.setter @@ -888,7 +888,7 @@ def Jbu(self, Jbu): if isinstance(Jbu, np.ndarray): self.__idxbu = J_to_idx(Jbu) else: - raise Exception('Invalid Jbu value.') + raise ValueError('Invalid Jbu value.') # bounds on x at shooting node N @lbx_e.setter @@ -911,7 +911,7 @@ def Jbx_e(self, Jbx_e): if isinstance(Jbx_e, np.ndarray): self.__idxbx_e = J_to_idx(Jbx_e) else: - raise Exception('Invalid Jbx_e value.') + raise ValueError('Invalid Jbx_e value.') # polytopic constraints @D.setter @@ -1034,7 +1034,7 @@ def Jsbx(self, Jsbx): if isinstance(Jsbx, np.ndarray): self.__idxsbx = J_to_idx_slack(Jsbx) else: - raise Exception('Invalid Jsbx value, expected numpy array.') + raise TypeError('Invalid Jsbx value, expected numpy array.') # soft bounds on u @lsbu.setter @@ -1057,7 +1057,7 @@ def Jsbu(self, Jsbu): if isinstance(Jsbu, np.ndarray): self.__idxsbu = J_to_idx_slack(Jsbu) else: - raise Exception('Invalid Jsbu value.') + raise ValueError('Invalid Jsbu value.') # soft bounds on x at shooting node N @lsbx_e.setter @@ -1080,7 +1080,7 @@ def Jsbx_e(self, Jsbx_e): if isinstance(Jsbx_e, np.ndarray): self.__idxsbx_e = J_to_idx_slack(Jsbx_e) else: - raise Exception('Invalid Jsbx_e value.') + raise ValueError('Invalid Jsbx_e value.') # soft bounds on general linear constraints @lsg.setter @@ -1103,7 +1103,7 @@ def Jsg(self, Jsg): if isinstance(Jsg, np.ndarray): self.__idxsg = J_to_idx_slack(Jsg) else: - raise Exception('Invalid Jsg value, expected numpy array.') + raise TypeError('Invalid Jsg value, expected numpy array.') # soft bounds on nonlinear constraints @lsh.setter @@ -1127,7 +1127,7 @@ def Jsh(self, Jsh): if isinstance(Jsh, np.ndarray): self.__idxsh = J_to_idx_slack(Jsh) else: - raise Exception('Invalid Jsh value, expected numpy array.') + raise TypeError('Invalid Jsh value, expected numpy array.') # soft bounds on convex-over-nonlinear constraints @lsphi.setter @@ -1150,7 +1150,7 @@ def Jsphi(self, Jsphi): if isinstance(Jsphi, np.ndarray): self.__idxsphi = J_to_idx_slack(Jsphi) else: - raise Exception('Invalid Jsphi value, expected numpy array.') + raise TypeError('Invalid Jsphi value, expected numpy array.') # soft bounds on general linear constraints at shooting node N @lsg_e.setter @@ -1173,7 +1173,7 @@ def Jsg_e(self, Jsg_e): if isinstance(Jsg_e, np.ndarray): self.__idxsg_e = J_to_idx_slack(Jsg_e) else: - raise Exception('Invalid Jsg_e value, expected numpy array.') + raise TypeError('Invalid Jsg_e value, expected numpy array.') # soft bounds on nonlinear constraints at shooting node N @lsh_e.setter @@ -1196,7 +1196,7 @@ def Jsh_e(self, Jsh_e): if isinstance(Jsh_e, np.ndarray): self.__idxsh_e = J_to_idx_slack(Jsh_e) else: - raise Exception('Invalid Jsh_e value, expected numpy array.') + raise TypeError('Invalid Jsh_e value, expected numpy array.') # soft bounds on convex-over-nonlinear constraints at shooting node N @@ -1220,7 +1220,7 @@ def Jsphi_e(self, Jsphi_e): if isinstance(Jsphi_e, np.ndarray): self.__idxsphi_e = J_to_idx_slack(Jsphi_e) else: - raise Exception('Invalid Jsphi_e value.') + raise ValueError('Invalid Jsphi_e value.') # soft constraints at shooting node 0 @lsh_0.setter @@ -1243,7 +1243,7 @@ def Jsh_0(self, Jsh_0): if isinstance(Jsh_0, np.ndarray): self.__idxsh_0 = J_to_idx_slack(Jsh_0) else: - raise Exception('Invalid Jsh_0 value, expected numpy array.') + raise TypeError('Invalid Jsh_0 value, expected numpy array.') @lsphi_0.setter def lsphi_0(self, value): @@ -1265,7 +1265,7 @@ def Jsphi_0(self, Jsphi_0): if isinstance(Jsphi_0, np.ndarray): self.__idxsphi_0 = J_to_idx_slack(Jsphi_0) else: - raise Exception('Invalid Jsphi_0 value.') + raise ValueError('Invalid Jsphi_0 value.') def set(self, attr, value): setattr(self, attr, value) diff --git a/interfaces/acados_template/acados_template/acados_ocp_cost.py b/interfaces/acados_template/acados_template/acados_ocp_cost.py index b085ce5efe..e4eef65075 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_cost.py +++ b/interfaces/acados_template/acados_template/acados_ocp_cost.py @@ -197,7 +197,7 @@ def cost_ext_fun_type_0(self, cost_ext_fun_type_0): if cost_ext_fun_type_0 in ['casadi', 'generic']: self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 else: - raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array.') + raise TypeError('Invalid cost_ext_fun_type_0 value, expected numpy array.') # Lagrange term @property @@ -286,7 +286,7 @@ def cost_type(self, cost_type): if cost_type in cost_types: self.__cost_type = cost_type else: - raise Exception('Invalid cost_type value.') + raise ValueError('Invalid cost_type value.') @cost_type_0.setter def cost_type_0(self, cost_type_0): @@ -294,7 +294,7 @@ def cost_type_0(self, cost_type_0): if cost_type_0 in cost_types: self.__cost_type_0 = cost_type_0 else: - raise Exception('Invalid cost_type_0 value.') + raise ValueError('Invalid cost_type_0 value.') @W.setter def W(self, W): @@ -327,35 +327,35 @@ def Zl(self, Zl): if isinstance(Zl, np.ndarray): self.__Zl = Zl else: - raise Exception('Invalid Zl value, expected numpy array.') + raise TypeError('Invalid Zl value, expected numpy array.') @Zu.setter def Zu(self, Zu): if isinstance(Zu, np.ndarray): self.__Zu = Zu else: - raise Exception('Invalid Zu value, expected numpy array.') + raise TypeError('Invalid Zu value, expected numpy array.') @zl.setter def zl(self, zl): if isinstance(zl, np.ndarray): self.__zl = zl else: - raise Exception('Invalid zl value, expected numpy array.') + raise TypeError('Invalid zl value, expected numpy array.') @zu.setter def zu(self, zu): if isinstance(zu, np.ndarray): self.__zu = zu else: - raise Exception('Invalid zu value, expected numpy array.') + raise TypeError('Invalid zu value, expected numpy array.') @cost_ext_fun_type.setter def cost_ext_fun_type(self, cost_ext_fun_type): if cost_ext_fun_type in ['casadi', 'generic']: self.__cost_ext_fun_type = cost_ext_fun_type else: - raise Exception("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].") + raise ValueError("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].") # Mayer term @property @@ -459,7 +459,7 @@ def cost_type_e(self, cost_type_e): if cost_type_e in cost_types: self.__cost_type_e = cost_type_e else: - raise Exception('Invalid cost_type_e value.') + raise ValueError('Invalid cost_type_e value.') @W_e.setter def W_e(self, W_e): @@ -521,7 +521,7 @@ def cost_ext_fun_type_e(self, cost_ext_fun_type_e): if cost_ext_fun_type_e in ['casadi', 'generic']: self.__cost_ext_fun_type_e = cost_ext_fun_type_e else: - raise Exception("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].") + raise ValueError("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].") def set(self, attr, value): setattr(self, attr, value) diff --git a/interfaces/acados_template/acados_template/acados_ocp_iterate.py b/interfaces/acados_template/acados_template/acados_ocp_iterate.py index fe428b4061..a892d95ffa 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_iterate.py +++ b/interfaces/acados_template/acados_template/acados_ocp_iterate.py @@ -106,7 +106,7 @@ def as_array(self, field: str, ) -> np.ndarray: """ if field not in self.__iterate_fields: - raise Exception(f"Invalid field: got {field}, expected value in {self.__iterate_fields}") + raise ValueError(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] @@ -114,6 +114,6 @@ def as_array(self, field: str, ) -> np.ndarray: try: traj = np.array(traj_, dtype=float) except ValueError: - raise Exception(f"Stage-wise dimensions are not the same for {field} trajectory.") + raise ValueError(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 4e642995b3..eb311c1eec 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -1242,7 +1242,7 @@ def qp_solver(self, qp_solver): if qp_solver in qp_solvers: self.__qp_solver = qp_solver else: - raise Exception('Invalid qp_solver value. Possible values are:\n\n' \ + raise ValueError('Invalid qp_solver value. Possible values are:\n\n' \ + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\n') @@ -1253,7 +1253,7 @@ def regularize_method(self, regularize_method): if regularize_method in regularize_methods: self.__regularize_method = regularize_method else: - raise Exception('Invalid regularize_method value. Possible values are:\n\n' \ + raise ValueError('Invalid regularize_method value. Possible values are:\n\n' \ + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\n') @collocation_type.setter @@ -1261,7 +1261,7 @@ def collocation_type(self, collocation_type): if collocation_type in COLLOCATION_TYPES: self.__collocation_type = collocation_type else: - raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ + raise ValueError('Invalid collocation_type value. Possible values are:\n\n' \ + ',\n'.join(COLLOCATION_TYPES) + '.\n\nYou have: ' + collocation_type + '.\n\n') @hpipm_mode.setter @@ -1270,7 +1270,7 @@ def hpipm_mode(self, hpipm_mode): if hpipm_mode in hpipm_modes: self.__hpipm_mode = hpipm_mode else: - raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \ + raise ValueError('Invalid hpipm_mode value. Possible values are:\n\n' \ + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\n') @with_solution_sens_wrt_params.setter @@ -1278,44 +1278,44 @@ def with_solution_sens_wrt_params(self, with_solution_sens_wrt_params): if isinstance(with_solution_sens_wrt_params, bool): self.__with_solution_sens_wrt_params = with_solution_sens_wrt_params else: - raise Exception('Invalid with_solution_sens_wrt_params value. Expected bool.') + raise TypeError('Invalid with_solution_sens_wrt_params value. Expected bool.') @with_value_sens_wrt_params.setter def with_value_sens_wrt_params(self, with_value_sens_wrt_params): if isinstance(with_value_sens_wrt_params, bool): self.__with_value_sens_wrt_params = with_value_sens_wrt_params else: - raise Exception('Invalid with_value_sens_wrt_params value. Expected bool.') + raise TypeError('Invalid with_value_sens_wrt_params value. Expected bool.') @ext_fun_compile_flags.setter def ext_fun_compile_flags(self, ext_fun_compile_flags): if isinstance(ext_fun_compile_flags, str): self.__ext_fun_compile_flags = ext_fun_compile_flags else: - raise Exception('Invalid ext_fun_compile_flags value, expected a string.\n') + raise TypeError('Invalid ext_fun_compile_flags value, expected a string.\n') @ext_fun_expand_constr.setter def ext_fun_expand_constr(self, ext_fun_expand_constr): if not isinstance(ext_fun_expand_constr, bool): - raise Exception('Invalid ext_fun_expand_constr value, expected bool.\n') + raise TypeError('Invalid ext_fun_expand_constr value, expected bool.\n') self.__ext_fun_expand_constr = ext_fun_expand_constr @ext_fun_expand_cost.setter def ext_fun_expand_cost(self, ext_fun_expand_cost): if not isinstance(ext_fun_expand_cost, bool): - raise Exception('Invalid ext_fun_expand_cost value, expected bool.\n') + raise TypeError('Invalid ext_fun_expand_cost value, expected bool.\n') self.__ext_fun_expand_cost = ext_fun_expand_cost @ext_fun_expand_dyn.setter def ext_fun_expand_dyn(self, ext_fun_expand_dyn): if not isinstance(ext_fun_expand_dyn, bool): - raise Exception('Invalid ext_fun_expand_dyn value, expected bool.\n') + raise TypeError('Invalid ext_fun_expand_dyn value, expected bool.\n') self.__ext_fun_expand_dyn = ext_fun_expand_dyn @ext_fun_expand_precompute.setter def ext_fun_expand_precompute(self, ext_fun_expand_precompute): if not isinstance(ext_fun_expand_precompute, bool): - raise Exception('Invalid ext_fun_expand_precompute value, expected bool.\n') + raise TypeError('Invalid ext_fun_expand_precompute value, expected bool.\n') self.__ext_fun_expand_precompute = ext_fun_expand_precompute @custom_update_filename.setter @@ -1323,18 +1323,18 @@ def custom_update_filename(self, custom_update_filename): if isinstance(custom_update_filename, str): self.__custom_update_filename = custom_update_filename else: - raise Exception('Invalid custom_update_filename, expected a string.\n') + raise TypeError('Invalid custom_update_filename, expected a string.\n') @custom_templates.setter def custom_templates(self, custom_templates): if not isinstance(custom_templates, list): - raise Exception('Invalid custom_templates, expected a list.\n') + raise TypeError('Invalid custom_templates, expected a list.\n') for tup in custom_templates: if not isinstance(tup, tuple): - raise Exception('Invalid custom_templates, shoubld be list of tuples.\n') + raise TypeError('Invalid custom_templates, should be list of tuples.\n') for s in tup: if not isinstance(s, str): - raise Exception('Invalid custom_templates, shoubld be list of tuples of strings.\n') + raise TypeError('Invalid custom_templates, should be list of tuples of strings.\n') self.__custom_templates = custom_templates @custom_update_header_filename.setter @@ -1342,14 +1342,14 @@ def custom_update_header_filename(self, custom_update_header_filename): if isinstance(custom_update_header_filename, str): self.__custom_update_header_filename = custom_update_header_filename else: - raise Exception('Invalid custom_update_header_filename, expected a string.\n') + raise TypeError('Invalid custom_update_header_filename, expected a string.\n') @custom_update_copy.setter def custom_update_copy(self, custom_update_copy): if isinstance(custom_update_copy, bool): self.__custom_update_copy = custom_update_copy else: - raise Exception('Invalid custom_update_copy, expected a bool.\n') + raise TypeError('Invalid custom_update_copy, expected a bool.\n') @hessian_approx.setter def hessian_approx(self, hessian_approx): @@ -1357,7 +1357,7 @@ def hessian_approx(self, hessian_approx): if hessian_approx in hessian_approxs: self.__hessian_approx = hessian_approx else: - raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \ + raise ValueError('Invalid hessian_approx value. Possible values are:\n\n' \ + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\n') @integrator_type.setter @@ -1365,7 +1365,7 @@ def integrator_type(self, integrator_type): if integrator_type in INTEGRATOR_TYPES: self.__integrator_type = integrator_type else: - raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ + raise ValueError('Invalid integrator_type value. Possible values are:\n\n' \ + ',\n'.join(INTEGRATOR_TYPES) + '.\n\nYou have: ' + integrator_type + '.\n\n') @tf.setter @@ -1377,7 +1377,7 @@ def N_horizon(self, N_horizon): if isinstance(N_horizon, int) and N_horizon > 0: self.__N_horizon = N_horizon else: - raise Exception('Invalid N_horizon value, expected positive integer.') + raise ValueError('Invalid N_horizon value, expected positive integer.') @time_steps.setter def time_steps(self, time_steps): @@ -1404,7 +1404,7 @@ def globalization(self, globalization): if globalization in globalization_types: self.__globalization = globalization else: - raise Exception('Invalid globalization value. Possible values are:\n\n' \ + raise ValueError('Invalid globalization value. Possible values are:\n\n' \ + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\n') @reg_epsilon.setter @@ -1414,19 +1414,19 @@ def reg_epsilon(self, reg_epsilon): @reg_max_cond_block.setter def reg_max_cond_block(self, reg_max_cond_block): if not isinstance(reg_max_cond_block, float) or reg_max_cond_block < 1.0: - raise Exception('Invalid reg_max_cond_block value, expected float > 1.0.') + raise ValueError('Invalid reg_max_cond_block value, expected float > 1.0.') self.__reg_max_cond_block = reg_max_cond_block @reg_adaptive_eps.setter def reg_adaptive_eps(self, reg_adaptive_eps): if not isinstance(reg_adaptive_eps, bool): - raise Exception(f'Invalid reg_adaptive_eps value, expected bool, got {reg_adaptive_eps}') + raise TypeError(f'Invalid reg_adaptive_eps value, expected bool, got {reg_adaptive_eps}') self.__reg_adaptive_eps = reg_adaptive_eps @reg_min_epsilon.setter def reg_min_epsilon(self, reg_min_epsilon): if not isinstance(reg_min_epsilon, float) or reg_min_epsilon < 0: - raise Exception(f'Invalid reg_min_epsilon value, expected float > 0, got {reg_min_epsilon}') + raise ValueError(f'Invalid reg_min_epsilon value, expected float > 0, got {reg_min_epsilon}') self.__reg_min_epsilon = reg_min_epsilon @globalization_alpha_min.setter @@ -1452,7 +1452,7 @@ def globalization_line_search_use_sufficient_descent(self, globalization_line_se if globalization_line_search_use_sufficient_descent in [0, 1]: self.__globalization_line_search_use_sufficient_descent = globalization_line_search_use_sufficient_descent else: - raise Exception(f'Invalid value for globalization_line_search_use_sufficient_descent. Possible values are 0, 1, got {globalization_line_search_use_sufficient_descent}') + raise ValueError(f'Invalid value for globalization_line_search_use_sufficient_descent. Possible values are 0, 1, got {globalization_line_search_use_sufficient_descent}') @line_search_use_sufficient_descent.setter def line_search_use_sufficient_descent(self, globalization_line_search_use_sufficient_descent): @@ -1460,21 +1460,21 @@ def line_search_use_sufficient_descent(self, globalization_line_search_use_suffi if globalization_line_search_use_sufficient_descent in [0, 1]: self.__globalization_line_search_use_sufficient_descent = globalization_line_search_use_sufficient_descent else: - raise Exception(f'Invalid value for globalization_line_search_use_sufficient_descent. Possible values are 0, 1, got {globalization_line_search_use_sufficient_descent}') + raise ValueError(f'Invalid value for globalization_line_search_use_sufficient_descent. Possible values are 0, 1, got {globalization_line_search_use_sufficient_descent}') @globalization_use_SOC.setter def globalization_use_SOC(self, globalization_use_SOC): if globalization_use_SOC in [0, 1]: self.__globalization_use_SOC = globalization_use_SOC else: - raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}') + raise ValueError(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}') @globalization_full_step_dual.setter def globalization_full_step_dual(self, globalization_full_step_dual): if globalization_full_step_dual in [0, 1]: self.__globalization_full_step_dual = globalization_full_step_dual else: - raise Exception(f'Invalid value for globalization_full_step_dual. Possible values are 0, 1, got {globalization_full_step_dual}') + raise ValueError(f'Invalid value for globalization_full_step_dual. Possible values are 0, 1, got {globalization_full_step_dual}') @full_step_dual.setter def full_step_dual(self, globalization_full_step_dual): @@ -1487,63 +1487,63 @@ def globalization_funnel_init_increase_factor(self, globalization_funnel_init_in if globalization_funnel_init_increase_factor > 1.0: self.__globalization_funnel_init_increase_factor = globalization_funnel_init_increase_factor else: - raise Exception(f'Invalid value for globalization_funnel_init_increase_factor. Should be > 1, got {globalization_funnel_init_increase_factor}') + raise ValueError(f'Invalid value for globalization_funnel_init_increase_factor. Should be > 1, got {globalization_funnel_init_increase_factor}') @globalization_funnel_init_upper_bound.setter def globalization_funnel_init_upper_bound(self, globalization_funnel_init_upper_bound): if globalization_funnel_init_upper_bound > 0.0: self.__globalization_funnel_init_upper_bound = globalization_funnel_init_upper_bound else: - raise Exception(f'Invalid value for globalization_funnel_init_upper_bound. Should be > 0, got {globalization_funnel_init_upper_bound}') + raise ValueError(f'Invalid value for globalization_funnel_init_upper_bound. Should be > 0, got {globalization_funnel_init_upper_bound}') @globalization_funnel_sufficient_decrease_factor.setter def globalization_funnel_sufficient_decrease_factor(self, globalization_funnel_sufficient_decrease_factor): if globalization_funnel_sufficient_decrease_factor > 0.0 and globalization_funnel_sufficient_decrease_factor < 1.0: self.__globalization_funnel_sufficient_decrease_factor = globalization_funnel_sufficient_decrease_factor else: - raise Exception(f'Invalid value for globalization_funnel_sufficient_decrease_factor. Should be in (0,1), got {globalization_funnel_sufficient_decrease_factor}') + raise ValueError(f'Invalid value for globalization_funnel_sufficient_decrease_factor. Should be in (0,1), got {globalization_funnel_sufficient_decrease_factor}') @globalization_funnel_kappa.setter def globalization_funnel_kappa(self, globalization_funnel_kappa): if globalization_funnel_kappa > 0.0 and globalization_funnel_kappa < 1.0: self.__globalization_funnel_kappa = globalization_funnel_kappa else: - raise Exception(f'Invalid value for globalization_funnel_kappa. Should be in (0,1), got {globalization_funnel_kappa}') + raise ValueError(f'Invalid value for globalization_funnel_kappa. Should be in (0,1), got {globalization_funnel_kappa}') @globalization_funnel_fraction_switching_condition.setter def globalization_funnel_fraction_switching_condition(self, globalization_funnel_fraction_switching_condition): if globalization_funnel_fraction_switching_condition > 0.0 and globalization_funnel_fraction_switching_condition < 1.0: self.__globalization_funnel_fraction_switching_condition = globalization_funnel_fraction_switching_condition else: - raise Exception(f'Invalid value for globalization_funnel_fraction_switching_condition. Should be in (0,1), got {globalization_funnel_fraction_switching_condition}') + raise ValueError(f'Invalid value for globalization_funnel_fraction_switching_condition. Should be in (0,1), got {globalization_funnel_fraction_switching_condition}') @globalization_funnel_initial_penalty_parameter.setter def globalization_funnel_initial_penalty_parameter(self, globalization_funnel_initial_penalty_parameter): if globalization_funnel_initial_penalty_parameter >= 0.0 and globalization_funnel_initial_penalty_parameter <= 1.0: self.__globalization_funnel_initial_penalty_parameter = globalization_funnel_initial_penalty_parameter else: - raise Exception(f'Invalid value for globalization_funnel_initial_penalty_parameter. Should be in [0,1], got {globalization_funnel_initial_penalty_parameter}') + raise ValueError(f'Invalid value for globalization_funnel_initial_penalty_parameter. Should be in [0,1], got {globalization_funnel_initial_penalty_parameter}') @globalization_funnel_use_merit_fun_only.setter def globalization_funnel_use_merit_fun_only(self, globalization_funnel_use_merit_fun_only): if isinstance(globalization_funnel_use_merit_fun_only, bool): self.__globalization_funnel_use_merit_fun_only = globalization_funnel_use_merit_fun_only else: - raise Exception(f'Invalid type for globalization_funnel_use_merit_fun_only. Should be bool, got {globalization_funnel_use_merit_fun_only}') + raise TypeError(f'Invalid type for globalization_funnel_use_merit_fun_only. Should be bool, got {globalization_funnel_use_merit_fun_only}') @eval_residual_at_max_iter.setter def eval_residual_at_max_iter(self, eval_residual_at_max_iter): if isinstance(eval_residual_at_max_iter, bool): self.__eval_residual_at_max_iter = eval_residual_at_max_iter else: - raise Exception(f'Invalid datatype for eval_residual_at_max_iter. Should be bool, got {type(eval_residual_at_max_iter)}') + raise TypeError(f'Invalid datatype for eval_residual_at_max_iter. Should be bool, got {type(eval_residual_at_max_iter)}') @use_constraint_hessian_in_feas_qp.setter def use_constraint_hessian_in_feas_qp(self, use_constraint_hessian_in_feas_qp): if isinstance(use_constraint_hessian_in_feas_qp, bool): self.__use_constraint_hessian_in_feas_qp = use_constraint_hessian_in_feas_qp else: - raise Exception(f'Invalid datatype for use_constraint_hessian_in_feas_qp. Should be bool, got {type(use_constraint_hessian_in_feas_qp)}') + raise TypeError(f'Invalid datatype for use_constraint_hessian_in_feas_qp. Should be bool, got {type(use_constraint_hessian_in_feas_qp)}') @search_direction_mode.setter def search_direction_mode(self, search_direction_mode): @@ -1552,23 +1552,23 @@ def search_direction_mode(self, search_direction_mode): if search_direction_mode in search_direction_modes: self.__search_direction_mode = search_direction_mode else: - raise Exception(f'Invalid string for search_direction_mode. Possible search_direction_modes are'+', '.join(search_direction_modes) + f', got {search_direction_mode}') + raise ValueError(f'Invalid string for search_direction_mode. Possible search_direction_modes are'+', '.join(search_direction_modes) + f', got {search_direction_mode}') else: - raise Exception(f'Invalid datatype for search_direction_mode. Should be str, got {type(search_direction_mode)}') + raise TypeError(f'Invalid datatype for search_direction_mode. Should be str, got {type(search_direction_mode)}') @allow_direction_mode_switch_to_nominal.setter def allow_direction_mode_switch_to_nominal(self, allow_direction_mode_switch_to_nominal): if isinstance(allow_direction_mode_switch_to_nominal, bool): self.__allow_direction_mode_switch_to_nominal = allow_direction_mode_switch_to_nominal else: - raise Exception(f'Invalid datatype for allow_direction_mode_switch_to_nominal. Should be str, got {type(allow_direction_mode_switch_to_nominal)}') + raise TypeError(f'Invalid datatype for allow_direction_mode_switch_to_nominal. Should be str, got {type(allow_direction_mode_switch_to_nominal)}') @globalization_eps_sufficient_descent.setter def globalization_eps_sufficient_descent(self, globalization_eps_sufficient_descent): if isinstance(globalization_eps_sufficient_descent, float) and globalization_eps_sufficient_descent > 0: self.__globalization_eps_sufficient_descent = globalization_eps_sufficient_descent else: - raise Exception('Invalid globalization_eps_sufficient_descent value. globalization_eps_sufficient_descent must be a positive float.') + raise ValueError('Invalid globalization_eps_sufficient_descent value. globalization_eps_sufficient_descent must be a positive float.') @eps_sufficient_descent.setter def eps_sufficient_descent(self, globalization_eps_sufficient_descent): @@ -1581,7 +1581,7 @@ def sim_method_num_stages(self, sim_method_num_stages): # if isinstance(sim_method_num_stages, int): # self.__sim_method_num_stages = sim_method_num_stages # else: - # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer.') + # raise ValueError('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer.') self.__sim_method_num_stages = sim_method_num_stages @@ -1591,7 +1591,7 @@ def sim_method_num_steps(self, sim_method_num_steps): # if isinstance(sim_method_num_steps, int): # self.__sim_method_num_steps = sim_method_num_steps # else: - # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer.') + # raise ValueError('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer.') self.__sim_method_num_steps = sim_method_num_steps @@ -1601,14 +1601,14 @@ def sim_method_newton_iter(self, sim_method_newton_iter): if isinstance(sim_method_newton_iter, int): self.__sim_method_newton_iter = sim_method_newton_iter else: - raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer.') + raise ValueError('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer.') @sim_method_newton_tol.setter def sim_method_newton_tol(self, sim_method_newton_tol): if isinstance(sim_method_newton_tol, float) and sim_method_newton_tol > 0: self.__sim_method_newton_tol = sim_method_newton_tol else: - raise Exception('Invalid sim_method_newton_tol value. sim_method_newton_tol must be a positive float.') + raise ValueError('Invalid sim_method_newton_tol value. sim_method_newton_tol must be a positive float.') @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): @@ -1620,7 +1620,7 @@ def nlp_solver_type(self, nlp_solver_type): if nlp_solver_type in nlp_solver_types: self.__nlp_solver_type = nlp_solver_type else: - raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ + raise ValueError('Invalid nlp_solver_type value. Possible values are:\n\n' \ + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\n') @cost_discretization.setter @@ -1628,7 +1628,7 @@ def cost_discretization(self, cost_discretization): if cost_discretization in COST_DISCRETIZATION_TYPES: self.__cost_discretization = cost_discretization else: - raise Exception('Invalid cost_discretization value. Possible values are:\n\n' \ + raise ValueError('Invalid cost_discretization value. Possible values are:\n\n' \ + ',\n'.join(COST_DISCRETIZATION_TYPES) + '.\n\nYou have: ' + cost_discretization + '.') @globalization_fixed_step_length.setter @@ -1636,7 +1636,7 @@ def globalization_fixed_step_length(self, globalization_fixed_step_length): if isinstance(globalization_fixed_step_length, float) and globalization_fixed_step_length >= 0.: self.__globalization_fixed_step_length = globalization_fixed_step_length else: - raise Exception('Invalid globalization_fixed_step_length value. globalization_fixed_step_length must be a positive float.') + raise ValueError('Invalid globalization_fixed_step_length value. globalization_fixed_step_length must be a positive float.') @nlp_solver_step_length.setter def nlp_solver_step_length(self, nlp_solver_step_length): @@ -1648,12 +1648,12 @@ def nlp_solver_warm_start_first_qp(self, nlp_solver_warm_start_first_qp): if isinstance(nlp_solver_warm_start_first_qp, bool): self.__nlp_solver_warm_start_first_qp = nlp_solver_warm_start_first_qp else: - raise Exception('Invalid nlp_solver_warm_start_first_qp value. Expected bool.') + raise TypeError('Invalid nlp_solver_warm_start_first_qp value. Expected bool.') @nlp_solver_warm_start_first_qp_from_nlp.setter def nlp_solver_warm_start_first_qp_from_nlp(self, nlp_solver_warm_start_first_qp_from_nlp): if not isinstance(nlp_solver_warm_start_first_qp_from_nlp, bool): - raise Exception('Invalid nlp_solver_warm_start_first_qp_from_nlp value. Expected bool.') + raise TypeError('Invalid nlp_solver_warm_start_first_qp_from_nlp value. Expected bool.') self.__nlp_solver_warm_start_first_qp_from_nlp = nlp_solver_warm_start_first_qp_from_nlp @levenberg_marquardt.setter @@ -1661,112 +1661,112 @@ def levenberg_marquardt(self, levenberg_marquardt): if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0: self.__levenberg_marquardt = levenberg_marquardt else: - raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float.') + raise ValueError('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float.') @qp_solver_mu0.setter def qp_solver_mu0(self, qp_solver_mu0): if isinstance(qp_solver_mu0, float) and qp_solver_mu0 >= 0: self.__qp_solver_mu0 = qp_solver_mu0 else: - raise Exception('Invalid qp_solver_mu0 value. qp_solver_mu0 must be a positive float.') + raise ValueError('Invalid qp_solver_mu0 value. qp_solver_mu0 must be a positive float.') @qp_solver_t0_init.setter def qp_solver_t0_init(self, qp_solver_t0_init): if qp_solver_t0_init in [0, 1, 2]: self.__qp_solver_t0_init = qp_solver_t0_init else: - raise Exception('Invalid qp_solver_t0_init value. Must be in [0, 1, 2].') + raise ValueError('Invalid qp_solver_t0_init value. Must be in [0, 1, 2].') @tau_min.setter def tau_min(self, tau_min): if isinstance(tau_min, float) and tau_min >= 0: self.__tau_min = tau_min else: - raise Exception('Invalid tau_min value. tau_min must be a positive float.') + raise ValueError('Invalid tau_min value. tau_min must be a positive float.') @solution_sens_qp_t_lam_min.setter def solution_sens_qp_t_lam_min(self, solution_sens_qp_t_lam_min): if isinstance(solution_sens_qp_t_lam_min, float) and solution_sens_qp_t_lam_min >= 0: self.__solution_sens_qp_t_lam_min = solution_sens_qp_t_lam_min else: - raise Exception('Invalid solution_sens_qp_t_lam_min value. solution_sens_qp_t_lam_min must be a nonnegative float.') + raise ValueError('Invalid solution_sens_qp_t_lam_min value. solution_sens_qp_t_lam_min must be a nonnegative float.') @qp_solver_iter_max.setter def qp_solver_iter_max(self, qp_solver_iter_max): if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max >= 0: self.__qp_solver_iter_max = qp_solver_iter_max else: - raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.') + raise ValueError('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.') @with_adaptive_levenberg_marquardt.setter def with_adaptive_levenberg_marquardt(self, with_adaptive_levenberg_marquardt): if isinstance(with_adaptive_levenberg_marquardt, bool): self.__with_adaptive_levenberg_marquardt = with_adaptive_levenberg_marquardt else: - raise Exception('Invalid with_adaptive_levenberg_marquardt value. Expected bool.') + raise TypeError('Invalid with_adaptive_levenberg_marquardt value. Expected bool.') @adaptive_levenberg_marquardt_lam.setter def adaptive_levenberg_marquardt_lam(self, adaptive_levenberg_marquardt_lam): if isinstance(adaptive_levenberg_marquardt_lam, float) and adaptive_levenberg_marquardt_lam >= 1.0: self.__adaptive_levenberg_marquardt_lam = adaptive_levenberg_marquardt_lam else: - raise Exception('Invalid adaptive_levenberg_marquardt_lam value. adaptive_levenberg_marquardt_lam must be a float greater 1.0.') + raise ValueError('Invalid adaptive_levenberg_marquardt_lam value. adaptive_levenberg_marquardt_lam must be a float greater 1.0.') @adaptive_levenberg_marquardt_mu_min.setter def adaptive_levenberg_marquardt_mu_min(self, adaptive_levenberg_marquardt_mu_min): if isinstance(adaptive_levenberg_marquardt_mu_min, float) and adaptive_levenberg_marquardt_mu_min >= 0.0: self.__adaptive_levenberg_marquardt_mu_min = adaptive_levenberg_marquardt_mu_min else: - raise Exception('Invalid adaptive_levenberg_marquardt_mu_min value. adaptive_levenberg_marquardt_mu_min must be a positive float.') + raise ValueError('Invalid adaptive_levenberg_marquardt_mu_min value. adaptive_levenberg_marquardt_mu_min must be a positive float.') @adaptive_levenberg_marquardt_mu0.setter def adaptive_levenberg_marquardt_mu0(self, adaptive_levenberg_marquardt_mu0): if isinstance(adaptive_levenberg_marquardt_mu0, float) and adaptive_levenberg_marquardt_mu0 >= 0.0: self.__adaptive_levenberg_marquardt_mu0 = adaptive_levenberg_marquardt_mu0 else: - raise Exception('Invalid adaptive_levenberg_marquardt_mu0 value. adaptive_levenberg_marquardt_mu0 must be a positive float.') + raise ValueError('Invalid adaptive_levenberg_marquardt_mu0 value. adaptive_levenberg_marquardt_mu0 must be a positive float.') @log_primal_step_norm.setter def log_primal_step_norm(self, val): if isinstance(val, bool): self.__log_primal_step_norm = val else: - raise Exception('Invalid log_primal_step_norm value. Expected bool.') + raise TypeError('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.') + raise TypeError('Invalid store_iterates value. Expected bool.') @timeout_max_time.setter def timeout_max_time(self, val): if isinstance(val, float) and val >= 0: self.__timeout_max_time = val else: - raise Exception('Invalid timeout_max_time value. Expected nonnegative float.') + raise ValueError('Invalid timeout_max_time value. Expected nonnegative float.') @timeout_heuristic.setter def timeout_heuristic(self, val): if val in ["MAX_CALL", "MAX_OVERALL", "LAST", "AVERAGE", "ZERO"]: self.__timeout_heuristic = val else: - raise Exception('Invalid timeout_heuristic value. Expected value in ["MAX_CALL", "MAX_OVERALL", "LAST", "AVERAGE", "ZERO"].') + raise ValueError('Invalid timeout_heuristic value. Expected value in ["MAX_CALL", "MAX_OVERALL", "LAST", "AVERAGE", "ZERO"].') @as_rti_iter.setter def as_rti_iter(self, as_rti_iter): if isinstance(as_rti_iter, int) and as_rti_iter >= 0: self.__as_rti_iter = as_rti_iter else: - raise Exception('Invalid as_rti_iter value. as_rti_iter must be a nonnegative int.') + raise ValueError('Invalid as_rti_iter value. as_rti_iter must be a nonnegative int.') @as_rti_level.setter def as_rti_level(self, as_rti_level): if as_rti_level in [0, 1, 2, 3, 4]: self.__as_rti_level = as_rti_level else: - raise Exception('Invalid as_rti_level value must be in [0, 1, 2, 3, 4].') + raise ValueError('Invalid as_rti_level value must be in [0, 1, 2, 3, 4].') @qp_solver_ric_alg.setter @@ -1774,14 +1774,14 @@ def qp_solver_ric_alg(self, qp_solver_ric_alg): if qp_solver_ric_alg in [0, 1]: self.__qp_solver_ric_alg = qp_solver_ric_alg else: - raise Exception(f'Invalid qp_solver_ric_alg value. qp_solver_ric_alg must be in [0, 1], got {qp_solver_ric_alg}.') + raise ValueError(f'Invalid qp_solver_ric_alg value. qp_solver_ric_alg must be in [0, 1], got {qp_solver_ric_alg}.') @qp_solver_cond_ric_alg.setter def qp_solver_cond_ric_alg(self, qp_solver_cond_ric_alg): if qp_solver_cond_ric_alg in [0, 1]: self.__qp_solver_cond_ric_alg = qp_solver_cond_ric_alg else: - raise Exception(f'Invalid qp_solver_cond_ric_alg value. qp_solver_cond_ric_alg must be in [0, 1], got {qp_solver_cond_ric_alg}.') + raise ValueError(f'Invalid qp_solver_cond_ric_alg value. qp_solver_cond_ric_alg must be in [0, 1], got {qp_solver_cond_ric_alg}.') @qp_solver_cond_N.setter @@ -1789,15 +1789,15 @@ def qp_solver_cond_N(self, qp_solver_cond_N): if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0: self.__qp_solver_cond_N = qp_solver_cond_N else: - raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int.') + raise ValueError('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int.') @qp_solver_cond_block_size.setter def qp_solver_cond_block_size(self, qp_solver_cond_block_size): if not isinstance(qp_solver_cond_block_size, list): - raise Exception('Invalid qp_solver_cond_block_size value. qp_solver_cond_block_size must be a list of nonnegative integers.') + raise ValueError('Invalid qp_solver_cond_block_size value. qp_solver_cond_block_size must be a list of nonnegative integers.') for i in qp_solver_cond_block_size: if not isinstance(i, int) or not i >= 0: - raise Exception('Invalid qp_solver_cond_block_size value. qp_solver_cond_block_size must be a list of nonnegative integers.') + raise ValueError('Invalid qp_solver_cond_block_size value. qp_solver_cond_block_size must be a list of nonnegative integers.') self.__qp_solver_cond_block_size = qp_solver_cond_block_size @qp_solver_warm_start.setter @@ -1805,7 +1805,7 @@ def qp_solver_warm_start(self, qp_solver_warm_start): if qp_solver_warm_start in [0, 1, 2, 3]: self.__qp_solver_warm_start = qp_solver_warm_start else: - raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2 or 3.') + raise ValueError('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2 or 3.') @qp_tol.setter def qp_tol(self, qp_tol): @@ -1815,35 +1815,35 @@ def qp_tol(self, qp_tol): self.__qp_solver_tol_stat = qp_tol self.__qp_solver_tol_comp = qp_tol else: - raise Exception('Invalid qp_tol value. qp_tol must be a positive float.') + raise ValueError('Invalid qp_tol value. qp_tol must be a positive float.') @qp_solver_tol_stat.setter def qp_solver_tol_stat(self, qp_solver_tol_stat): if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0: self.__qp_solver_tol_stat = qp_solver_tol_stat else: - raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float.') + raise ValueError('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float.') @qp_solver_tol_eq.setter def qp_solver_tol_eq(self, qp_solver_tol_eq): if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0: self.__qp_solver_tol_eq = qp_solver_tol_eq else: - raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float.') + raise ValueError('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float.') @qp_solver_tol_ineq.setter def qp_solver_tol_ineq(self, qp_solver_tol_ineq): if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0: self.__qp_solver_tol_ineq = qp_solver_tol_ineq else: - raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float.') + raise ValueError('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float.') @qp_solver_tol_comp.setter def qp_solver_tol_comp(self, qp_solver_tol_comp): if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0: self.__qp_solver_tol_comp = qp_solver_tol_comp else: - raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float.') + raise ValueError('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float.') @tol.setter def tol(self, tol): @@ -1853,63 +1853,63 @@ def tol(self, tol): self.__nlp_solver_tol_stat = tol self.__nlp_solver_tol_comp = tol else: - raise Exception('Invalid tol value. tol must be a positive float.') + raise ValueError('Invalid tol value. tol must be a positive float.') @nlp_solver_tol_stat.setter def nlp_solver_tol_stat(self, nlp_solver_tol_stat): if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0: self.__nlp_solver_tol_stat = nlp_solver_tol_stat else: - raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float.') + raise ValueError('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float.') @nlp_solver_tol_eq.setter def nlp_solver_tol_eq(self, nlp_solver_tol_eq): if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0: self.__nlp_solver_tol_eq = nlp_solver_tol_eq else: - raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float.') + raise ValueError('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float.') @nlp_solver_tol_ineq.setter def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq): if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0: self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq else: - raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float.') + raise ValueError('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float.') @nlp_solver_tol_min_step_norm.setter def nlp_solver_tol_min_step_norm(self, nlp_solver_tol_min_step_norm): if isinstance(nlp_solver_tol_min_step_norm, float) and nlp_solver_tol_min_step_norm >= 0.0: self.__nlp_solver_tol_min_step_norm = nlp_solver_tol_min_step_norm else: - raise Exception('Invalid nlp_solver_tol_min_step_norm value. nlp_solver_tol_min_step_norm must be a positive float.') + raise ValueError('Invalid nlp_solver_tol_min_step_norm value. nlp_solver_tol_min_step_norm must be a positive float.') @nlp_solver_ext_qp_res.setter def nlp_solver_ext_qp_res(self, nlp_solver_ext_qp_res): if nlp_solver_ext_qp_res in [0, 1]: self.__nlp_solver_ext_qp_res = nlp_solver_ext_qp_res else: - raise Exception('Invalid nlp_solver_ext_qp_res value. nlp_solver_ext_qp_res must be in [0, 1].') + raise ValueError('Invalid nlp_solver_ext_qp_res value. nlp_solver_ext_qp_res must be in [0, 1].') @rti_log_residuals.setter def rti_log_residuals(self, rti_log_residuals): if rti_log_residuals in [0, 1]: self.__rti_log_residuals = rti_log_residuals else: - raise Exception('Invalid rti_log_residuals value. rti_log_residuals must be in [0, 1].') + raise ValueError('Invalid rti_log_residuals value. rti_log_residuals must be in [0, 1].') @rti_log_only_available_residuals.setter def rti_log_only_available_residuals(self, rti_log_only_available_residuals): if rti_log_only_available_residuals in [0, 1]: self.__rti_log_only_available_residuals = rti_log_only_available_residuals else: - raise Exception('Invalid rti_log_only_available_residuals value. rti_log_only_available_residuals must be in [0, 1].') + raise ValueError('Invalid rti_log_only_available_residuals value. rti_log_only_available_residuals must be in [0, 1].') @nlp_solver_tol_comp.setter def nlp_solver_tol_comp(self, nlp_solver_tol_comp): if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0: self.__nlp_solver_tol_comp = nlp_solver_tol_comp else: - raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float.') + raise ValueError('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float.') @nlp_solver_max_iter.setter def nlp_solver_max_iter(self, nlp_solver_max_iter): @@ -1917,33 +1917,33 @@ def nlp_solver_max_iter(self, nlp_solver_max_iter): if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter >= 0: self.__nlp_solver_max_iter = nlp_solver_max_iter else: - raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a nonnegative int.') + raise ValueError('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a nonnegative int.') @print_level.setter def print_level(self, print_level): if isinstance(print_level, int) and print_level >= 0: self.__print_level = print_level else: - raise Exception('Invalid print_level value. print_level takes one of the values >=0.') + raise ValueError('Invalid print_level value. print_level takes one of the values >=0.') @model_external_shared_lib_dir.setter def model_external_shared_lib_dir(self, model_external_shared_lib_dir): if isinstance(model_external_shared_lib_dir, str) : self.__model_external_shared_lib_dir = model_external_shared_lib_dir else: - raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \ + raise TypeError('Invalid model_external_shared_lib_dir value. Str expected.' \ + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\n') @model_external_shared_lib_name.setter def model_external_shared_lib_name(self, model_external_shared_lib_name): if isinstance(model_external_shared_lib_name, str) : if model_external_shared_lib_name[-3:] == '.so' : - raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ + raise ValueError('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') else : self.__model_external_shared_lib_name = model_external_shared_lib_name else: - raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \ + raise TypeError('Invalid model_external_shared_lib_name value. Str expected.' + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') @exact_hess_constr.setter @@ -1951,35 +1951,35 @@ def exact_hess_constr(self, exact_hess_constr): if exact_hess_constr in [0, 1]: self.__exact_hess_constr = exact_hess_constr else: - raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1.') + raise ValueError('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1.') @exact_hess_cost.setter def exact_hess_cost(self, exact_hess_cost): if exact_hess_cost in [0, 1]: self.__exact_hess_cost = exact_hess_cost else: - raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1.') + raise ValueError('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1.') @exact_hess_dyn.setter def exact_hess_dyn(self, exact_hess_dyn): if exact_hess_dyn in [0, 1]: self.__exact_hess_dyn = exact_hess_dyn else: - raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1.') + raise ValueError('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1.') @fixed_hess.setter def fixed_hess(self, fixed_hess): if fixed_hess in [0, 1]: self.__fixed_hess = fixed_hess else: - raise Exception('Invalid fixed_hess value. fixed_hess takes one of the values 0, 1.') + raise ValueError('Invalid fixed_hess value. fixed_hess takes one of the values 0, 1.') @ext_cost_num_hess.setter def ext_cost_num_hess(self, ext_cost_num_hess): if ext_cost_num_hess in [0, 1]: self.__ext_cost_num_hess = ext_cost_num_hess else: - raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.') + raise ValueError('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.') @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): @@ -1987,7 +1987,7 @@ def num_threads_in_batch_solve(self, num_threads_in_batch_solve): if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: - raise Exception('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') + raise ValueError('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') @with_batch_functionality.setter def with_batch_functionality(self, with_batch_functionality): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 1b6bcad28d..baa434f0c5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -107,7 +107,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: acados_ocp.json_file = json_file if simulink_opts is not None: if acados_ocp.simulink_opts is not None: - raise Exception('simulink_opts are already set in acados_ocp.') + raise RuntimeError('simulink_opts are already set in acados_ocp.') else: acados_ocp.simulink_opts = simulink_opts @@ -216,14 +216,14 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__p_global_values = np.array([]) if not (isinstance(acados_ocp, (AcadosOcp, AcadosMultiphaseOcp)) or acados_ocp is None): - raise Exception('acados_ocp should be of type AcadosOcp or AcadosMultiphaseOcp.') + raise TypeError('acados_ocp should be of type AcadosOcp or AcadosMultiphaseOcp.') if acados_ocp is None: if json_file is None: - raise Exception('json_file should be provided if acados_ocp is None.') + raise ValueError('json_file should be provided if acados_ocp is None.') if generate or build: - raise Exception('generate and build should be False if acados_ocp is None.') + raise ValueError('generate and build should be False if acados_ocp is None.') if not os.path.exists(json_file): - raise Exception(f'json_file {json_file} does not exist.') + raise FileNotFoundError(f'json_file {json_file} does not exist.') if generate: if json_file is not None: @@ -470,7 +470,7 @@ def solve_for_x0(self, x0_bar, fail_on_nonzero_status=True, print_stats_on_failu if print_stats_on_failure: self.print_statistics() if fail_on_nonzero_status: - raise Exception(f'acados acados_ocp_solver returned status {status}') + raise RuntimeError(f'acados acados_ocp_solver returned status {status}') elif print_stats_on_failure: print(f'Warning: acados acados_ocp_solver returned status {status}') @@ -501,7 +501,7 @@ def setup_qp_matrices_and_factorize(self) -> int: This is only implemented for HPIPM QP solver without condensing. """ if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM' or self.__solver_options["qp_solver_cond_N"] != self.N: - raise Exception('This function is only implemented for HPIPM QP solver without condensing!') + raise NotImplementedError('This function is only implemented for HPIPM QP solver without condensing!') self.status = getattr(self.shared_lib, f"{self.name}_acados_setup_qp_matrices_and_factorize")(self.capsule) @@ -513,7 +513,7 @@ def get_dim_flat(self, field: str): Get dimension of flattened iterate. """ if field not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: - raise Exception(f'AcadosOcpSolver.get_dim_flat(field={field}): \'{field}\' is an invalid argument.') + raise ValueError(f'AcadosOcpSolver.get_dim_flat(field={field}): \'{field}\' is an invalid argument.') return self.__acados_lib.ocp_nlp_dims_get_total_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, field.encode('utf-8')) @@ -553,11 +553,11 @@ def set_new_time_steps(self, new_time_steps): do not require a new code export and compilation. """ if self.__problem_class == "MOCP": - raise Exception('This function can only be used for single phase OCPs!') + raise ValueError('This function can only be used for single phase OCPs!') # unlikely but still possible if not self.solver_created: - raise Exception('Solver was not yet created!') + raise RuntimeError('Solver was not yet created!') # check if time steps really changed in value if np.array_equal(self.__solver_options['time_steps'], new_time_steps): @@ -604,13 +604,13 @@ def update_qp_solver_cond_N(self, qp_solver_cond_N: int): `qp_solver_cond_N < N`. """ if self.__problem_class == "MOCP": - raise Exception('This function can only be used for single phase OCPs!') + raise ValueError('This function can only be used for single phase OCPs!') # unlikely but still possible if not self.solver_created: - raise Exception('Solver was not yet created!') + raise RuntimeError('Solver was not yet created!') if self.N < qp_solver_cond_N: - raise Exception('Setting qp_solver_cond_N to be larger than N does not work!') + raise ValueError('Setting qp_solver_cond_N to be larger than N does not work!') if self.__solver_options['qp_solver_cond_N'] != qp_solver_cond_N: self.solver_created = False @@ -650,7 +650,7 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st if with_respect_to == "initial_state": if not self.__has_x0: - raise Exception("OCP does not have an initial state constraint.") + raise ValueError("OCP does not have an initial state constraint.") nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) nbu = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "lbu".encode('utf-8')) @@ -668,7 +668,7 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st ubu = self.get_from_qp_in(0, 'ubu') if not (nbu == nu and np.all(lbu == ubu) and self.__nsbu_0 == 0): - raise Exception("OCP does not have an equality constraint on the initial control.") + raise ValueError("OCP does not have an equality constraint on the initial control.") lam = self.get(0, 'lam') nlam_non_slack = lam.shape[0]//2 - ns @@ -685,7 +685,7 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st self.time_value_grad = time.time() - t0 else: - raise Exception(f"AcadosOcpSolver.eval_and_get_optimal_value_gradient(): Unknown field: with_respect_to = {with_respect_to}") + raise ValueError(f"AcadosOcpSolver.eval_and_get_optimal_value_gradient(): Unknown field: with_respect_to = {with_respect_to}") return grad @@ -697,13 +697,13 @@ def get_optimal_value_gradient(self, with_respect_to: str = "initial_state") -> def _sanity_check_solution_sensitivities(self, parametric=True) -> None: if not (self.__solver_options["qp_solver"] == 'FULL_CONDENSING_HPIPM' or self.__solver_options["qp_solver"] == 'PARTIAL_CONDENSING_HPIPM'): - raise Exception("Parametric sensitivities are only available with HPIPM as QP solver.") + raise NotImplementedError("Parametric sensitivities are only available with HPIPM as QP solver.") if not self.__uses_exact_hessian: - raise Exception("Parametric sensitivities are only correct if an exact Hessian is used!") + raise ValueError("Parametric sensitivities are only correct if an exact Hessian is used!") if parametric and not self.__solver_options["with_solution_sens_wrt_params"]: - raise Exception("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") + raise ValueError("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") def eval_solution_sensitivity(self, @@ -769,7 +769,7 @@ def eval_solution_sensitivity(self, if sanity_checks: for s in stages_: if not isinstance(s, int) or s < 0 or s > self.N: - raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or list[int] and in [0, N], got stages = {stages_}.") + raise TypeError(f"AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or list[int] and in [0, N], got stages = {stages_}.") if with_respect_to == "initial_state": nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) @@ -791,7 +791,7 @@ def eval_solution_sensitivity(self, self.time_solution_sens_lin = time.time() - t0 else: - raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): Unknown field: with_respect_to = {with_respect_to}") + raise ValueError(f"AcadosOcpSolver.eval_solution_sensitivity(): Unknown field: with_respect_to = {with_respect_to}") # initialize jacobians with zeros for s in stages_: @@ -890,28 +890,28 @@ def eval_adjoint_solution_sensitivity(self, if seed_x is None: seed_x = [] elif not isinstance(seed_x, Sequence): - raise Exception(f"seed_x should be a Sequence, got {type(seed_x)}") + raise TypeError(f"seed_x should be a Sequence, got {type(seed_x)}") if seed_u is None: seed_u = [] elif not isinstance(seed_u, Sequence): - raise Exception(f"seed_u should be a Sequence, got {type(seed_u)}") + raise TypeError(f"seed_u should be a Sequence, got {type(seed_u)}") if len(seed_x) == 0 and len(seed_u) == 0: - raise Exception("seed_x and seed_u cannot both be empty.") + raise ValueError("seed_x and seed_u cannot both be empty.") if len(seed_x) > 0: if not isinstance(seed_x[0], tuple) or len(seed_x[0]) != 2: - raise Exception(f"seed_x[0] should be tuple of length 2, got seed_x[0] {seed_x[0]}") + raise TypeError(f"seed_x[0] should be tuple of length 2, got seed_x[0] {seed_x[0]}") s = seed_x[0][1] if not isinstance(s, np.ndarray): - raise Exception(f"seed_x[0][1] should be np.ndarray, got {type(s)}") + raise TypeError(f"seed_x[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_x[0][1].shape[1] if len(seed_u) > 0: if not isinstance(seed_u[0], tuple) or len(seed_u[0]) != 2: - raise Exception(f"seed_u[0] should be tuple of length 2, got seed_u[0] {seed_u[0]}") + raise ValueError(f"seed_u[0] should be tuple of length 2, got seed_u[0] {seed_u[0]}") s = seed_u[0][1] if not isinstance(s, np.ndarray): - raise Exception(f"seed_u[0][1] should be np.ndarray, got {type(s)}") + raise TypeError(f"seed_u[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_u[0][1].shape[1] if sanity_checks: @@ -923,11 +923,11 @@ def eval_adjoint_solution_sensitivity(self, for seed, name, dim in [(seed_x, "seed_x", nx), (seed_u, "seed_u", nu)]: for stage, seed_stage in seed: if not isinstance(stage, int) or stage < 0 or stage > self.N: - raise Exception(f"AcadosOcpSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") + raise ValueError(f"AcadosOcpSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") if not isinstance(seed_stage, np.ndarray): - raise Exception(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") + raise TypeError(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") if seed_stage.shape != (dim, n_seeds): - raise Exception(f"{name} for stage {stage} should have shape (dim, n_seeds) = ({dim}, {n_seeds}), got {seed_stage.shape}.") + raise ValueError(f"{name} for stage {stage} should have shape (dim, n_seeds) = ({dim}, {n_seeds}), got {seed_stage.shape}.") if with_respect_to == "p_global": field = "p_global".encode('utf-8') @@ -986,21 +986,21 @@ def eval_param_sens(self, index: int, stage: int=0, field="ex"): field = field.encode('utf-8') if not isinstance(index, int): - raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.') + raise TypeError('AcadosOcpSolver.eval_param_sens(): index must be Integer.') if field == "ex": if not stage == 0: - raise Exception('AcadosOcpSolver.eval_param_sens(): only stage == 0 is supported.') + raise NotImplementedError('AcadosOcpSolver.eval_param_sens(): only stage == 0 is supported.') nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, "x".encode('utf-8')) if index < 0 or index > nx: - raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.') + raise ValueError(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.') elif field == "p_global": nparam = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "p".encode('utf-8')) if index < 0 or index > nparam: - raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nparam-1], got: {index}.') + raise IndexError(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nparam-1], got: {index}.') # actual eval_param self.__acados_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out) @@ -1033,17 +1033,17 @@ def get(self, stage_: int, field_: str): all_fields = out_fields + in_fields + sens_fields if (field_ not in all_fields): - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): \'{field_}\' is an invalid argument.\ - \n Possible values are {all_fields}.') + raise ValueError(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): \'{field_}\' is an invalid argument.' + f'\n Possible values are {all_fields}.') if not isinstance(stage_, int): - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be an integer, got type {type(stage_)}.') + raise TypeError(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be an integer, got type {type(stage_)}.') if stage_ < 0 or stage_ > self.N: - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be in [0, {self.N}], got: {stage_}.') + raise ValueError(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be in [0, {self.N}], got: {stage_}.') if stage_ == self.N and field_ == 'pi': - raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): field \'{field_}\' does not exist at final stage {stage_}.') + raise KeyError(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): field \'{field_}\' does not exist at final stage {stage_}.') field = field_.replace('sens_', '') if field_ in sens_fields else field_ field = field.encode('utf-8') @@ -1072,11 +1072,11 @@ def get_flat(self, field_: str) -> np.ndarray: In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n """ if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global']: - raise Exception(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') if field_ == 'p_global': if not self.__save_p_global: - raise Exception(f'The field \'{field_}\' is not stored within the solver by default. Please set the option \'save_p_global=True\' when instantiating the solver.') + raise ValueError(f'The field \'{field_}\' is not stored within the solver by default. Please set the option \'save_p_global=True\' when instantiating the solver.') return self.__p_global_values field = field_.encode('utf-8') @@ -1099,11 +1099,11 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: """ field = field_.encode('utf-8') if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: - raise Exception(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') dims = self.__acados_lib.ocp_nlp_dims_get_total_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, field) if len(value_) != dims: - raise Exception(f'AcadosOcpSolver.set_flat(field={field_}, value): value has wrong length, expected {dims}, got {len(value_)}.') + raise ValueError(f'AcadosOcpSolver.set_flat(field={field_}, value): value has wrong length, expected {dims}, got {len(value_)}.') value_ = value_.astype(float) value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -1437,7 +1437,7 @@ def load_iterate(self, filename:str, verbose: bool = True): Note: This does not contain the iterate of the integrators, and the parameters. """ if not os.path.isfile(filename): - raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + raise FileNotFoundError('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) with open(filename, 'r') as f: solution = json.load(f) @@ -1638,7 +1638,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if self.__solver_options['nlp_solver_type'] == 'SQP': return full_stats[7, :] else: # self.__solver_options['nlp_solver_type'] == 'SQP_RTI': - raise Exception("alpha values are not available for SQP_RTI") + raise ValueError("alpha values are not available for SQP_RTI") elif field_ == 'residuals': return self.get_residuals() @@ -1651,9 +1651,9 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if self.__solver_options['rti_log_residuals'] == 1: return full_stats[4, :] else: - raise Exception("res_eq_all is not available for SQP_RTI if rti_log_residuals is not enabled.") + raise ValueError("res_eq_all is not available for SQP_RTI if rti_log_residuals is not enabled.") else: - raise Exception(f"res_eq_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + raise KeyError(f"res_eq_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") elif field_ == 'res_stat_all': full_stats = self.get_stats('statistics') @@ -1663,9 +1663,9 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if self.__solver_options['rti_log_residuals'] == 1: return full_stats[3, :] else: - raise Exception("res_stat_all is not available for SQP_RTI if rti_log_residuals is not enabled.") + raise ValueError("res_stat_all is not available for SQP_RTI if rti_log_residuals is not enabled.") else: - raise Exception(f"res_stat_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + raise ValueError(f"res_stat_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") elif field_ == 'res_ineq_all': full_stats = self.get_stats('statistics') @@ -1675,9 +1675,9 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if self.__solver_options['rti_log_residuals'] == 1: return full_stats[5, :] else: - raise Exception("res_ineq_all is not available for SQP_RTI if rti_log_residuals is not enabled.") + raise ValueError("res_ineq_all is not available for SQP_RTI if rti_log_residuals is not enabled.") else: - raise Exception(f"res_ineq_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + raise KeyError(f"res_ineq_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") elif field_ == 'res_comp_all': full_stats = self.get_stats('statistics') @@ -1687,12 +1687,12 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if self.__solver_options['rti_log_residuals'] == 1: return full_stats[6, :] else: - raise Exception("res_comp_all is not available for SQP_RTI if rti_log_residuals is not enabled.") + raise ValueError("res_comp_all is not available for SQP_RTI if rti_log_residuals is not enabled.") else: - raise Exception(f"res_comp_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + raise KeyError(f"res_comp_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") else: - raise Exception(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' + raise ValueError(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' + f'\n Possible values are {fields}.') @@ -1762,9 +1762,9 @@ def get_initial_residuals(self) -> np.ndarray: if self.__solver_options['rti_log_residuals'] == 1: return full_stats[3:7, 0] else: - raise Exception("initial_residuals is only available for SQP_RTI if rti_log_residuals is enabled, for efficiency the rti_log_only_available_residuals option is recommended.") + raise ValueError("initial_residuals is only available for SQP_RTI if rti_log_residuals is enabled, for efficiency the rti_log_only_available_residuals option is recommended.") else: - raise Exception(f"initial_residuals is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + raise ValueError(f"initial_residuals is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") # Note: this function should not be used anymore, better use cost_set, constraints_set def set(self, stage_: int, field_: str, value_: np.ndarray): @@ -1792,9 +1792,9 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): sens_fields = ['sens_x', 'sens_u'] if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') # cast value_ to avoid conversion issues if isinstance(value_, (float, int)): @@ -1811,7 +1811,7 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): assert getattr(self.shared_lib, f"{self.name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 else: if field_ not in constraints_fields + cost_fields + out_fields + mem_fields + sens_fields: - raise Exception(f"AcadosOcpSolver.set(): '{field}' is not a valid argument.\n" + raise ValueError(f"AcadosOcpSolver.set(): '{field}' is not a valid argument.\n" f" Possible values are {constraints_fields + cost_fields + out_fields + mem_fields + sens_fields + ['p']}.") dims = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ @@ -1820,7 +1820,7 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): if value_.shape[0] != dims: msg = f'AcadosOcpSolver.set(): mismatching dimension for field "{field_}" ' msg += f'with dimension {dims} (you have {value_.shape[0]})' - raise Exception(msg) + raise RuntimeError(msg) value_data = cast(value_.ctypes.data, POINTER(c_double)) value_data_p = cast((value_data), c_void_p) @@ -1861,9 +1861,9 @@ def cost_get(self, stage_: int, field_: str) -> np.ndarray: """ if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') field = field_.encode('utf-8') stage = c_int(stage_) @@ -1905,9 +1905,9 @@ def cost_set(self, stage_: int, field_: str, value_, api='warn'): value_ = np.array([value_]) if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') value_ = value_.astype(float) field = field_.encode('utf-8') @@ -1928,7 +1928,7 @@ def cost_set(self, stage_: int, field_: str, value_, api='warn'): pass elif api=='warn': if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): - raise Exception("Ambiguity in API detected.\n" + raise ValueError("Ambiguity in API detected.\n" "Are you making an acados model from scratch? Add api='new' to cost_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" f" You are relying on a now-fixed bug in cost_set for field '{field_}'.\n" + @@ -1944,10 +1944,10 @@ def cost_set(self, stage_: int, field_: str, value_, api='warn'): # Get elements in column major order value_ = np.ravel(value_, order='F') else: - raise Exception("Unknown api: '{}'".format(api)) + raise ValueError("Unknown api: '{}'".format(api)) if value_shape != tuple(dims): - raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' + + raise ValueError('AcadosOcpSolver.cost_set(): mismatching dimension' + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -1968,9 +1968,9 @@ def constraints_get(self, stage_: int, field_: str) -> np.ndarray: """ if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') field = field_.encode('utf-8') stage = c_int(stage_) @@ -2010,9 +2010,9 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war value_ = value_.astype(float) if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') field = field_.encode('utf-8') stage = c_int(stage_) @@ -2031,7 +2031,7 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war pass elif api=='warn': if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): - raise Exception("Ambiguity in API detected.\n" + raise RuntimeError("Ambiguity in API detected.\n" "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" f" You are relying on a now-fixed bug in constraints_set for field '{field}'.\n" + @@ -2047,10 +2047,10 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war # Get elements in column major order value_ = np.ravel(value_, order='F') else: - raise Exception(f"Unknown api: '{api}'") + raise ValueError(f"Unknown api: '{api}'") if value_shape != tuple(dims): - raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' + + raise ValueError(f'AcadosOcpSolver.constraints_set(): mismatching dimension' + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -2091,20 +2091,20 @@ def get_from_qp_in(self, stage_: int, field_: str): if not isinstance(stage_, int): raise TypeError("stage should be int") if stage_ > self.N: - raise Exception("stage should be <= self.N") + raise ValueError("stage should be <= self.N") if field_ in self.__qp_dynamics_fields and stage_ >= self.N: raise ValueError(f"dynamics field {field_} not available at terminal stage") if field_ not in self.__all_qp_fields | self.__all_relaxed_qp_fields: - raise Exception(f"field {field_} not supported.") + raise ValueError(f"field {field_} not supported.") if field_ in self.__qp_pc_hpipm_fields: if self.__solver_options["qp_solver"] != "PARTIAL_CONDENSING_HPIPM" or self.__solver_options["qp_solver_cond_N"] != self.N: - raise Exception(f"field {field_} only works for PARTIAL_CONDENSING_HPIPM QP solver with qp_solver_cond_N == N.") + raise ValueError(f"field {field_} only works for PARTIAL_CONDENSING_HPIPM QP solver with qp_solver_cond_N == N.") if field_ in ["P", "K", "p"] and stage_ == 0 and self.__nbxe_0 > 0: - raise Exception(f"getting field {field_} at stage 0 only works without x0 elimination (see nbxe_0).") + raise ValueError(f"getting field {field_} at stage 0 only works without x0 elimination (see nbxe_0).") if field_ in self.__qp_pc_fields and not self.__solver_options["qp_solver"].startswith("PARTIAL_CONDENSING"): - raise Exception(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") + raise ValueError(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") if field_ in self.__all_relaxed_qp_fields and not self.__solver_options["nlp_solver_type"] == "SQP_WITH_FEASIBLE_QP": - raise Exception(f"field {field_} only works for SQP_WITH_FEASIBLE_QP nlp_solver_type.") + raise ValueError(f"field {field_} only works for SQP_WITH_FEASIBLE_QP nlp_solver_type.") field = field_.encode('utf-8') stage = c_int(stage_) @@ -2153,13 +2153,13 @@ def get_iterate(self, iteration: int) -> AcadosOcpIterate: nlp_iter = self.get_stats('nlp_iter') if iteration < -1 or iteration > nlp_iter: - raise Exception("get_iterate: iteration needs to be nonnegative and <= nlp_iter or -1.") + raise ValueError("get_iterate: iteration needs to be nonnegative and <= nlp_iter or -1.") if not self.__solver_options["store_iterates"]: - raise Exception("get_iterate: the solver option store_iterates needs to be true in order to get iterates.") + raise ValueError("get_iterate: the solver option store_iterates needs to be true in order to get iterates.") if self.__solver_options["nlp_solver_type"] == "SQP_RTI": - raise Exception("get_iterate: SQP_RTI not supported.") + raise NotImplementedError("get_iterate: SQP_RTI not supported.") # set to nlp_iter if -1 iteration = nlp_iter if iteration == -1 else iteration @@ -2278,41 +2278,41 @@ def options_set(self, field_, value_): # check field availability and type if field_ in int_fields: if not isinstance(value_, int): - raise Exception(f'solver option \'{field_}\' must be of type int. You have {type(value_)}.') + raise TypeError(f'solver option \'{field_}\' must be of type int. You have {type(value_)}.') else: value_ctypes = c_int(value_) elif field_ in double_fields: if not isinstance(value_, float): - raise Exception(f'solver option \'{field_}\' must be of type float. You have {type(value_)}.') + raise TypeError(f'solver option \'{field_}\' must be of type float. You have {type(value_)}.') else: value_ctypes = c_double(value_) elif field_ in bool_fields: if not isinstance(value_, bool): - raise Exception(f'solver option \'{field_}\' must be of type bool. You have {type(value_)}.') + raise TypeError(f'solver option \'{field_}\' must be of type bool. You have {type(value_)}.') else: value_ctypes = c_bool(value_) elif field_ in string_fields: if not isinstance(value_, str): - raise Exception(f'solver option \'{field_}\' must be of type str. You have {type(value_)}.') + raise TypeError(f'solver option \'{field_}\' must be of type str. You have {type(value_)}.') else: value_ctypes = value_.encode('utf-8') else: fields = ', '.join(int_fields + double_fields + string_fields) - raise Exception(f'AcadosOcpSolver.options_set() does not support field \'{field_}\'.\n'\ + raise ValueError(f'AcadosOcpSolver.options_set() does not support field \'{field_}\'.\n' f' Possible values are {fields}.') if (field_ == 'max_iter' or field_ == 'nlp_solver_max_iter') and value_ > self.__solver_options['nlp_solver_max_iter']: - raise Exception('AcadosOcpSolver.options_set() cannot increase nlp_solver_max_iter' \ + raise ValueError('AcadosOcpSolver.options_set() cannot increase nlp_solver_max_iter' \ f' above initial value {self.__nlp_solver_max_iter} (you have {value_})') return if field_ == 'rti_phase': if value_ < 0 or value_ > 2: - raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' + raise ValueError('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' 'take only values 0, 1, 2 for SQP-RTI-type solvers') if self.__solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0: - raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' + raise ValueError('AcadosOcpSolver.options_set(): argument \'rti_phase\' can ' 'take only value 0 for SQP-type solvers') # encode @@ -2340,25 +2340,25 @@ def set_params_sparse(self, stage_: int, idx_values_: np.ndarray, param_values_) """ if not isinstance(stage_, int): - raise Exception('stage should be integer.') + raise TypeError('stage should be integer.') elif stage_ < 0 or stage_ > self.N: - raise Exception(f'stage should be in [0, N], got {stage_}') + raise ValueError(f'stage should be in [0, N], got {stage_}') # if not isinstance(idx_values_, np.ndarray) or not issubclass(type(idx_values_[0]), np.integer): - # raise Exception('idx_values_ must be np.array of integers.') + # raise TypeError('idx_values_ must be np.array of integers.') if not isinstance(param_values_, np.ndarray): - raise Exception('param_values_ must be np.array.') + raise TypeError('param_values_ must be np.array.') elif np.float64 != param_values_.dtype: raise TypeError('param_values_ must be np.array of float64.') if param_values_.shape[0] != len(idx_values_): - raise Exception(f'param_values_ and idx_values_ must be of the same size.' + + raise ValueError(f'param_values_ and idx_values_ must be of the same size.' + f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.') p_dimension = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, "p".encode('utf-8')) if any(idx_values_ >= p_dimension): - raise Exception(f'idx_values_ contains value >= np = {p_dimension} for stage {stage_}.') + raise ValueError(f'idx_values_ contains value >= np = {p_dimension} for stage {stage_}.') stage = c_int(stage_) n_update = c_int(len(param_values_)) @@ -2379,17 +2379,17 @@ def set_p_global_and_precompute_dependencies(self, data_: np.ndarray): np_global = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, 0, "p_global".encode('utf-8')) if not isinstance(data_, np.ndarray): - raise Exception('data must be np.array.') + raise TypeError('data must be np.array.') if np.float64 != data_.dtype: raise TypeError('data must be np.array of float64.') if data_.ndim != 1: - raise Exception('data must be one-dimensional np array.') + raise ValueError('data must be one-dimensional np array.') data = np.ascontiguousarray(data_, dtype=np.float64) c_data = cast(data.ctypes.data, POINTER(c_double)) data_len = len(data) if data_len != np_global: - raise Exception(f'data must have length {np_global}, got {data_len}.') + raise ValueError(f'data must have length {np_global}, got {data_len}.') status = getattr(self.shared_lib, f"{self.name}_acados_set_p_global_and_precompute_dependencies")(self.capsule, c_data, data_len) if self.__save_p_global: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index 5c319aee01..16ada19927 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -122,7 +122,7 @@ cdef class AcadosOcpSolverCython: if print_stats_on_failure: self.print_statistics() if fail_on_nonzero_status: - raise Exception(f'acados acados_ocp_solver returned status {status}') + raise RuntimeError(f'acados acados_ocp_solver returned status {status}') elif print_stats_on_failure: print(f'Warning: acados acados_ocp_solver returned status {status}') @@ -219,7 +219,7 @@ cdef class AcadosOcpSolverCython: cdef cnp.ndarray[cnp.float64_t, ndim=1] grad if with_respect_to == "initial_state": # if not self.acados_ocp.constraints.has_x0: - # raise Exception("OCP does not have an initial state constraint.") + # raise ValueError("OCP does not have an initial state constraint.") nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) nbu = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "lbu".encode('utf-8')) @@ -239,7 +239,7 @@ cdef class AcadosOcpSolverCython: self.time_value_grad = time.time() - t0 else: - raise Exception(f"AcadosOcpSolver.eval_and_get_optimal_value_gradient(): Unknown field: with_respect_to = {with_respect_to}") + raise ValueError(f"AcadosOcpSolver.eval_and_get_optimal_value_gradient(): Unknown field: with_respect_to = {with_respect_to}") return grad @@ -274,7 +274,7 @@ cdef class AcadosOcpSolverCython: # if not (self.acados_ocp.solver_options.qp_solver == 'FULL_CONDENSING_HPIPM' or # self.acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_HPIPM'): - # raise Exception("Parametric sensitivities are only available with HPIPM as QP solver.") + # raise NotImplementedError("Parametric sensitivities are only available with HPIPM as QP solver.") # if not ( # (self.acados_ocp.solver_options.hessian_approx == 'EXACT' or @@ -285,7 +285,7 @@ cdef class AcadosOcpSolverCython: # self.acados_ocp.solver_options.regularize_method == 'NO_REGULARIZE' and # self.acados_ocp.solver_options.levenberg_marquardt == 0 # ): - # raise Exception("Parametric sensitivities are only correct if an exact Hessian is used!") + # raise ValueError("Parametric sensitivities are only correct if an exact Hessian is used!") stages_is_list = isinstance(stages, list) stages_ = stages if stages_is_list else [stages] @@ -299,7 +299,7 @@ cdef class AcadosOcpSolverCython: # for s in stages_: # if not isinstance(s, int) or s < 0 or s > N: - # raise Exception("AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or [int] and in [0, N].") + # raise TypeError("AcadosOcpSolver.eval_solution_sensitivity(): stages need to be int or [int] and in [0, N].") if with_respect_to == "initial_state": nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) @@ -319,7 +319,7 @@ cdef class AcadosOcpSolverCython: self.time_solution_sens_lin = time.time() - t0 else: - raise Exception(f"AcadosOcpSolver.eval_solution_sensitivity(): Unknown field: with_respect_to = {with_respect_to}") + raise ValueError(f"AcadosOcpSolver.eval_solution_sensitivity(): Unknown field: with_respect_to = {with_respect_to}") # initialize jacobians with zeros for s in stages_: @@ -367,12 +367,12 @@ cdef class AcadosOcpSolverCython: # checks if not isinstance(index, int): - raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.') + raise TypeError('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.') cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) if index < 0 or index > nx: - raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.') + raise IndexError(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.') # actual eval_param acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out) @@ -405,14 +405,14 @@ cdef class AcadosOcpSolverCython: all_fields = out_fields + in_fields + sens_fields if field_ not in all_fields: - raise Exception(f'AcadosOcpSolver.get(stage={stage}, field={field_}): \'{field_}\' is an invalid argument.\ + raise ValueError(f'AcadosOcpSolver.get(stage={stage}, field={field_}): \'{field_}\' is an invalid argument.\ \n Possible values are {all_fields}.') if stage < 0 or stage > self.N: - raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N)) + raise ValueError('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N)) if stage == self.N and field_ == 'pi': - raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\ + raise KeyError('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\ .format(field_, stage)) field = field_ @@ -504,7 +504,7 @@ cdef class AcadosOcpSolverCython: """ import json if not os.path.isfile(filename): - raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + raise FileNotFoundError('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) with open(filename, 'r') as f: solution = json.load(f) @@ -590,7 +590,7 @@ cdef class AcadosOcpSolverCython: if self.nlp_solver_type == 'SQP': return full_stats[7, :] else: # self.nlp_solver_type == 'SQP_RTI': - raise Exception("alpha values are not available for SQP_RTI") + raise ValueError("alpha values are not available for SQP_RTI") elif field_ == 'residuals': return self.get_residuals() @@ -682,7 +682,7 @@ cdef class AcadosOcpSolverCython: su: slack variables of soft upper inequality constraints \n """ if not isinstance(value_, np.ndarray): - raise Exception(f"set: value must be numpy array, got {type(value_)}.") + raise TypeError(f"set: value must be numpy array, got {type(value_)}.") cost_fields = ['y_ref', 'yref'] constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] out_fields = ['x', 'u', 'pi', 'lam', 'z', 'sl', 'su'] @@ -697,7 +697,7 @@ cdef class AcadosOcpSolverCython: assert acados_solver.acados_update_params(self.capsule, stage, value.data, value.shape[0]) == 0 else: if field_ not in constraints_fields + cost_fields + out_fields: - raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\ + raise ValueError("AcadosOcpSolverCython.set(): {} is not a valid argument.\ \nPossible values are {}.".format(field, \ constraints_fields + cost_fields + out_fields + ['p'])) @@ -707,7 +707,7 @@ cdef class AcadosOcpSolverCython: if value_.shape[0] != dims: msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_) msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) - raise Exception(msg) + raise ValueError(msg) if field_ in constraints_fields: acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, @@ -735,7 +735,7 @@ cdef class AcadosOcpSolverCython: :param value: of appropriate size """ if not isinstance(value_, np.ndarray): - raise Exception(f"cost_set: value must be numpy array, got {type(value_)}.") + raise TypeError(f"cost_set: value must be numpy array, got {type(value_)}.") field = field_.encode('utf-8') cdef int dims[2] @@ -754,7 +754,7 @@ cdef class AcadosOcpSolverCython: value = np.asfortranarray(value_) if value_shape[0] != dims[0] or value_shape[1] != dims[1]: - raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' + + raise ValueError('AcadosOcpSolverCython.cost_set(): mismatching dimension' + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \ @@ -770,7 +770,7 @@ cdef class AcadosOcpSolverCython: :param value: of appropriate size """ if not isinstance(value_, np.ndarray): - raise Exception(f"constraints_set: value must be numpy array, got {type(value_)}.") + raise TypeError(f"constraints_set: value must be numpy array, got {type(value_)}.") field = field_.encode('utf-8') @@ -790,7 +790,7 @@ cdef class AcadosOcpSolverCython: value = np.asfortranarray(value_) if value_shape != tuple(dims): - raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' + + raise ValueError(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' + f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \ @@ -852,14 +852,14 @@ cdef class AcadosOcpSolverCython: # check field availability and type if field_ in int_fields: if not isinstance(value_, int): - raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) + raise TypeError('solver option {} must be of type int. You have {}.'.format(field_, type(value_))) if field_ == 'rti_phase': if value_ < 0 or value_ > 2: - raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' + raise ValueError('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' 'take only values 0, 1, 2 for SQP-RTI-type solvers') if self.nlp_solver_type != 'SQP_RTI': - raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' + raise ValueError('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can ' 'take only value 0 for SQP-type solvers') int_value = value_ @@ -867,20 +867,20 @@ cdef class AcadosOcpSolverCython: elif field_ in double_fields: if not isinstance(value_, float): - raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) + raise TypeError('solver option {} must be of type float. You have {}.'.format(field_, type(value_))) double_value = value_ acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &double_value) elif field_ in string_fields: if not isinstance(value_, bytes): - raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) + raise TypeError('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) string_value = value_.encode('utf-8') acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &string_value[0]) else: - raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\ + raise NotImplementedError('AcadosOcpSolverCython.options_set() does not support field {}.'\ '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) @@ -895,10 +895,10 @@ cdef class AcadosOcpSolverCython: """ if not isinstance(param_values_, np.ndarray): - raise Exception('param_values_ must be np.array.') + raise TypeError('param_values_ must be np.array.') if param_values_.shape[0] != len(idx_values_): - raise Exception(f'param_values_ and idx_values_ must be of the same size.' + + raise ValueError(f'param_values_ and idx_values_ must be of the same size.' + f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.') # n_update = c_int(len(param_values_)) diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 12348cb694..8ef0255073 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -186,14 +186,14 @@ def ext_fun_compile_flags(self, ext_fun_compile_flags): if isinstance(ext_fun_compile_flags, str): self.__ext_fun_compile_flags = ext_fun_compile_flags else: - raise Exception('Invalid ext_fun_compile_flags value, expected a string.\n') + raise TypeError('Invalid ext_fun_compile_flags value, expected a string.\n') @ext_fun_expand_dyn.setter def ext_fun_expand_dyn(self, ext_fun_expand_dyn): if isinstance(ext_fun_expand_dyn, bool): self.__ext_fun_expand_dyn = ext_fun_expand_dyn else: - raise Exception('Invalid ext_fun_expand_dyn value, expected bool.\n') + raise TypeError('Invalid ext_fun_expand_dyn value, expected bool.\n') @integrator_type.setter def integrator_type(self, integrator_type): @@ -201,7 +201,7 @@ def integrator_type(self, integrator_type): if integrator_type in integrator_types: self.__integrator_type = integrator_type else: - raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ + raise ValueError('Invalid integrator_type value. Possible values are:\n\n' \ + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n') @collocation_type.setter @@ -210,7 +210,7 @@ def collocation_type(self, collocation_type): if collocation_type in collocation_types: self.__collocation_type = collocation_type else: - raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ + raise ValueError('Invalid collocation_type value. Possible values are:\n\n' \ + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n') @T.setter @@ -222,70 +222,70 @@ def num_stages(self, num_stages): if isinstance(num_stages, int): self.__sim_method_num_stages = num_stages else: - raise Exception('Invalid num_stages value. num_stages must be an integer.') + raise ValueError('Invalid num_stages value. num_stages must be an integer.') @num_steps.setter def num_steps(self, num_steps): if isinstance(num_steps, int): self.__sim_method_num_steps = num_steps else: - raise Exception('Invalid num_steps value. num_steps must be an integer.') + raise TypeError('Invalid num_steps value. num_steps must be an integer.') @newton_iter.setter def newton_iter(self, newton_iter): if isinstance(newton_iter, int): self.__sim_method_newton_iter = newton_iter else: - raise Exception('Invalid newton_iter value. newton_iter must be an integer.') + raise TypeError('Invalid newton_iter value. newton_iter must be an integer.') @newton_tol.setter def newton_tol(self, newton_tol): if isinstance(newton_tol, float): self.__sim_method_newton_tol = newton_tol else: - raise Exception('Invalid newton_tol value. newton_tol must be an float.') + raise TypeError('Invalid newton_tol value. newton_tol must be a float.') @sens_forw.setter def sens_forw(self, sens_forw): if sens_forw in (True, False): self.__sens_forw = sens_forw else: - raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.') + raise ValueError('Invalid sens_forw value. sens_forw must be a Boolean.') @sens_adj.setter def sens_adj(self, sens_adj): if sens_adj in (True, False): self.__sens_adj = sens_adj else: - raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.') + raise ValueError('Invalid sens_adj value. sens_adj must be a Boolean.') @sens_hess.setter def sens_hess(self, sens_hess): if sens_hess in (True, False): self.__sens_hess = sens_hess else: - raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.') + raise ValueError('Invalid sens_hess value. sens_hess must be a Boolean.') @sens_algebraic.setter def sens_algebraic(self, sens_algebraic): if sens_algebraic in (True, False): self.__sens_algebraic = sens_algebraic else: - raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.') + raise ValueError('Invalid sens_algebraic value. sens_algebraic must be a Boolean.') @output_z.setter def output_z(self, output_z): if output_z in (True, False): self.__output_z = output_z else: - raise Exception('Invalid output_z value. output_z must be a Boolean.') + raise ValueError('Invalid output_z value. output_z must be a Boolean.') @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): if sim_method_jac_reuse in (0, 1): self.__sim_method_jac_reuse = sim_method_jac_reuse else: - raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.') + raise ValueError('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.') @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): @@ -293,7 +293,7 @@ def num_threads_in_batch_solve(self, num_threads_in_batch_solve): if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: - raise Exception('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') + raise ValueError('Invalid num_threads_in_batch_solve value. num_threads_in_batch_solve must be a positive integer.') @with_batch_functionality.setter def with_batch_functionality(self, with_batch_functionality): @@ -354,19 +354,19 @@ def parameter_values(self, parameter_values): if isinstance(parameter_values, np.ndarray): self.__parameter_values = parameter_values else: - raise Exception('Invalid parameter_values value. ' + + raise ValueError('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') def make_consistent(self): self.model.make_consistent(self.dims) if self.parameter_values.shape[0] != self.dims.np: - raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ + raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ f'\nGot np = {self.dims.np}, acados_sim.parameter_values.shape = {self.parameter_values.shape[0]}\n') # check required arguments are given if self.solver_options.T is None: - raise Exception('acados_sim.solver_options.T is None, should be provided.') + raise ValueError('acados_sim.solver_options.T is None, should be provided.') def to_dict(self) -> dict: @@ -392,7 +392,7 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): json_path = os.path.join(os.getcwd(), json_file) if not os.path.exists(json_path): - raise Exception(f"{json_path} not found!") + raise FileNotFoundError(f"{json_path} not found!") template_list = [ ('acados_sim_solver.in.c', f'acados_sim_solver_{self.model.name}.c'), diff --git a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py index 65bc55d624..5d0ac0d466 100644 --- a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py @@ -52,13 +52,13 @@ class AcadosSimBatchSolver(): def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None , json_file: str = 'acados_sim.json', build: bool = True, generate: bool = True, verbose: bool=True): if not isinstance(N_batch, int) or N_batch <= 0: - raise Exception("AcadosSimBatchSolver: argument N_batch should be a positive integer.") + raise ValueError("AcadosSimBatchSolver: argument N_batch should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = sim.solver_options.num_threads_in_batch_solve print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in sim.solver_options instead.") print("In the future, it should be passed explicitly in the AcadosSimBatchSolver constructor.") if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: - raise Exception("AcadosSimBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") + raise ValueError("AcadosSimBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") if not sim.solver_options.with_batch_functionality: print("Warning: Using AcadosSimBatchSolver, but sim.solver_options.with_batch_functionality is False.") print("Attempting to compile with openmp nonetheless.") diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index 13dc5e10ed..8e7e021d59 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -97,7 +97,7 @@ def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_bui # module dependent post processing if acados_sim.solver_options.integrator_type == 'GNSF': if acados_sim.solver_options.sens_hess == True: - raise Exception("AcadosSimSolver: GNSF does not support sens_hess = True.") + raise ValueError("AcadosSimSolver: GNSF does not support sens_hess = True.") if 'gnsf_model' in acados_sim.__dict__: set_up_imported_gnsf_model(acados_sim) else: @@ -259,7 +259,7 @@ def simulate(self, x=None, u=None, z=None, xdot=None, p=None): status = self.solve() if status != 0: - raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.') + raise RuntimeError(f'acados_sim_solver for model {self.model_name} returned status {status}.') x_next = self.get('x') return x_next @@ -313,7 +313,7 @@ def get(self, field_): out = scalar.value else: - raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ + raise KeyError(f'AcadosSimSolver.get(): Unknown field {field_},' f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}') return out @@ -361,7 +361,7 @@ def set(self, field_: str, value_): value_shape = (value_shape[0], 0) if value_shape != tuple(dims): - raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \ + raise ValueError(f'AcadosSimSolver.set(): mismatching dimension' \ f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).') if field_ == 'T': @@ -373,7 +373,7 @@ def set(self, field_: str, value_): elif field_ in settable: self.__acados_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p) else: - raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \ + raise KeyError(f'AcadosSimSolver.set(): Unknown field {field_},' f' available fields are {", ".join(settable)}') return @@ -388,7 +388,7 @@ def options_set(self, field_: str, value_: bool): """ fields = ['sens_forw', 'sens_adj', 'sens_hess'] if field_ not in fields: - raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") + raise ValueError(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") field = field_.encode('utf-8') value_ctypes = c_bool(value_) diff --git a/interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx index be400addc7..dfe09a95ad 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx @@ -121,7 +121,7 @@ cdef class AcadosSimSolverCython: if status == 2: print("Warning: acados_sim_solver reached maximum iterations.") elif status != 0: - raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.') + raise RuntimeError(f'acados_sim_solver for model {self.model_name} returned status {status}.') x_next = self.get('x') return x_next @@ -149,7 +149,7 @@ cdef class AcadosSimSolverCython: elif field_ in self.gettable_scalars: return self.__get_scalar(field) else: - raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ + raise KeyError(f'AcadosSimSolver.get(): Unknown field {field_},' \ f' available fields are {", ".join(self.gettable.keys())}') @@ -210,7 +210,7 @@ cdef class AcadosSimSolverCython: value_shape = (value_shape[0], 0) if value_shape != tuple(dims): - raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \ + raise ValueError(f'AcadosSimSolverCython.set(): mismatching dimension' \ f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).') # set @@ -219,7 +219,7 @@ cdef class AcadosSimSolverCython: elif field_ in settable: acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value.data) else: - raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \ + raise ValueError(f'AcadosSimSolverCython.set(): Unknown field {field_},' \ f' available fields are {", ".join(settable)}') @@ -232,7 +232,7 @@ cdef class AcadosSimSolverCython: """ fields = ['sens_forw', 'sens_adj', 'sens_hess'] if field_ not in fields: - raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") + raise ValueError(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") field = field_.encode('utf-8') diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 3f60a2e82c..56eb08390f 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -98,7 +98,7 @@ def __generate_functions(self): try: fun = ca.Function(name, inputs, outputs, self.__casadi_fun_opts) # print(f"Generating function {name} with inputs {inputs}") - except Exception as e: + except RuntimeError as e: print(f"\nError while creating function {name} with inputs {inputs} and outputs {outputs}") print(e) raise e @@ -120,7 +120,7 @@ def __generate_functions(self): with set_directory(output_dir): try: fun.generate(name, self.casadi_codegen_opts) - except Exception as e: + except RuntimeError as e: print(f"Error while generating function {name} in directory {output_dir}") print(e) raise e @@ -507,7 +507,7 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, if opts.with_solution_sens_wrt_params: if casadi_length(z) > 0: - raise Exception("acados: solution sensitivities wrt parameters not supported with algebraic variables.") + raise NotImplementedError("acados: solution sensitivities wrt parameters not supported with algebraic variables.") grad_ux = ca.jacobian(ext_cost, ca.vertcat(u, x)) hess_xu_p = ca.jacobian(grad_ux, p_global) context.add_function_definition(fun_name_param, [x, u, z, p], [hess_xu_p], cost_dir, 'cost') @@ -701,7 +701,7 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con con_phi_expr = model.con_phi_expr if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): - raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") + raise ValueError("acados: you can either have constraint_h, or constraint_phi, not both.") if (is_empty(con_h_expr) and is_empty(con_phi_expr)): # both empty -> nothing to generate diff --git a/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py b/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py index 315ffc4d11..c5e3cff724 100644 --- a/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py +++ b/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py @@ -73,7 +73,7 @@ def detect_gnsf_structure(acados_ocp, transcribe_opts=None): # acados_root_dir = getenv('ACADOS_INSTALL_DIR') if not is_empty(acados_ocp.model.p_global) and depends_on(acados_ocp.model.f_impl_expr, acados_ocp.model.p_global): - Exception("GNSF does not support global parameters") + NotImplementedError("GNSF does not support global parameters") ## load transcribe_opts if transcribe_opts is None: diff --git a/interfaces/acados_template/acados_template/gnsf/reformulate_with_invertible_E_mat.py b/interfaces/acados_template/acados_template/gnsf/reformulate_with_invertible_E_mat.py index 21ab8ebfd5..e4a2494bf2 100644 --- a/interfaces/acados_template/acados_template/gnsf/reformulate_with_invertible_E_mat.py +++ b/interfaces/acados_template/acados_template/gnsf/reformulate_with_invertible_E_mat.py @@ -121,7 +121,7 @@ def reformulate_with_invertible_E_mat(gnsf, model, print_info): else: ind_f = np.nonzero(gnsf["C"][sub_max, :])[0] if len(ind_f) != 1: - raise Exception("C is assumed to be a selection matrix") + raise ValueError("C is assumed to be a selection matrix") else: ind_f = ind_f[0] # add term to corresponding nonlinearity entry diff --git a/interfaces/acados_template/acados_template/mpc_utils.py b/interfaces/acados_template/acados_template/mpc_utils.py index 1798fc3f8d..6cb70f088b 100644 --- a/interfaces/acados_template/acados_template/mpc_utils.py +++ b/interfaces/acados_template/acados_template/mpc_utils.py @@ -411,7 +411,7 @@ def get_path_cost_expression(ocp: AcadosOcp): cost_dot = ca.substitute( model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) else: - raise Exception("create_model_with_cost_state: Unknown cost type.") + raise ValueError("create_model_with_cost_state: Unknown cost type.") return cost_dot @@ -434,7 +434,7 @@ def get_terminal_cost_expression(ocp: AcadosOcp): cost_dot = ca.substitute( model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) else: - raise Exception("create_model_with_cost_state: Unknown terminal cost type.") + raise ValueError("create_model_with_cost_state: Unknown terminal cost type.") return cost_dot diff --git a/interfaces/acados_template/acados_template/penalty_utils.py b/interfaces/acados_template/acados_template/penalty_utils.py index 0509f88f9e..ce72a4332a 100644 --- a/interfaces/acados_template/acados_template/penalty_utils.py +++ b/interfaces/acados_template/acados_template/penalty_utils.py @@ -68,10 +68,10 @@ def one_sided_huber_penalty( if tau is None: if w is None: - raise Exception("Either specify w or tau") + raise ValueError("Either specify w or tau") tau = 2 * w * delta elif w is not None: - raise Exception("Either specify w or tau") + raise ValueError("Either specify w or tau") loss, _, _, loss_hess_XGN = huber_loss(u, delta, tau) # shifted by delta to get a penalty @@ -112,10 +112,10 @@ def symmetric_huber_penalty( if tau is None: if w is None: - raise Exception("Either specify w or tau") + raise ValueError("Either specify w or tau") tau = 2 * w * delta elif w is not None: - raise Exception("Either specify w or tau") + raise ValueError("Either specify w or tau") loss, _, _, loss_hess_XGN = huber_loss(u, delta, tau) diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index cc4d9b7a31..480c1019c5 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -144,8 +144,8 @@ def check_casadi_version(): def check_casadi_version_supports_p_global(): try: from casadi import extract_parametric, cse - except: - raise Exception("CasADi version does not support extract_parametric or cse functions.\nNeeds nightly-se2 release or later, see: https://github.com/casadi/casadi/releases/tag/nightly-se2") + except ImportError: + raise ImportError("CasADi version does not support extract_parametric or cse functions.\nNeeds nightly-se2 release or later, see: https://github.com/casadi/casadi/releases/tag/nightly-se2") def get_simulink_default_opts() -> dict: @@ -174,7 +174,7 @@ def is_column(x): elif x == None or x == []: return False else: - raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." + raise TypeError("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX." + " Got: " + str(type(x))) @@ -190,7 +190,7 @@ def is_empty(x): elif isinstance(x, (float, int)): return False else: - raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, " + raise TypeError("is_empty expects one of the following types: casadi.MX, casadi.SX, " + "None, numpy array empty list, set. Got: " + str(type(x))) @@ -202,7 +202,7 @@ def casadi_length(x): elif isinstance(x, list): return len(x) else: - raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX." + raise TypeError("casadi_length expects one of the following types: casadi.MX, casadi.SX." + " Got: " + str(type(x))) def get_shared_lib_ext(): @@ -300,7 +300,7 @@ def render_template(in_file, out_file, output_dir, json_path, template_glob=None status = os.system(os_cmd) if status != 0: - raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n') + raise RuntimeError(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n') @@ -351,18 +351,18 @@ def get_default_simulink_opts() -> dict: def J_to_idx(J): if not isinstance(J, np.ndarray): - raise Exception('J_to_idx: J must be a numpy array.') + raise TypeError('J_to_idx: J must be a numpy array.') if J.ndim != 2: - raise Exception('J_to_idx: J must be a 2D numpy array.') + raise ValueError('J_to_idx: J must be a 2D numpy array.') nrows = J.shape[0] idx = np.zeros((nrows, )) for i in range(nrows): this_idx = np.nonzero(J[i,:])[0] if len(this_idx) != 1: - raise Exception('Invalid J matrix structure detected, ' \ + raise ValueError('Invalid J matrix structure detected, ' \ 'must contain exactly one nonzero element per row.') if this_idx.size > 0 and J[i,this_idx[0]] != 1: - raise Exception('J matrices can only contain 1 and 0 entries.') + raise ValueError('J matrices can only contain 1 and 0 entries.') idx[i] = this_idx[0] return idx @@ -378,19 +378,19 @@ def J_to_idx_slack(J): idx[i_idx] = i i_idx = i_idx + 1 elif len(this_idx) > 1: - raise Exception('J_to_idx_slack: Invalid J matrix. ' \ + raise ValueError('J_to_idx_slack: Invalid J matrix. ' \ 'Found more than one nonzero in row ' + str(i)) if this_idx.size > 0 and J[i,this_idx[0]] != 1: - raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \ + raise ValueError('J_to_idx_slack: J matrices can only contain 1s, ' \ 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) if not i_idx == ncol: - raise Exception('J_to_idx_slack: J must contain a 1 in every column!') + raise ValueError('J_to_idx_slack: J must contain a 1 in every column!') return idx def check_if_nparray_and_flatten(val, name) -> np.ndarray: if not isinstance(val, np.ndarray): - raise Exception(f"{name} must be a numpy array, got {type(val)}") + raise TypeError(f"{name} must be a numpy array, got {type(val)}") return val.reshape(-1) def check_if_nparray_or_casadi_symbolic_and_flatten(val, name) -> np.ndarray: @@ -405,9 +405,9 @@ def check_if_nparray_or_casadi_symbolic_and_flatten(val, name) -> np.ndarray: def check_if_2d_nparray(val, name) -> None: if not isinstance(val, np.ndarray): - raise Exception(f"{name} must be a numpy array, got {type(val)}") + raise TypeError(f"{name} must be a numpy array, got {type(val)}") if val.ndim != 2: - raise Exception(f"{name} must be a 2D numpy array, got shape {val.shape}") + raise ValueError(f"{name} must be a 2D numpy array, got shape {val.shape}") return diff --git a/interfaces/acados_template/acados_template/zoro_description.py b/interfaces/acados_template/acados_template/zoro_description.py index 31b84c0d41..2afdf1dec6 100644 --- a/interfaces/acados_template/acados_template/zoro_description.py +++ b/interfaces/acados_template/acados_template/zoro_description.py @@ -110,7 +110,7 @@ def process_zoro_description(zoro_description: ZoroDescription): zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t) if zoro_description.input_P0_diag and zoro_description.input_P0: - raise Exception("Only one of input_P0_diag and input_P0 can be True") + raise ValueError("Only one of input_P0_diag and input_P0 can be True") # Print input note: print(f"\nThe data of the generated custom update function consists of the concatenation of:") From 6951b4f312c44dbd939b482d04b98b7d86bbed19 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 7 Apr 2025 16:34:20 +0200 Subject: [PATCH 019/164] MATLAB: AS-RTI closed-loop example (#1490) Plus: - add sanity checks on cost type for AS-RTI with level B and C - simplify copy cost formulation to initial stage --- .github/linux/export_paths.sh | 2 +- .../closed_loop_as_rti.m | 239 ++++++++++++++++++ .../as_rti/as_rti_closed_loop_example.py | 3 +- interfaces/CMakeLists.txt | 5 + interfaces/acados_matlab_octave/AcadosOcp.m | 40 ++- .../create_AcadosSim_from_AcadosOcp.m | 57 +++++ .../acados_template/acados_ocp.py | 4 + .../matlab_templates/mex_solver.in.m | 6 +- 8 files changed, 330 insertions(+), 26 deletions(-) create mode 100644 examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m create mode 100644 interfaces/acados_matlab_octave/create_AcadosSim_from_AcadosOcp.m diff --git a/.github/linux/export_paths.sh b/.github/linux/export_paths.sh index 9c4937543f..22d3800a10 100755 --- a/.github/linux/export_paths.sh +++ b/.github/linux/export_paths.sh @@ -34,5 +34,5 @@ echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV -echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase" >> $GITHUB_ENV +echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/pendulum_on_cart_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase" >> $GITHUB_ENV echo "ENV_RUN=true" >> $GITHUB_ENV diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m b/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m new file mode 100644 index 0000000000..6c2d0d2581 --- /dev/null +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m @@ -0,0 +1,239 @@ +% +% 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.; + + +function closed_loop_as_rti() + algorithms = {'SQP', 'RTI', 'AS-RTI-A', 'AS-RTI-B', 'AS-RTI-C', 'AS-RTI-D'}; + % algorithms = {'AS-RTI-B'}; + for i = 1:length(algorithms) + main(algorithms{i}, 1); + end +end + +function [ocp_solver, integrator] = setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter) + if nargin < 6 + as_rti_iter = 1; + end + + disp(['running with algorithm: ', algorithm, ', as_rti_iter: ', num2str(as_rti_iter)]); + + % create ocp object to formulate the OCP + ocp = AcadosOcp(); + + % set model + model = get_pendulum_on_cart_model(); + ocp.model = model; + + nx = size(model.x, 1); + nu = size(model.u, 1); + ny = nx + nu; + ny_e = nx; + + ocp.solver_options.N_horizon = N_horizon; + + % set cost module + Q_mat = 2 * diag([1e3, 1e3, 1e-2, 1e-2]); + R_mat = 2 * diag([1e-2]); + if 0 + % NOTE: not yet implemented in MATLAB + ocp.cost.cost_type = 'NONLINEAR_LS'; + ocp.cost.cost_type_e = 'NONLINEAR_LS'; + + ocp.cost.W = blkdiag(Q_mat, R_mat); + ocp.cost.W_e = Q_mat; + + ocp.model.cost_y_expr = [model.x; model.u]; + ocp.model.cost_y_expr_e = model.x; + ocp.cost.yref = zeros(ny, 1); + ocp.cost.yref_e = zeros(ny_e, 1); + ocp.translate_nls_cost_to_conl(); + else + ocp.cost.cost_type = 'EXTERNAL'; + ocp.cost.cost_type_e = 'EXTERNAL'; + + ocp.model.cost_expr_ext_cost = model.x' * Q_mat * model.x + model.u' * R_mat * model.u; + ocp.model.cost_expr_ext_cost_e = model.x' * Q_mat * model.x; + end + + % set constraints + ocp.constraints.lbu = -Fmax; + ocp.constraints.ubu = Fmax; + + ocp.constraints.x0 = x0; + ocp.constraints.idxbu = [0]; + + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; + ocp.solver_options.integrator_type = 'IRK'; + ocp.solver_options.sim_method_newton_iter = 10; + + + if ismember(algorithm, {'RTI', 'AS-RTI-A', 'AS-RTI-B', 'AS-RTI-C', 'AS-RTI-D'}) + ocp.solver_options.nlp_solver_type = 'SQP_RTI'; + elseif strcmp(algorithm, 'SQP') + ocp.solver_options.nlp_solver_type = 'SQP'; + else + error(['unknown algorithm: ', algorithm]); + end + + if strcmp(algorithm, 'AS-RTI-A') + ocp.solver_options.as_rti_iter = as_rti_iter; + ocp.solver_options.as_rti_level = 0; + elseif strcmp(algorithm, 'AS-RTI-B') + ocp.solver_options.as_rti_iter = as_rti_iter; + ocp.solver_options.as_rti_level = 1; + elseif strcmp(algorithm, 'AS-RTI-C') + ocp.solver_options.as_rti_iter = as_rti_iter; + ocp.solver_options.as_rti_level = 2; + elseif strcmp(algorithm, 'AS-RTI-D') + ocp.solver_options.as_rti_iter = as_rti_iter; + ocp.solver_options.as_rti_level = 3; + end + + ocp.solver_options.qp_solver_cond_N = N_horizon; + + % set prediction horizon + ocp.solver_options.tf = Tf; + + ocp.model.name = strcat(ocp.model.name, '_', strrep(algorithm, '-', '_')); + ocp_solver = AcadosOcpSolver(ocp); + + % create an integrator with the same settings as used in the OCP solver + sim = create_AcadosSim_from_AcadosOcp(ocp); + integrator = AcadosSimSolver(sim); +end + +function main(algorithm, as_rti_iter) + if nargin < 2 + as_rti_iter = 1; + end + + x0 = [0.0; pi; 0.0; 0.0]; + Fmax = 80; + + Tf = 0.8; + N_horizon = 40; + + [ocp_solver, integrator] = setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter); + + nx = ocp_solver.ocp.dims.nx; + nu = ocp_solver.ocp.dims.nu; + + Nsim = 100; + simX = zeros(Nsim+1, nx); + simU = zeros(Nsim, nu); + + simX(1, :) = x0'; + + if ~strcmp(algorithm, 'SQP') + t_preparation = zeros(Nsim, 1); + t_feedback = zeros(Nsim, 1); + else + t = zeros(Nsim, 1); + end + + % closed loop + for i = 1:Nsim + if ~strcmp(algorithm, 'SQP') + % preparation phase + ocp_solver.set('rti_phase', 1); + ocp_solver.solve(); + status = ocp_solver.get('status'); + t_preparation(i) = ocp_solver.get('time_tot'); + + if ~ismember(status, [0, 2, 5]) + error(['acados returned status ', num2str(status), '. Exiting.']); + end + + % set initial state + % NOTE: all bounds can be updated between the phases + % all other updates, such as parameters are not used in the preparation phase + ocp_solver.set('constr_lbx', simX(i, :), 0); + ocp_solver.set('constr_ubx', simX(i, :), 0); + + % feedback phase + ocp_solver.set('rti_phase', 2); + ocp_solver.solve(); + t_feedback(i) = ocp_solver.get('time_tot'); + + simU(i, :) = ocp_solver.get('u', 0); + else + % solve ocp and get next control input + ocp_solver.set('constr_lbx', simX(i, :), 0); + ocp_solver.set('constr_ubx', simX(i, :), 0); + ocp_solver.solve(); + status = ocp_solver.get('status'); + simU(i, :) = ocp_solver.get('u', 0); + t(i) = ocp_solver.get('time_tot'); + end + + if ~ismember(status, [0, 2, 5]) + error(['acados returned status ', num2str(status), '. Exiting.']); + end + + % simulate system + integrator.set('x', simX(i, :)); + integrator.set('u', simU(i, :)); + sim_status = integrator.solve(); + simX(i+1, :) = integrator.get('x'); + end + + % evaluate timings + if ~strcmp(algorithm, 'SQP') + % scale to milliseconds + t_preparation = t_preparation * 1000; + t_feedback = t_feedback * 1000; + disp(['Computation time in preparation phase in ms: min ', num2str(min(t_preparation)), ... + ' median ', num2str(median(t_preparation)), ' max ', num2str(max(t_preparation))]); + disp(['Computation time in feedback phase in ms: min ', num2str(min(t_feedback)), ... + ' median ', num2str(median(t_feedback)), ' max ', num2str(max(t_feedback))]); + else + % scale to milliseconds + t = t * 1000; + disp(['Computation time in ms: min ', num2str(min(t)), ... + ' median ', num2str(median(t)), ' max ', num2str(max(t))]); + end + + if 1 % plot + figure; + subplot(2,1,1); + plot(0:Nsim, simX); + xlim([0 Nsim]); + legend('p', 'theta', 'v', 'omega'); + subplot(2,1,2); + plot(0:Nsim-1, simU); + xlim([0 Nsim]); + legend('F'); + title(['algorithm: ', algorithm, '-', num2str(as_rti_iter)]); + + % if is_octave() + % waitforbuttonpress; + % end + end +end diff --git a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py index dc45148360..c80f8410d2 100644 --- a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py +++ b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py @@ -45,6 +45,7 @@ ALGORITHMS = ["SQP"] + REAL_TIME_ALGORITHMS def setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter=1): + print(f'running with algorithm: {algorithm}, as_rti_iter: {as_rti_iter}') # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -112,7 +113,7 @@ def setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter=1): ocp.solver_options.tf = Tf solver_json = 'acados_ocp_' + model.name + '.json' - acados_ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json) + acados_ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False) # create an integrator with the same settings as used in the OCP solver. acados_integrator = AcadosSimSolver(ocp, json_file = solver_json) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index bd17c3a10f..272cca6c7c 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -77,6 +77,11 @@ if(ACADOS_OCTAVE) COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/simple_dae_model octave --no-gui --no-window-system ./example_ocp.m) + # AS-RTI closed loop example + add_test(NAME octave_closed_loop_as_rti_example + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/pendulum_on_cart_model + octave --no-gui --no-window-system ./closed_loop_as_rti.m) + add_test(NAME octave_test_mhe_lorentz COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/lorentz octave --no-gui --no-window-system ./example_mhe.m) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 88f889b970..79b638c958 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -745,6 +745,12 @@ function make_consistent(self, is_mocp_phase) error('tau_min > 0 is only compatible with HPIPM.'); end + if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any([strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]) + error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); + end + % Set default parameters for globalization ddp_with_merit_or_funnel = strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') || (strcmp(opts.globalization, 'MERIT_BACKTRACKING') && strcmp(opts.nlp_solver_type, 'DDP')); @@ -875,27 +881,19 @@ function make_consistent(self, is_mocp_phase) if isempty(cost_types{1}) warning("cost_type_0 not set, using path cost"); self.cost.cost_type_0 = self.cost.cost_type; - if (strcmp(self.cost.cost_type, 'LINEAR_LS')) - self.cost.Vx_0 = self.cost.Vx; - self.cost.Vu_0 = self.cost.Vu; - self.cost.Vz_0 = self.cost.Vz; - elseif (strcmp(self.cost.cost_type, 'NONLINEAR_LS')) - self.model.cost_y_expr_0 = self.model.cost_y_expr; - elseif (strcmp(self.cost.cost_type, 'EXTERNAL')) - self.cost.cost_ext_fun_type_0 = self.cost.cost_ext_fun_type; - if strcmp(self.cost.cost_ext_fun_type_0, 'casadi') - self.model.cost_expr_ext_cost_0 = self.model.cost_expr_ext_cost; - self.model.cost_expr_ext_cost_custom_hess_0 = self.model.cost_expr_ext_cost_custom_hess; - else % generic - self.cost.cost_source_ext_cost_0 = self.cost.cost_source_ext_cost; - self.cost.cost_function_ext_cost_0 = self.cost.cost_function_ext_cost; - end - end - if (strcmp(self.cost.cost_type, 'LINEAR_LS')) || (strcmp(self.cost.cost_type, 'NONLINEAR_LS')) - self.cost.W_0 = self.cost.W; - self.cost.yref_0 = self.cost.yref; - self.dims.ny_0 = self.dims.ny; - end + self.cost.Vx_0 = self.cost.Vx; + self.cost.Vu_0 = self.cost.Vu; + self.cost.Vz_0 = self.cost.Vz; + self.model.cost_y_expr_0 = self.model.cost_y_expr; + self.cost.cost_ext_fun_type_0 = self.cost.cost_ext_fun_type; + self.model.cost_expr_ext_cost_0 = self.model.cost_expr_ext_cost; + self.model.cost_expr_ext_cost_custom_hess_0 = self.model.cost_expr_ext_cost_custom_hess; + self.cost.cost_source_ext_cost_0 = self.cost.cost_source_ext_cost; + self.cost.cost_function_ext_cost_0 = self.cost.cost_function_ext_cost; + self.cost.W_0 = self.cost.W; + self.cost.yref_0 = self.cost.yref; + self.model.cost_psi_expr_0 = self.model.cost_psi_expr; + self.model.cost_r_in_psi_expr_0 = self.model.cost_r_in_psi_expr; end % detect constraint structure diff --git a/interfaces/acados_matlab_octave/create_AcadosSim_from_AcadosOcp.m b/interfaces/acados_matlab_octave/create_AcadosSim_from_AcadosOcp.m new file mode 100644 index 0000000000..32f17a0d1c --- /dev/null +++ b/interfaces/acados_matlab_octave/create_AcadosSim_from_AcadosOcp.m @@ -0,0 +1,57 @@ +% +% 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.; +% + +% creates an AcadosSim object from an AcadosOcp object with the same model and integrator options corresponding to the first stage of the OCP +function sim = create_AcadosSim_from_AcadosOcp(ocp) + + if ~isa(ocp, 'AcadosOcp') + error('create_AcadosSim_from_AcadosOcp: First argument must be an AcadosOcp object.'); + end + ocp.make_consistent(); + if strcmp(ocp.solver_options.integrator_type, 'DISCRETE') + error('create_AcadosSim_from_AcadosOcp: AcadosOcp cannot have integrator_type DISCRETE.'); + end + if ~ocp.model.p_global.is_empty() + error('create_AcadosSim_from_AcadosOcp: AcadosOcp cannot have p_global.'); + end + sim = AcadosSim(); + sim.model = ocp.model; + % copy all relevant options + sim.solver_options.integrator_type = ocp.solver_options.integrator_type; + sim.solver_options.collocation_type = ocp.solver_options.collocation_type; + sim.solver_options.Tsim = ocp.solver_options.Tsim; + sim.solver_options.num_stages = ocp.solver_options.sim_method_num_stages(1); + sim.solver_options.num_steps = ocp.solver_options.sim_method_num_steps(1); + sim.solver_options.newton_iter = ocp.solver_options.sim_method_newton_iter(1); + sim.solver_options.newton_tol = ocp.solver_options.sim_method_newton_tol(1); + sim.solver_options.jac_reuse = ocp.solver_options.sim_method_jac_reuse(1); + sim.solver_options.ext_fun_compile_flags = ocp.solver_options.ext_fun_compile_flags; + sim.parameter_values = ocp.parameter_values; +end \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index e14b70188b..2be43613e5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -980,6 +980,10 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: opts.globalization_full_step_dual = 0 + # AS-RTI + if opts.as_rti_level in [1, 2] and any([cost.cost_type.endswith('LINEAR_LS'), cost.cost_type_0.endswith('LINEAR_LS'), cost.cost_type_e.endswith('LINEAR_LS')]): + raise NotImplementedError('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.') + # sanity check for Funnel globalization and SQP if opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' and opts.nlp_solver_type not in ['SQP', 'SQP_WITH_FEASIBLE_QP']: raise NotImplementedError('FUNNEL_L1PEN_LINESEARCH only supports SQP.') 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 e905a89f21..3145928dc7 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 @@ -127,12 +127,12 @@ function eval_param_sens(obj, field, stage, index) % obj.get(field, value, [stage]) obj = varargin{1}; field = varargin{2}; - if any(strfind('sens', field)) - error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') - end if ~isa(field, 'char') error('field must be a char vector, use '' '''); end + if any(strfind('sens', field)) + error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.') + end if nargin==2 value = ocp_get(obj.C_ocp, field); From 0285deb9dc6918680c53fb7f18c8fa64591dcb18 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 7 Apr 2025 17:25:19 +0200 Subject: [PATCH 020/164] Fix globalization calls in AS-RTI (#1491) - fix AS-RTI, broken in #1254 - add tests on closed loop simulation results --- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 8 ++++---- .../pendulum_on_cart_model/closed_loop_as_rti.m | 2 +- .../as_rti/as_rti_closed_loop_example.py | 15 ++++++++++++++- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index e67f3bf2a3..8a75be3fd9 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -899,7 +899,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // update variables double step_size; - globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_out, &step_size); + globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_opts, &step_size); if (globalization_status != ACADOS_SUCCESS) { if (nlp_opts->print_level > 1) @@ -967,7 +967,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // update variables double step_size; - globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_out, &step_size); + globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_opts, &step_size); if (globalization_status != ACADOS_SUCCESS) { if (nlp_opts->print_level > 1) @@ -1031,7 +1031,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // update variables double step_size; - globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_out, &step_size); + globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_opts, &step_size); if (globalization_status != ACADOS_SUCCESS) { if (nlp_opts->print_level > 1) @@ -1097,7 +1097,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // update variables double step_size; - globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_out, &step_size); + globalization_status = config->globalization->find_acceptable_iterate(config, dims, nlp_in, nlp_out, nlp_mem, mem, nlp_work, nlp_opts, &step_size); if (globalization_status != ACADOS_SUCCESS) { if (nlp_opts->print_level > 1) diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m b/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m index 6c2d0d2581..1205e0777b 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/closed_loop_as_rti.m @@ -173,7 +173,7 @@ function main(algorithm, as_rti_iter) % set initial state % NOTE: all bounds can be updated between the phases - % all other updates, such as parameters are not used in the preparation phase + % all other updates, such as parameters are not used in the feedback phase ocp_solver.set('constr_lbx', simX(i, :), 0); ocp_solver.set('constr_ubx', simX(i, :), 0); diff --git a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py index c80f8410d2..5432e47551 100644 --- a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py +++ b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py @@ -199,11 +199,24 @@ def main(algorithm='RTI', as_rti_iter=1): # plot results plot_pendulum(np.linspace(0, (Tf/N_horizon)*Nsim, Nsim+1), Fmax, simU, simX, title=algorithm) + # check terminal state + if algorithm == "RTI": + x_terminal_ref = np.array([-0.01402487, -0.02343146, 0.00874453, 0.07601564]) + else: + x_terminal_ref = np.array([-0.0028129 , -0.00106827, 0.00653341, 0.00663193]) + + x_terminal = simX[-1, :] + if np.linalg.norm(x_terminal - x_terminal_ref) > 1e-3: + raise Exception(f"Terminal state {x_terminal} does not match reference {x_terminal_ref}. Exiting.") + else: + print(f"Terminal state {x_terminal} matches reference {x_terminal_ref}. Test passed.") + + # delete solver ocp_solver = None if __name__ == '__main__': - main(algorithm="AS-RTI-D", as_rti_iter=1) + # main(algorithm="AS-RTI-D", as_rti_iter=1) for algorithm in ["SQP", "RTI", "AS-RTI-A", "AS-RTI-B", "AS-RTI-C", "AS-RTI-D"]: main(algorithm=algorithm, as_rti_iter=1) From df4ed6d57cd3c68547c50518cb1406c150ad76e8 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 8 Apr 2025 15:06:52 +0200 Subject: [PATCH 021/164] Consistent mask and multipliers (#1333) This PR addresses the issue that masked constraints and multipliers might be inconsistent: If a constraint is masked, the corresponding multiplier should always be zero. - The mask is updated whenever the constraint bounds are updated. - lambdas are multiplied with the mask whenever they or the bounds are updated `dmask` is now stored in `ocp_nlp_in` as the mask ist now required in `ocp_nlp_out_set` and `ocp_nlp_constraints_model_set`. BREAKING CHANGES in C interface: - `ocp_nlp_out_set` now requires `nlp_in` as additional argument - `ocp_nlp_constraints_model_set` now requires `nlp_out` as additional argument - `ocp_nlp_out` needs to be created before `ocp_nlp_in` --------- Co-authored-by: Jonathan Frey --- acados/dense_qp/dense_qp_ooqp.c | 6 +- acados/dense_qp/dense_qp_qore.c | 16 +- acados/dense_qp/dense_qp_qpoases.c | 16 +- acados/ocp_nlp/ocp_nlp_common.c | 123 ++++++------ acados/ocp_nlp/ocp_nlp_common.h | 4 +- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 143 +++++++++----- acados/ocp_nlp/ocp_nlp_constraints_bgh.h | 2 +- acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 141 ++++++++------ acados/ocp_nlp/ocp_nlp_constraints_bgp.h | 4 +- acados/ocp_nlp/ocp_nlp_constraints_common.c | 2 +- acados/ocp_nlp/ocp_nlp_constraints_common.h | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 4 +- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 2 +- .../tests/one_sided_constraints_test.py | 23 ++- examples/c/engine_example.c | 41 ++-- examples/c/nonlinear_chain_ocp_nlp.c | 44 +++-- examples/c/simple_dae_example.c | 17 +- examples/c/wind_turbine_nmpc.c | 64 +++--- examples/c/wind_turbine_nmpc_soft.c | 72 +++---- .../pendulum/acados_solver_pendulum_ode.c | 60 +++--- .../pendulum/main_pendulum_ode.c | 10 +- interfaces/acados_c/ocp_nlp_interface.c | 18 +- interfaces/acados_c/ocp_nlp_interface.h | 4 +- .../acados_template/acados_ocp_solver.py | 17 +- .../acados_template/acados_ocp_solver_pyx.pyx | 6 +- .../acados_template/acados_solver_common.pxd | 4 +- .../c_templates_tera/acados_multi_solver.in.c | 181 ++++++++--------- .../c_templates_tera/acados_solver.in.c | 182 +++++++++--------- .../c_templates_tera/main.in.c | 10 +- .../c_templates_tera/main_multi.in.c | 6 +- .../matlab_templates/acados_mex_set.in.c | 26 +-- .../matlab_templates/acados_solver_sfun.in.c | 32 +-- .../custom_update_function_zoro_template.in.c | 32 +-- test/ocp_nlp/test_wind_turbine.cpp | 61 +++--- 34 files changed, 754 insertions(+), 621 deletions(-) diff --git a/acados/dense_qp/dense_qp_ooqp.c b/acados/dense_qp/dense_qp_ooqp.c index feafc10573..b25497a946 100644 --- a/acados/dense_qp/dense_qp_ooqp.c +++ b/acados/dense_qp/dense_qp_ooqp.c @@ -40,6 +40,7 @@ // blasfeo #include "blasfeo_d_aux.h" #include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_blas.h" // acados #include "acados/dense_qp/dense_qp_ooqp.h" @@ -601,8 +602,11 @@ int_t dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, voi acados_tic(&interface_timer); fill_in_qp_out(qp_in, qp_out, work); dense_qp_compute_t(qp_in, qp_out); - info->interface_time += acados_toc(&interface_timer); + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); + + info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = -1; info->t_computed = 1; diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 3e37a68e5f..3da2c73909 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -36,6 +36,7 @@ // blasfeo #include "blasfeo_d_aux.h" #include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_blas.h" // acados #include "acados/dense_qp/dense_qp_common.h" #include "acados/dense_qp/dense_qp_qore.h" @@ -537,12 +538,8 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } - info->interface_time += acados_toc(&interface_timer); - info->total_time = acados_toc(&tot_timer); - info->num_iter = num_iter; - - mem->time_qp_solver_call = info->solve_QP_time; - mem->iter = num_iter; + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); // compute slacks if (opts->compute_t) @@ -551,6 +548,13 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void info->t_computed = 1; } + info->interface_time += acados_toc(&interface_timer); + info->total_time = acados_toc(&tot_timer); + info->num_iter = num_iter; + + mem->time_qp_solver_call = info->solve_QP_time; + mem->iter = num_iter; + int acados_status = qore_status; if (qore_status == QPSOLVER_DENSE_OPTIMAL) acados_status = ACADOS_SUCCESS; if (qore_status == QPSOLVER_DENSE_ITER_LIMIT) acados_status = ACADOS_MAXITER; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 36cc6fa30f..f7ac8355c4 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -742,6 +742,16 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } + // compute slacks + if (opts->compute_t) + { + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; + } + + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); + info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = nwsr; @@ -749,12 +759,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo memory->time_qp_solver_call = info->solve_QP_time; memory->iter = nwsr; - // compute slacks - if (opts->compute_t) - { - dense_qp_compute_t(qp_in, qp_out); - info->t_computed = 1; - } int acados_status = qpoases_status; if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS; diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1ffbce8f55..65d6636454 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -779,17 +779,21 @@ void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, * in ************************************************/ -static acados_size_t ocp_nlp_in_calculate_size_self(ocp_nlp_dims *dims) +acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) { int N = dims->N; + int i; + acados_size_t size = sizeof(ocp_nlp_in); size += N * sizeof(double); // Ts + // parameter values - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += dims->np[i] * sizeof(double); } + // global_data size += dims->n_global_data * sizeof(double); @@ -801,38 +805,34 @@ static acados_size_t ocp_nlp_in_calculate_size_self(ocp_nlp_dims *dims) size += (N + 1) * sizeof(void *); // constraints - size += 4*8; // aligns - return size; -} - + size += (N + 1) * sizeof(struct blasfeo_dvec); // dmask - -acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) -{ - int N = dims->N; - - acados_size_t size = ocp_nlp_in_calculate_size_self(dims); + for (i = 0; i <= N; i++) + { + size += blasfeo_memsize_dvec(2*dims->ni[i]); // dmask + } // dynamics - for (int i = 0; i < N; i++) + for (i = 0; i < N; i++) { - size += - config->dynamics[i]->model_calculate_size(config->dynamics[i], dims->dynamics[i]); + size += config->dynamics[i]->model_calculate_size(config->dynamics[i], dims->dynamics[i]); } // cost - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += config->cost[i]->model_calculate_size(config->cost[i], dims->cost[i]); } // constraints - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += config->constraints[i]->model_calculate_size(config->constraints[i], - dims->constraints[i]); + dims->constraints[i]); } + size += 4*8 + 64; // aligns + make_int_multiple_of(8, &size); return size; @@ -840,9 +840,10 @@ acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *di -static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) +ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) { int N = dims->N; + char *c_ptr = (char *) raw_memory; // initial align @@ -852,30 +853,7 @@ static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) ocp_nlp_in *in = (ocp_nlp_in *) c_ptr; c_ptr += sizeof(ocp_nlp_in); - // align - align_char_to(8, &c_ptr); - - // double pointers - assign_and_advance_double_ptrs(N+1, &in->parameter_values, &c_ptr); - - align_char_to(8, &c_ptr); - - // doubles - // Ts - assign_and_advance_double(N, &in->Ts, &c_ptr); - - // parameter values - for (int i = 0; i <= N; i++) - { - assign_and_advance_double(dims->np[i], &in->parameter_values[i], &c_ptr); - for (int ip = 0; ip < dims->np[i]; ip++) - { - in->parameter_values[i][ip] = 0.0; - } - } - assign_and_advance_double(dims->n_global_data, &in->global_data, &c_ptr); - - + // ** pointers to substructures ** // dynamics in->dynamics = (void **) c_ptr; c_ptr += N * sizeof(void *); @@ -888,24 +866,13 @@ static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) in->constraints = (void **) c_ptr; c_ptr += (N + 1) * sizeof(void *); + // align align_char_to(8, &c_ptr); - assert((char *) raw_memory + ocp_nlp_in_calculate_size_self(dims) >= c_ptr); - - return in; -} - - - -ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) -{ - int N = dims->N; - - char *c_ptr = (char *) raw_memory; + // ** substructures ** - // struct - ocp_nlp_in *in = ocp_nlp_in_assign_self(dims, c_ptr); - c_ptr += ocp_nlp_in_calculate_size_self(dims); + // dmask + assign_and_advance_blasfeo_dvec_structs(N + 1, &in->dmask, &c_ptr); // dynamics for (int i = 0; i < N; i++) @@ -932,8 +899,44 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * dims->constraints[i]); } + // ** doubles ** + // Ts + assign_and_advance_double(N, &in->Ts, &c_ptr); + + // double pointers + assign_and_advance_double_ptrs(N+1, &in->parameter_values, &c_ptr); + align_char_to(8, &c_ptr); + + // parameter values + for (int i = 0; i <= N; i++) + { + assign_and_advance_double(dims->np[i], &in->parameter_values[i], &c_ptr); + for (int ip = 0; ip < dims->np[i]; ip++) + { + in->parameter_values[i][ip] = 0.0; + } + } + assign_and_advance_double(dims->n_global_data, &in->global_data, &c_ptr); + + // blasfeo_mem align + align_char_to(64, &c_ptr); + + // dmask + for (int i = 0; i <= N; ++i) + { + assign_and_advance_blasfeo_dvec_mem(2 * dims->ni[i], in->dmask + i, &c_ptr); + } + + align_char_to(8, &c_ptr); + assert((char *) raw_memory + ocp_nlp_in_calculate_size(config, dims) >= c_ptr); + for (int i = 0; i <= N; i++) + { + blasfeo_dvecse(2*dims->ni[i], 1.0, &in->dmask[i], 0); + config->constraints[i]->model_set_dmask_ptr(&in->dmask[i], in->constraints[i]); + } + return in; } @@ -2610,7 +2613,6 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->constraints[i]->memory_set_idxb_ptr(nlp_mem->qp_in->idxb[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxs_rev_ptr(nlp_mem->qp_in->idxs_rev[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxe_ptr(nlp_mem->qp_in->idxe[i], nlp_mem->constraints[i]); - config->constraints[i]->memory_set_dmask_ptr(nlp_mem->qp_in->d_mask+i, nlp_mem->constraints[i]); if (opts->with_solution_sens_wrt_params) { config->constraints[i]->memory_set_jac_lag_stat_p_global_ptr(nlp_mem->jac_lag_stat_p_global+i, nlp_mem->constraints[i]); @@ -2618,6 +2620,9 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di } } + // set pointer to dmask in qp_in to dmask in nlp_in + nlp_mem->qp_in->d_mask = nlp_in->dmask; + // alias to regularize memory ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 669c890d1f..f1a90e5a36 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -66,7 +66,6 @@ extern "C" { #include "acados/utils/types.h" - /************************************************ * config ************************************************/ @@ -233,6 +232,9 @@ typedef struct ocp_nlp_in /// Global data double *global_data; + /// Constraint mask + struct blasfeo_dvec *dmask; + /// Pointers to cost functions (TBC). void **cost; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 21ba85761f..0f3425a3b0 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -376,6 +376,29 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims_, void *raw_ } +void ocp_nlp_constraints_bgh_update_mask_lower(ocp_nlp_constraints_bgh_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) <= -ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + +void ocp_nlp_constraints_bgh_update_mask_upper(ocp_nlp_constraints_bgh_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) >= ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, void *model_, const char *field, void *value) @@ -385,10 +408,11 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, int ii; int *ptr_i; + int offset; if (!dims || !model || !field || !value) { - printf("ocp_nlp_constraints_bgh_model_set: got Null pointer \n"); + printf("ocp_nlp_constraints_bgh_model_set: got null pointer \n"); exit(1); } @@ -409,6 +433,7 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, int nge = dims->nge; int nhe = dims->nhe; + // If model->d is updated, we always also update dmask. 0 means unconstrained. if (!strcmp(field, "idxbx")) { ptr_i = (int *) value; @@ -417,11 +442,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nbu); + offset = nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nbx, offset); } else if (!strcmp(field, "ubx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nb + ng + nh + nbu); + offset = nb + ng + nh + nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nbx, offset); } else if (!strcmp(field, "idxbu")) { @@ -431,11 +460,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, 0); + offset = 0; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nbu, offset); + } else if (!strcmp(field, "ubu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, nb + ng + nh); + offset = nb + ng + nh; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nbu, offset); } else if (!strcmp(field, "C")) { @@ -447,11 +481,17 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lg")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, nb); + offset = nb; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, ng, offset); + } else if (!strcmp(field, "ug")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, 2*nb+ng+nh); + offset = 2*nb+ng+nh; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, ng, offset); + } else if (!strcmp(field, "nl_constr_h_fun")) { @@ -475,11 +515,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lh")) { - blasfeo_pack_dvec(nh, value, 1, &model->d, nb+ng); + offset = nb+ng; + blasfeo_pack_dvec(nh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nh, offset); } else if (!strcmp(field, "uh")) { - blasfeo_pack_dvec(nh, value, 1, &model->d, 2*nb+2*ng+nh); + offset = 2*nb+2*ng+nh; + blasfeo_pack_dvec(nh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nh, offset); } else if (!strcmp(field, "idxsbu")) { @@ -489,11 +533,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nh); + offset = 2*nb+2*ng+2*nh; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "usbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nh+ns); + offset = 2*nb+2*ng+2*nh+ns; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "idxsbx")) { @@ -503,11 +551,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu); + offset = 2*nb+2*ng+2*nh+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "usbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu); + offset = 2*nb+2*ng+2*nh+ns+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbx, offset); + } else if (!strcmp(field, "idxsg")) { @@ -517,11 +570,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu+nsbx); + offset = 2*nb+2*ng+2*nh+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "usg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu+nsbx); + offset = 2*nb+2*ng+2*nh+ns+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "idxsh")) { @@ -531,11 +588,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsh")) { - blasfeo_pack_dvec(nsh, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nh+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsh, offset); + } else if (!strcmp(field, "ush")) { - blasfeo_pack_dvec(nsh, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nh+ns+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsh, offset); } else if (!strcmp(field, "idxbue")) { @@ -837,6 +899,14 @@ void *ocp_nlp_constraints_bgh_memory_assign(void *config_, void *dims_, void *op } +void ocp_nlp_constraints_bgh_model_set_dmask_ptr(struct blasfeo_dvec *dmask, void *model_) +{ + ocp_nlp_constraints_bgh_model *model = model_; + + model->dmask = dmask; +} + + struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_) { @@ -902,13 +972,6 @@ void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, vo memory->z_alg = z_alg; } -void ocp_nlp_constraints_bgh_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) -{ - ocp_nlp_constraints_bgh_memory *memory = memory_; - - memory->dmask = dmask; -} - void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_) { @@ -1418,7 +1481,7 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nh+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1458,34 +1521,8 @@ void ocp_nlp_constraints_bgh_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); - // Set dmask for QP: 0 means unconstrained. - for (int i = 0; i < nb+ng+nh; i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = nb+ng+nh; i < 2*(nb+ng+nh); i++) - { - if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = 2*(nb+ng+nh); i < 2*(nb+ng+nh+ns); i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found lower infinity bound on slacks\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nh+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1677,6 +1714,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_constraints_bgh_model_assign; config->model_set = &ocp_nlp_constraints_bgh_model_set; config->model_get = &ocp_nlp_constraints_bgh_model_get; + config->model_set_dmask_ptr = &ocp_nlp_constraints_bgh_model_set_dmask_ptr; config->opts_calculate_size = &ocp_nlp_constraints_bgh_opts_calculate_size; config->opts_assign = &ocp_nlp_constraints_bgh_opts_assign; config->opts_initialize_default = &ocp_nlp_constraints_bgh_opts_initialize_default; @@ -1691,7 +1729,6 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_, int stage) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgh_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgh_memory_set_z_alg_ptr; - config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgh_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgh_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 951b1e3db7..e3241fbd55 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -95,6 +95,7 @@ typedef struct int *idxb; int *idxs; int *idxe; + struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; // gathers bounds struct blasfeo_dmat DCt; // general linear constraint matrix // lg <= [D, C] * [u; x] <= ug @@ -156,7 +157,6 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory - struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp memory struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index e08cc6d068..9516da69c3 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -381,6 +381,31 @@ void *ocp_nlp_constraints_bgp_model_assign(void *config, void *dims_, void *raw_ } +void ocp_nlp_constraints_bgp_update_mask_lower(ocp_nlp_constraints_bgp_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) <= -ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + +void ocp_nlp_constraints_bgp_update_mask_upper(ocp_nlp_constraints_bgp_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) >= ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + + int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, void *model_, const char *field, void *value) { @@ -390,10 +415,11 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, int ii; int *ptr_i; + int offset; if (!dims || !model || !field || !value) { - printf("ocp_nlp_constraints_bgp_model_set: got Null pointer \n"); + printf("ocp_nlp_constraints_bgp_model_set: got null pointer \n"); exit(1); } @@ -414,6 +440,7 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, int nge = dims->nge; int nphie = dims->nphie; + // If model->d is updated, we always also update dmask. 0 means unconstrained. if (!strcmp(field, "idxbx")) { ptr_i = (int *) value; @@ -422,11 +449,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nbu); + offset = nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nbx, offset); } else if (!strcmp(field, "ubx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nb + ng + nphi + nbu); + offset = nb + ng + nphi + nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nbx, offset); } else if (!strcmp(field, "idxbu")) { @@ -436,11 +467,16 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbu")) { + offset = 0; blasfeo_pack_dvec(nbu, value, 1, &model->d, 0); + ocp_nlp_constraints_bgp_update_mask_lower(model, nbu, offset); + } else if (!strcmp(field, "ubu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, nb + ng + nphi); + offset = nb + ng + nphi; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nbu, offset); } else if (!strcmp(field, "C")) { @@ -452,11 +488,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lg")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, nb); + offset = nb; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, ng, offset); } else if (!strcmp(field, "ug")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, 2*nb+ng+nphi); + offset = 2*nb+ng+nphi; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, ng, offset); } else if (!strcmp(field, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux")) { @@ -468,11 +508,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lphi")) { - blasfeo_pack_dvec(nphi, value, 1, &model->d, nb+ng); + offset = nb + ng; + blasfeo_pack_dvec(nphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nphi, offset); } else if (!strcmp(field, "uphi")) { - blasfeo_pack_dvec(nphi, value, 1, &model->d, 2*nb+2*ng+nphi); + offset = 2*nb+2*ng+nphi; + blasfeo_pack_dvec(nphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nphi, offset); } else if (!strcmp(field, "idxsbu")) { @@ -482,11 +526,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nphi); + offset = 2*nb+2*ng+2*nphi; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "usbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns); + offset = 2*nb+2*ng+2*nphi+ns; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "idxsbx")) { @@ -496,11 +544,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu); + offset = 2*nb+2*ng+2*nphi+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "usbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu); + offset = 2*nb+2*ng+2*nphi+ns+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "idxsg")) { @@ -510,11 +562,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu+nsbx); + offset = 2*nb+2*ng+2*nphi+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "usg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu+nsbx); + offset = 2*nb+2*ng+2*nphi+ns+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "idxsphi")) { @@ -524,11 +580,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsphi")) { - blasfeo_pack_dvec(nsphi, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nphi+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsphi, offset); } else if (!strcmp(field, "usphi")) { - blasfeo_pack_dvec(nsphi, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nphi+ns+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsphi, offset); } else if (!strcmp(field, "idxbue")) { @@ -820,6 +880,12 @@ void *ocp_nlp_constraints_bgp_memory_assign(void *config_, void *dims_, void *op } +void ocp_nlp_constraints_bgp_model_set_dmask_ptr(struct blasfeo_dvec *dmask, void *model_) +{ + ocp_nlp_constraints_bgp_model *model = model_; + model->dmask = dmask; +} + struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_) { @@ -865,13 +931,6 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * } -void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) -{ - ocp_nlp_constraints_bgp_memory *memory = memory_; - memory->dmask = dmask; -} - - void ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_) { ocp_nlp_constraints_bgp_memory *memory = memory_; @@ -1312,7 +1371,7 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nphi), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nphi), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; @@ -1355,38 +1414,8 @@ void ocp_nlp_constraints_bgp_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); - // Set dmask for QP: 0 means unconstrained. - for (int i = 0; i < nb+ng+nphi; i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = nb+ng+nphi; i < 2*(nb+ng+nphi); i++) - { - if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = 2*(nb+ng+nphi); i < 2*(nb+ng+nphi+ns); i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found lower infinity bound on slacks\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nphi+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); - - // printf("bgp mask\n"); - // blasfeo_print_tran_dvec(2*(nb+ng+nphi), memory->dmask, 0); - // blasfeo_print_exp_tran_dvec(2*(nb+ng+nphi), &model->d, 0); + blasfeo_dvecmul(2*(nb+ng+nphi+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1445,6 +1474,7 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_constraints_bgp_model_assign; config->model_set = &ocp_nlp_constraints_bgp_model_set; config->model_get = &ocp_nlp_constraints_bgp_model_get; + config->model_set_dmask_ptr = &ocp_nlp_constraints_bgp_model_set_dmask_ptr; config->opts_calculate_size = &ocp_nlp_constraints_bgp_opts_calculate_size; config->opts_assign = &ocp_nlp_constraints_bgp_opts_assign; config->opts_initialize_default = &ocp_nlp_constraints_bgp_opts_initialize_default; @@ -1459,7 +1489,6 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_, int stage) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgp_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgp_memory_set_z_alg_ptr; - config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgp_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgp_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index 97dfea0bef..86c992c43b 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -89,6 +89,7 @@ typedef struct int *idxb; int *idxs; int *idxe; + struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; struct blasfeo_dmat DCt; external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux; @@ -136,7 +137,6 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory - struct blasfeo_dvec *dmask; // pointer to dmask in qp_in struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory @@ -163,8 +163,6 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * // void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // -void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_); -// void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); // void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.c b/acados/ocp_nlp/ocp_nlp_constraints_common.c index 719c8edc21..4bc5378547 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.c @@ -65,4 +65,4 @@ ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory) c_ptr += sizeof(ocp_nlp_constraints_config); return config; -} +} \ No newline at end of file diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index c194f4f270..fdfca35594 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -61,6 +61,7 @@ typedef struct void *(*model_assign)(void *config, void *dims, void *raw_memory); int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value); void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value); + void (*model_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *model_); acados_size_t (*opts_calculate_size)(void *config, void *dims); void *(*opts_assign)(void *config, void *dims, void *raw_memory); void (*opts_initialize_default)(void *config, void *dims, void *opts); @@ -74,7 +75,6 @@ typedef struct void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory); void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); - void (*memory_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *memory); void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory); void (*memory_set_idxb_ptr)(int *idxb, void *memory); void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 8a75be3fd9..833a06938e 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -749,8 +749,8 @@ static void as_rti_advance_problem(ocp_nlp_config *config, ocp_nlp_dims *dims, o } if (opts->as_rti_advancement_strategy != NO_ADVANCE) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", nlp_work->tmp_nv_double); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", nlp_work->tmp_nv_double); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", nlp_work->tmp_nv_double); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", nlp_work->tmp_nv_double); } // printf("advanced x value\n"); // blasfeo_print_exp_tran_dvec(dims->nx[1], nlp_out->ux+1, dims->nu[1]); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 4fe685f69c..aab1ac0886 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1344,7 +1344,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf blasfeo_dveccp(2*n_nominal_ineq_nlp+ns[i], nominal_qp_in->d + i, 0, relaxed_qp_in->d + i, 0); blasfeo_dveccp(ns[i], nominal_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i], relaxed_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i]+nns[i]); } - // setup d_mask + // setup d_mask; TODO: this is only needed at the start of each NLP solve if (sqp_iter == 0) { int offset_dmask; diff --git a/examples/acados_python/tests/one_sided_constraints_test.py b/examples/acados_python/tests/one_sided_constraints_test.py index 05e0e838db..f78f948b3c 100644 --- a/examples/acados_python/tests/one_sided_constraints_test.py +++ b/examples/acados_python/tests/one_sided_constraints_test.py @@ -144,10 +144,29 @@ def main(constraint_variant='one_sided', # if unbounded constraint is defined properly, lambda should be zero i_infty = 1 if constraint_variant == 'one_sided': - assert lam[i_infty] == 0 + for lam in lambdas: + assert lam[i_infty] == 0 elif (constraint_variant == 'one_sided_wrong_infty' and qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']): - assert lam[i_infty] != 0 + for lam in lambdas: + assert lam[i_infty] != 0 + + if constraint_variant == 'one_sided': + stage = 1 + # check setting lambdas + ocp_solver.set(stage, "lam", np.ones(lambdas[0].shape)) + lam = ocp_solver.get(stage, "lam") + assert lam[i_infty] == 0 + + # check updating bound + i_infty_new = 2 # lam is ordered as lbu, lbx, ubu, ubx + ocp_solver.constraints_set(stage, "lbx", -10.) + ocp_solver.set(stage, "lam", np.ones(lambdas[0].shape)) + ocp_solver.constraints_set(stage, "ubu", ACADOS_INFTY) + lam = ocp_solver.get(stage, "lam") + + assert lam[i_infty_new] == 0 + assert lam[i_infty] == 1. if PLOT: plot_pendulum(np.linspace(0, Tf, N_horizon + 1), Fmax, simU0, simX0, latexify=False, plt_show=True, X_true_label=f'N={N_horizon}, Tf={Tf}') diff --git a/examples/c/engine_example.c b/examples/c/engine_example.c index 328cf2bc68..3aef29bc37 100644 --- a/examples/c/engine_example.c +++ b/examples/c/engine_example.c @@ -226,6 +226,8 @@ int main() ocp_nlp_dims_set_constraints(config, dims, i, "nbu", &nbu[i]); } + // out + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); // in ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -256,22 +258,22 @@ int main() } // constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx); for (int i = 1; i <= N; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx); } for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu); } // options @@ -288,20 +290,17 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "tol_ineq", &tol); ocp_nlp_solver_opts_set(config, nlp_opts, "tol_comp", &tol); - // out - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - // solver ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); // initialize for (int i = 0; i < N; ++i) { - ocp_nlp_out_set(config, dims, nlp_out, i, "u", u); - ocp_nlp_out_set(config, dims, nlp_out, i, "x", x0); - ocp_nlp_out_set(config, dims, nlp_out, i, "z", z0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "u", u); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "x", x0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "z", z0); } - ocp_nlp_out_set(config, dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, N, "x", x0); status = ocp_nlp_precompute(solver, nlp_in, nlp_out); @@ -330,16 +329,16 @@ int main() ocp_nlp_out_get(config, dims, nlp_out, jj, "z", z_sol+jj*nz_); // set x0 - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x_sol+1*nx_); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x_sol+1*nx_); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x_sol+1*nx_); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x_sol+1*nx_); // shift guess for(jj=0; jjget(config, dims, solver->mem, "sqp_iter", &sqp_iter); config->get(config, dims, solver->mem, "time_tot", &time_tot); diff --git a/examples/c/nonlinear_chain_ocp_nlp.c b/examples/c/nonlinear_chain_ocp_nlp.c index b4d90d730b..fd6183bb4a 100644 --- a/examples/c/nonlinear_chain_ocp_nlp.c +++ b/examples/c/nonlinear_chain_ocp_nlp.c @@ -1148,7 +1148,13 @@ int main() // #endif /************************************************ - * nlp_in + * ocp_nlp_out + ************************************************/ + + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + + /************************************************ + * ocp_nlp_in ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -1259,10 +1265,10 @@ int main() // fist stage #if CONSTRAINTS==0 // box constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lb0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lb0+NU); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ub0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ub0+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lb0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lb0+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ub0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ub0+NU); constraints[0]->idxb = idxb0; #elif CONSTRAINTS==1 // general constraints double *Cu0; d_zeros(&Cu0, ng[0], nu[0]); @@ -1275,8 +1281,8 @@ int main() blasfeo_pack_tran_dmat(ng[0], nu[0], Cu0, ng[0], &constraints[0]->DCt, 0, 0); blasfeo_pack_tran_dmat(ng[0], nx[0], Cx0, ng[0], &constraints[0]->DCt, nu[0], 0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lg", lb0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ug", ub0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lg", lb0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ug", ub0); d_free(Cu0); d_free(Cx0); @@ -1288,23 +1294,23 @@ int main() // ocp_nlp_constraints_bgh_model **nl_constr = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints; // nl_constr[0]->nl_constr_h_fun_jac = &nonlin_constr_generic; -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lg", lb0); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ug", ub0); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lh", &lb0[ng[0]]); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "uh", &ub0[ng[0]]); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lg", lb0); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ug", ub0); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lh", &lb0[ng[0]]); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "uh", &ub0[ng[0]]); #endif // other stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lb1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lb1+NU); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ub1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ub1+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lb1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lb1+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ub1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ub1+NU); constraints[i]->idxb = idxb1; } - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubN); constraints[NN]->idxb = idxbN; @@ -1357,11 +1363,9 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "tol_comp", &tol_comp); /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); int status = ocp_nlp_precompute(solver, nlp_in, nlp_out); diff --git a/examples/c/simple_dae_example.c b/examples/c/simple_dae_example.c index 8cb8bc08c8..4a1e91d3bf 100644 --- a/examples/c/simple_dae_example.c +++ b/examples/c/simple_dae_example.c @@ -293,6 +293,8 @@ int main() { c_ptr += external_function_param_casadi_calculate_size(impl_ode_jac_x_xdot_u_z+ii, 0, &ext_fun_opts); } + // ocp_nlp_out, ocp_nlp_in + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); for (int i = 0; i < N; ++i) @@ -333,8 +335,8 @@ int main() { // bounds ocp_nlp_constraints_bgh_model **constraints = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints; - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0); constraints[0]->idxb = idxb_0; if (FORMULATION == 2) { @@ -349,14 +351,14 @@ int main() { nl_constr_h_fun_jac[ii].casadi_n_in = &simple_dae_constr_h_fun_jac_ut_xt_n_in; nl_constr_h_fun_jac[ii].casadi_n_out = &simple_dae_constr_h_fun_jac_ut_xt_n_out; external_function_param_casadi_create(&nl_constr_h_fun_jac[ii], 0, &ext_fun_opts); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "lh", lh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "uh", uh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "nl_constr_h_fun_jac", &nl_constr_h_fun_jac[ii]); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "lh", lh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "uh", uh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "nl_constr_h_fun_jac", &nl_constr_h_fun_jac[ii]); } } else { for (int ii = 1; ii < N; ++ii) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "ubx", uh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "lbx", lh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "ubx", uh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "lbx", lh); constraints[ii]->idxb = idxb; } } @@ -392,7 +394,6 @@ int main() { ocp_nlp_solver_opts_set(config, nlp_opts, "qp_cond_N", &N2); } - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); for (int i = 0; i <= N; ++i) { blasfeo_dvecse(nu[i]+nx[i], 0.0, nlp_out->ux+i, 0); blasfeo_dvecse(2, 0.0, nlp_out->ux+i, 0); diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index f435070ab3..60bf0acd3e 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -652,9 +652,15 @@ int main() } } + /************************************************ + * ocp_nlp_out + ************************************************/ + + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + ocp_nlp_out *sens_nlp_out = ocp_nlp_out_create(config, dims); /************************************************ - * nlp_in + * ocp_nlp_in ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -751,26 +757,26 @@ int main() /* box constraints */ // fist stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbu", idxbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ubu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbu", idxbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ubu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ubx0); // middle stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx1); } // last stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxbx", idxbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxbx", idxbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubxN); /* nonlinear constraints */ @@ -779,9 +785,9 @@ int main() { if(nh[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -792,9 +798,9 @@ int main() { if (ns[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } } @@ -894,13 +900,9 @@ int main() } /************************************************ - * ocp_nlp_out & solver + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - - ocp_nlp_out *sens_nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -932,8 +934,8 @@ int main() } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); // store x0 for(int ii=0; ii0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -835,19 +840,19 @@ int main() { if (ns[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsbx", idxsbx1); // Added for testing soft constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsbx", lsbx1); // Added for testing soft constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "usbx", usbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsbx", idxsbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsbx", lsbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "usbx", usbx1); // Added for testing soft constraints } - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxsbx", idxsbxN); // Added for testing soft constraints - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lsbx", lsbxN); // Added for testing soft constraints - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "usbx", usbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxsbx", idxsbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lsbx", lsbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "usbx", usbxN); // Added for testing soft constraints /************************************************ @@ -944,12 +949,11 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "qp_cond_N", &cond_N); } + /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -981,8 +985,8 @@ int main() } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); // store x0 for(int ii=0; iinlp_in = nlp_in; ocp_nlp_in * nlp_in = capsule->nlp_in; + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps @@ -512,9 +511,9 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons lbx0[1] = 3.141592653589793; ubx0[1] = 3.141592653589793; - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); // idxbxe_0 @@ -524,7 +523,7 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons idxbxe_0[1] = 1; idxbxe_0[2] = 2; idxbxe_0[3] = 3; - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); /* constraints that are the same for initial and intermediate */ @@ -541,9 +540,9 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -645,6 +644,7 @@ void pendulum_ode_acados_set_nlp_out(pendulum_ode_solver_capsule* capsule) ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); @@ -659,11 +659,11 @@ void pendulum_ode_acados_set_nlp_out(pendulum_ode_solver_capsule* capsule) for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -708,25 +708,25 @@ int pendulum_ode_acados_create_with_discretization(pendulum_ode_solver_capsule* capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); pendulum_ode_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + pendulum_ode_acados_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) setup functions, nlp_in and default parameters + // 6) setup functions, nlp_in and default parameters pendulum_ode_acados_create_setup_functions(capsule); pendulum_ode_acados_setup_nlp_in(capsule, N, new_time_steps); pendulum_ode_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - pendulum_ode_acados_set_nlp_out(capsule); - - // 8) do precomputations + // 9) do precomputations int status = pendulum_ode_acados_create_precompute(capsule); return status; @@ -749,15 +749,15 @@ int pendulum_ode_acados_reset(pendulum_ode_solver_capsule* capsule, int reset_qp for(int i=0; iconstraints[stage]; - return constr_config->model_set(constr_config, dims->constraints[stage], + // this updates both the bounds and the mask + int status = constr_config->model_set(constr_config, dims->constraints[stage], in->constraints[stage], field, value); + // multiply lam with new mask to ensure that multipliers associated with masked constraints are zero. + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); + + return status; } @@ -625,7 +633,7 @@ void ocp_nlp_out_destroy(void *out_) -void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in, int stage, const char *field, void *value) { double *double_values = value; @@ -654,6 +662,8 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou else if (!strcmp(field, "lam")) { blasfeo_pack_dvec(2*dims->ni[stage], double_values, 1, &out->lam[stage], 0); + // multiply with mask to ensure that multiplier associated with masked constraints are zero + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); } else if (!strcmp(field, "z")) { @@ -1830,6 +1840,8 @@ void ocp_nlp_set_all(ocp_nlp_solver *solver, ocp_nlp_in *in, ocp_nlp_out *out, c { tmp_int = 2*dims->ni[stage]; blasfeo_pack_dvec(tmp_int, double_values + tmp_offset, 1, &out->lam[stage], 0); + // multiply with mask to ensure that multiplier associated with masked constraints are zero + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); tmp_offset += tmp_int; } } diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index fc3cb0cb7c..361c6a1ff1 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -265,7 +265,7 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_get(ocp_nlp_config *config, ocp_nlp_ /// \param field The name of the field, either lb, ub (others TBC) /// \param value Constraints function or values. ACADOS_SYMBOL_EXPORT int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in, int stage, const char *field, void *value); + ocp_nlp_in *in, ocp_nlp_out *out, int stage, const char *field, void *value); /// ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims, @@ -293,7 +293,7 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_out_destroy(void *out); /// \param stage Stage number. /// \param field The name of the field, either x, u, pi. /// \param value Initialization values. -ACADOS_SYMBOL_EXPORT void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +ACADOS_SYMBOL_EXPORT void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in, int stage, const char *field, void *value); diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index baa434f0c5..903b927c4d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -350,13 +350,12 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__acados_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] self.__acados_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p] - - self.__acados_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.__acados_lib.ocp_nlp_constraints_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_constraints_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] # TODO check again self.__acados_lib.ocp_nlp_cost_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_cost_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.__acados_lib.ocp_nlp_out_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_out_set.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_set.argtypes = [c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] @@ -1827,20 +1826,18 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): if field_ in constraints_fields: self.__acados_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value_data_p) elif field_ in cost_fields: self.__acados_lib.ocp_nlp_cost_model_set(self.nlp_config, \ self.nlp_dims, self.nlp_in, stage, field, value_data_p) elif field_ in out_fields: self.__acados_lib.ocp_nlp_out_set(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage, field, value_data_p) + self.nlp_dims, self.nlp_out, self.nlp_in, stage, field, value_data_p) elif field_ in mem_fields: self.__acados_lib.ocp_nlp_set(self.nlp_solver, stage, field, value_data_p) elif field_ in sens_fields: - self.__acados_lib.ocp_nlp_out_set.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_out_set(self.nlp_config, \ - self.nlp_dims, self.sens_out, stage, field, value_data_p) + self.nlp_dims, self.sens_out, self.nlp_in, stage, field, value_data_p) # also set z_guess, when setting z. if field_ == 'z': field = 'z_guess'.encode('utf-8') @@ -2057,7 +2054,7 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war value_data_p = cast((value_data), c_void_p) self.__acados_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value_data_p) return diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index 16ada19927..7c159768a5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -711,13 +711,13 @@ cdef class AcadosOcpSolverCython: if field_ in constraints_fields: acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, - self.nlp_dims, self.nlp_in, stage, field, value.data) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value.data) elif field_ in cost_fields: acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value.data) elif field_ in out_fields: acados_solver_common.ocp_nlp_out_set(self.nlp_config, - self.nlp_dims, self.nlp_out, stage, field, value.data) + self.nlp_dims, self.nlp_out, self.nlp_in, stage, field, value.data) elif field_ in mem_fields: acados_solver_common.ocp_nlp_set(self.nlp_solver, stage, field, value.data) @@ -794,7 +794,7 @@ cdef class AcadosOcpSolverCython: f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, &value[0][0]) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, &value[0][0]) return diff --git a/interfaces/acados_template/acados_template/acados_solver_common.pxd b/interfaces/acados_template/acados_template/acados_solver_common.pxd index 200071d391..f40d6b6fef 100644 --- a/interfaces/acados_template/acados_template/acados_solver_common.pxd +++ b/interfaces/acados_template/acados_template/acados_solver_common.pxd @@ -69,10 +69,10 @@ cdef extern from "acados_c/ocp_nlp_interface.h": int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_, int start_stage, const char *field, void *value) int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in_, int stage, const char *field, void *value) + ocp_nlp_in *in_, ocp_nlp_out *out_, int stage, const char *field, void *value) # out - void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in_, int stage, const char *field, void *value) void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, void *value) 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 2db025a1fc..cf44923d66 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 @@ -871,6 +871,10 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i * nlp_in ************************************************/ ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps @@ -1069,9 +1073,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); {%- endif %} @@ -1082,7 +1086,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- for i in range(end=dims_0.nbxe_0) %} idxbxe_0[{{ i }}] = {{ constraints_0.idxbxe_0[i] }}; {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); {%- endif %} @@ -1110,6 +1114,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_0_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", lh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", uh_0); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_0_jac_p_hess_xu_p); @@ -1118,8 +1124,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_adj_p", &capsule->nl_constr_h_0_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", lh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", uh_0); free(luh_0); {%- elif dims_0.nphi_0 > 0 and constraints_0.constr_type_0 == "BGP" %} // set up convex-over-nonlinear constraints for first stage @@ -1135,8 +1139,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lphi", lphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uphi", uphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lphi", lphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uphi", uphi_0); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_phi_o_r_fun", &capsule->phi_0_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, @@ -1162,9 +1166,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsh", idxsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsh", lsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ush", ush_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsh", idxsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsh", lsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ush", ush_0); free(idxsh_0); free(lush_0); {%- endif %} @@ -1187,9 +1191,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsphi", idxsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsphi", lsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usphi", usphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsphi", idxsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsphi", lsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usphi", usphi_0); free(idxsphi_0); free(lusphi_0); {%- endif %} @@ -1498,9 +1502,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i /* constraints that are the same for initial and intermediate */ {%- if phases_dims[jj].nsbx > 0 %} {# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); // soft bounds on x idxsbx = malloc({{ phases_dims[jj].nsbx }} * sizeof(int)); @@ -1522,9 +1526,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); } free(idxsbx); free(lusbx); @@ -1551,9 +1555,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -1578,9 +1582,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endfor %} for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbu", usbu); } free(idxsbu); free(lusbu); @@ -1606,9 +1610,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usg", usg); } free(idxsg); free(lusg); @@ -1634,9 +1638,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); } free(idxsh); free(lush); @@ -1662,9 +1666,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); } free(idxsphi); free(lusphi); @@ -1690,9 +1694,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); } free(idxbx); free(lubx); @@ -1736,10 +1740,10 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); } free(D); free(C); @@ -1775,6 +1779,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess_{{ jj }}[i_fun]); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uh", uh); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_jac_p_hess_xu_p_{{ jj }}[i_fun]); @@ -1783,8 +1789,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_adj_p", &capsule->nl_constr_h_adj_p_{{ jj }}[i_fun]); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); } free(luh); {%- endif %} @@ -1812,8 +1816,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun", &capsule->phi_constraint_fun[i_fun]); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint_fun_jac_hess[i_fun]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uphi", uphi); } free(luphi); {%- endif %} @@ -1947,9 +1951,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ubx_e[{{ i }}] = {{ constraints_e.ubx_e[i] }}; {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", ubx_e); free(idxbx_e); free(lubx_e); {%- endif %} @@ -1972,9 +1976,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); free(idxsg_e); free(lusg_e); {%- endif %} @@ -1997,9 +2001,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); free(idxsh_e); free(lush_e); {%- endif %} @@ -2022,9 +2026,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); free(idxsphi_e); free(lusphi_e); {%- endif %} @@ -2047,9 +2051,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); free(idxsbx_e); free(lusbx_e); {% endif %} @@ -2078,9 +2082,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); free(C_e); free(lug_e); {%- endif %} @@ -2108,6 +2112,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_e_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_e_jac_p_hess_xu_p); @@ -2116,8 +2122,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", &capsule->nl_constr_h_e_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); free(luh_e); {%- elif dims_e.nphi_e > 0 and constraints_e.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -2133,8 +2137,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint_fun_jac_hess); @@ -2521,6 +2525,8 @@ void {{ name }}_acados_create_set_nlp_out({{ name }}_solver_capsule* capsule) ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + int nx_max = {{ nx_max }}; int nu_max = {{ nu_max }}; @@ -2544,11 +2550,11 @@ void {{ name }}_acados_create_set_nlp_out({{ name }}_solver_capsule* capsule) for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -2591,23 +2597,24 @@ int {{ name }}_acados_create_with_discretization({{ name }}_solver_capsule* caps capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); {{ name }}_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + {{ name }}_acados_create_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) set default parameters in functions + // 6) set default parameters in functions {{ name }}_acados_create_setup_functions(capsule); {{ name }}_acados_create_setup_nlp_in(capsule, N); {{ name }}_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - {{ name }}_acados_create_set_nlp_out(capsule); // 8) do precomputations int status = {{ name }}_acados_create_precompute(capsule); @@ -2669,15 +2676,15 @@ int {{ name }}_acados_reset({{ name }}_solver_capsule* capsule, int reset_qp_sol // Reset stage {{ jj }} for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "sl", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "su", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "lam", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "z", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "sl", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "su", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "lam", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "z", buffer); if (inlp_in = nlp_in; ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps and cost_scaling {%- set all_equal = true -%} @@ -1487,9 +1489,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); {%- endif %} @@ -1500,7 +1502,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- for i in range(end=dims.nbxe_0) %} idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }}; {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); {%- endif %} @@ -1528,6 +1530,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_0_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", lh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", uh_0); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_0_jac_p_hess_xu_p); @@ -1536,8 +1540,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_adj_p", &capsule->nl_constr_h_0_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", lh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", uh_0); free(luh_0); {%- elif dims.nphi_0 > 0 and constraints.constr_type_0 == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -1553,8 +1555,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lphi", lphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uphi", uphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lphi", lphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uphi", uphi_0); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_phi_o_r_fun", &capsule->phi_0_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, @@ -1580,9 +1582,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsh", idxsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsh", lsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ush", ush_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsh", idxsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsh", lsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ush", ush_0); free(idxsh_0); free(lush_0); {%- endif %} @@ -1605,9 +1607,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsphi", idxsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsphi", lsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usphi", usphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsphi", idxsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsphi", lsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usphi", usphi_0); free(idxsphi_0); free(lusphi_0); {%- endif %} @@ -1615,9 +1617,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu /* constraints that are the same for initial and intermediate */ {%- if dims.nsbx > 0 %} {# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); // soft bounds on x int* idxsbx = malloc(NSBX * sizeof(int)); @@ -1639,9 +1641,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); } free(idxsbx); free(lusbx); @@ -1668,9 +1670,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -1695,9 +1697,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endfor %} for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbu", usbu); } free(idxsbu); free(lusbu); @@ -1723,9 +1725,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usg", usg); } free(idxsg); free(lusg); @@ -1751,9 +1753,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); } free(idxsh); free(lush); @@ -1779,9 +1781,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); } free(idxsphi); free(lusphi); @@ -1807,9 +1809,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); } free(idxbx); free(lubx); @@ -1853,10 +1855,10 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); } free(D); free(C); @@ -1891,6 +1893,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i-1]); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uh", uh); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_jac_p_hess_xu_p[i-1]); @@ -1899,8 +1903,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_adj_p", &capsule->nl_constr_h_adj_p[i-1]); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); } free(luh); {%- endif %} @@ -1928,8 +1930,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu "nl_constr_phi_o_r_fun", &capsule->phi_constraint_fun[i-1]); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint_fun_jac_hess[i-1]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uphi", uphi); } free(luphi); {%- endif %} @@ -1953,9 +1955,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }}; {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", ubx_e); free(idxbx_e); free(lubx_e); {%- endif %} @@ -1978,9 +1980,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); free(idxsg_e); free(lusg_e); {%- endif %} @@ -2003,9 +2005,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); free(idxsh_e); free(lush_e); {%- endif %} @@ -2028,9 +2030,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); free(idxsphi_e); free(lusphi_e); {%- endif %} @@ -2053,9 +2055,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); free(idxsbx_e); free(lusbx_e); {% endif %} @@ -2084,9 +2086,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); free(C_e); free(lug_e); {%- endif %} @@ -2114,6 +2116,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_e_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_e_jac_p_hess_xu_p); @@ -2122,8 +2126,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", &capsule->nl_constr_h_e_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); free(luh_e); {%- elif dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -2139,8 +2141,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, @@ -2562,6 +2564,7 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); @@ -2582,11 +2585,11 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -2632,23 +2635,24 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); {{ model.name }}_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + {{ model.name }}_acados_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) setup functions, nlp_in and default parameters + // 6) setup functions, nlp_in and default parameters {{ model.name }}_acados_create_setup_functions(capsule); {{ model.name }}_acados_setup_nlp_in(capsule, N, new_time_steps); {{ model.name }}_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - {{ model.name }}_acados_set_nlp_out(capsule); // 8) do precomputations int status = {{ model.name }}_acados_create_precompute(capsule); @@ -2707,15 +2711,15 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int for(int i=0; i 0 allows this to work // also for lbu/ubu. for which dimension at terminal stage is 0 MEX_DIM_CHECK_VEC_STAGE(fun_name, field, ii, matlab_size, acados_size) - ocp_nlp_constraints_model_set(config, dims, in, ii, field_name, value); + ocp_nlp_constraints_model_set(config, dims, in, out, ii, field_name, value); } } } @@ -189,7 +189,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) offset = 0; for (ii=0; ii<=N; ii++) // TODO implement set_all { - ocp_nlp_constraints_model_set(config, dims, in, ii, field_name, value+offset); + ocp_nlp_constraints_model_set(config, dims, in, out, ii, field_name, value+offset); tmp_int = ocp_nlp_dims_get_from_attr(config, dims, out, ii, field_name); offset += tmp_int; } @@ -200,7 +200,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, field_name); MEX_DIM_CHECK_VEC_STAGE(fun_name, field, s0, matlab_size, acados_size) if (matlab_size != 0) - ocp_nlp_constraints_model_set(config, dims, in, s0, field_name, value); + ocp_nlp_constraints_model_set(config, dims, in, out, s0, field_name, value); } } // cost: @@ -379,7 +379,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "x"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "x", value); + ocp_nlp_out_set(config, dims, out, in, s0, "x", value); } } else if (!strcmp(field, "init_u") || !strcmp(field, "u")) @@ -394,7 +394,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "u"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "u", value); + ocp_nlp_out_set(config, dims, out, in, s0, "u", value); } } else if (!strcmp(field, "init_z")||!strcmp(field, "z")) @@ -499,7 +499,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "pi"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "pi", value); + ocp_nlp_out_set(config, dims, out, in, s0, "pi", value); } } else if (!strcmp(field, "init_lam")||!strcmp(field, "lam")) @@ -514,7 +514,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "lam", value); + ocp_nlp_out_set(config, dims, out, in, s0, "lam", value); } } else if (!strcmp(field, "init_sl")||!strcmp(field, "sl")) @@ -529,7 +529,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, field, value); + ocp_nlp_out_set(config, dims, out, in, s0, field, value); } } else if (!strcmp(field, "init_su")||!strcmp(field, "su")) @@ -544,7 +544,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, field, value); + ocp_nlp_out_set(config, dims, out, in, s0, field, value); } } else if (!strcmp(field, "p")) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 579399b31c..1381bd4d48 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -799,7 +799,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_0.nbx_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", buffer); {%- endif %} {%- if dims_0.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} @@ -808,7 +808,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nbx_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", buffer); {%- endif %} {%- if np_total > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} @@ -889,7 +889,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lbx"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbx", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lbx", (void *) buffer); tmp_offset += tmp_int; } {%- endif %} @@ -903,7 +903,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "ubx"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubx", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ubx", (void *) buffer); tmp_offset += tmp_int; } {%- endif %} @@ -916,7 +916,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_e.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", buffer); {%- endif %} {%- if dims_e.nbx_e > 0 and solver_options.N_horizon > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} // ubx_e @@ -925,7 +925,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_e.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", buffer); {%- endif %} @@ -939,7 +939,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lbu"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbu", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lbu", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -953,7 +953,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "ubu"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubu", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ubu", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -968,7 +968,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) { for (int jj = 0; jj < {{ dims.ng }}; jj++) buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lg", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lg", (void *) buffer); } {%- endif -%} {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} @@ -980,7 +980,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) { for (int jj = 0; jj < {{ dims.ng }}; jj++) buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ug", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ug", (void *) buffer); } {%- endif -%} {%- endif -%} @@ -995,7 +995,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lh"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lh", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lh", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -1009,7 +1009,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "uh"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "uh", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "uh", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -1020,7 +1020,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nh_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", buffer); {%- endif -%} {%- if dims_0.nh_0 > 0 and simulink_opts.inputs.uh_0 -%} {#- uh_0 #} // uh_0 @@ -1028,7 +1028,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nh_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", buffer); {%- endif -%} {%- if dims_e.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} @@ -1037,7 +1037,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_e.nh_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", buffer); {%- endif -%} {%- if dims_e.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} // uh_e @@ -1045,7 +1045,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_e.nh_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", buffer); {%- endif -%} {% if problem_class == "OCP" %} diff --git a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c index d6bdf71647..1fc3bf3cdc 100644 --- a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c +++ b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c @@ -670,7 +670,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbu", custom_mem->d_lbu_tightened); {%- endif %} {%- if zoro_description.nubu_t > 0 %} // backoff ubu @@ -680,7 +680,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubu", custom_mem->d_ubu_tightened); {%- endif %} {%- endif %} // Middle Stages @@ -714,7 +714,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lbx", custom_mem->d_lbx_tightened); {%- endif %} {% if zoro_description.nubx_t > 0 %} // ubx @@ -723,7 +723,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ubx", custom_mem->d_ubx_tightened); {%- endif %} {%- endif %} @@ -739,7 +739,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lbu", custom_mem->d_lbu_tightened); {%- endif %} {%- if zoro_description.nubu_t > 0 %} {%- for it in zoro_description.idx_ubu_t %} @@ -747,7 +747,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ubu", custom_mem->d_ubu_tightened); {%- endif %} {%- endif %} @@ -764,7 +764,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lg[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lg", custom_mem->d_lg_tightened); {%- endif %} {%- if zoro_description.nug_t > 0 %} {%- for it in zoro_description.idx_ug_t %} @@ -772,7 +772,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_ug[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ug", custom_mem->d_ug_tightened); {%- endif %} {%- endif %} @@ -808,14 +808,14 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lh[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lh", custom_mem->d_lh_tightened); {%- endif %} {%- if zoro_description.nuh_t > 0 %} {%- for it in zoro_description.idx_uh_t %} custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "uh", custom_mem->d_uh_tightened); {%- endif %} {%- endif %} } @@ -848,7 +848,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", custom_mem->d_lbx_e_tightened); {%- endif %} {% if zoro_description.nubx_e_t > 0 %} // ubx_e @@ -857,7 +857,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", custom_mem->d_ubx_e_tightened); {%- endif %} {%- endif %} @@ -874,7 +874,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lg_e[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", custom_mem->d_lg_e_tightened); {%- endif %} {%- if zoro_description.nug_e_t > 0 %} {%- for it in zoro_description.idx_ug_e_t %} @@ -882,7 +882,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_ug_e[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", custom_mem->d_ug_e_tightened); {%- endif %} {%- endif %} @@ -905,14 +905,14 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lh_e[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", custom_mem->d_lh_e_tightened); {%- endif %} {%- if zoro_description.nuh_e_t > 0 %} {%- for it in zoro_description.idx_uh_e_t %} custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", custom_mem->d_uh_e_tightened); {%- endif %} {%- endif %} diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index a93a7d939b..a36be7692c 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -737,9 +737,13 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q } /************************************************ - * nlp_in + * ocp_nlp out ************************************************/ + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + /************************************************ + * ocp_nlp_in + ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); // sampling times @@ -855,26 +859,26 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q /* box constraints */ // fist stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbu", idxbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ubu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbu", idxbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ubu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ubx0); // middle stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx1); } // last stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxbx", idxbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxbx", idxbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubxN); /* nonlinear constraints */ @@ -884,9 +888,9 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q { if (nh[i] > 0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -897,9 +901,9 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q { if (ns[i] > 0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } } @@ -975,11 +979,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q config->opts_update(config, dims, nlp_opts); /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -1006,8 +1007,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); double total_power = 0.0; // sum up to get total objective double electrical_power = 0.0; @@ -1068,8 +1069,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q // update initial condition // TODO(dimitris): maybe simulate system instead of passing x[1] as next state ocp_nlp_out_get(config, dims, nlp_out, 1, "x", specific_x); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", specific_x); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", specific_x); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", specific_x); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", specific_x); int sqp_iter; double time_lin, time_qp_sol, time_tot; From 0d9b6aa6e5021c35c061ae3b1944600633406344 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 10:05:22 +0200 Subject: [PATCH 022/164] Globalization: fix iteration number in merit backtracking (#1492) --- acados/ocp_nlp/ocp_nlp_globalization_common.h | 9 +- ...ocp_nlp_globalization_merit_backtracking.c | 333 +++++++++--------- ...ocp_nlp_globalization_merit_backtracking.h | 9 +- .../non_ocp_nlp/maratos_test_problem.py | 14 +- 4 files changed, 174 insertions(+), 191 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_common.h b/acados/ocp_nlp/ocp_nlp_globalization_common.h index d8c6cead55..02cc9071e8 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_common.h +++ b/acados/ocp_nlp/ocp_nlp_globalization_common.h @@ -64,15 +64,10 @@ typedef struct /* functions */ int (*find_acceptable_iterate)(void *nlp_config, void *nlp_dims, void *nlp_in, void *nlp_out, void *nlp_mem, void *solver_mem, void *nlp_work, void *nlp_opts, double *step_size); void (*print_iteration_header)(); - void (*print_iteration)(double objective_value, - void *globalization_opts, - void* globalization_mem); + void (*print_iteration)(double objective_value, void *globalization_opts, void* globalization_mem); int (*needs_objective_value)(); int (*needs_qp_objective_value)(); - void (*initialize_memory)(void *config_, - void *dims_, - void *nlp_mem_, - void *nlp_opts_); + void (*initialize_memory)(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_); } ocp_nlp_globalization_config; // diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 1b7e3faa42..97c60b066d 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -143,9 +143,151 @@ void *ocp_nlp_globalization_merit_backtracking_memory_assign(void *config_, void * functions ************************************************/ -int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, +static double ocp_nlp_compute_merit_gradient(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) +{ + /* computes merit function gradient at iterate: out -- using already evaluated gradients of submodules + with weights: work->weight_merit_fun */ + int i, j; + + int N = dims->N; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + int *ni = dims->ni; + + double merit_grad = 0.0; + double weight; + + // NOTE: step is in: mem->qp_out->ux + struct blasfeo_dvec *tmp_vec; // size nv + struct blasfeo_dvec tmp_vec_nxu = work->tmp_nv; // size nxu + struct blasfeo_dvec dxnext_dy = work->dxnext_dy; // size nx + + // cost + for (i=0; i<=N; i++) + { + tmp_vec = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); + merit_grad += blasfeo_ddot(nv[i], tmp_vec, 0, mem->qp_out->ux + i, 0); + } + double merit_grad_cost = merit_grad; + + /* dynamics */ + double merit_grad_dyn = 0.0; + for (i=0; idynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); + + /* compute directional derivative of xnext with direction y -> dxnext_dy */ + blasfeo_dgemv_t(nx[i]+nu[i], nx[i+1], 1.0, mem->qp_in->BAbt+i, 0, 0, mem->qp_out->ux+i, 0, + 0.0, &dxnext_dy, 0, &dxnext_dy, 0); + + /* add merit gradient contributions depending on sign of shooting gap */ + for (j = 0; j < nx[i+1]; j++) + { + weight = BLASFEO_DVECEL(work->weight_merit_fun->pi+i, j); + double deqj_dy = BLASFEO_DVECEL(&dxnext_dy, j) - BLASFEO_DVECEL(mem->qp_out->ux+(i+1), nu[i+1]+j); + { + if (BLASFEO_DVECEL(tmp_vec, j) > 0) + { + merit_grad_dyn += weight * deqj_dy; + // printf("\ndyn_contribution +%e, weight %e, deqj_dy %e, i %d, j %d", weight * deqj_dy, weight, deqj_dy, i, j); + } + else + { + merit_grad_dyn -= weight * deqj_dy; + // printf("\ndyn_contribution %e, weight %e, deqj_dy %e, i %d, j %d", -weight * deqj_dy, weight, deqj_dy, i, j); + } + } + } + } + + /* inequality contributions */ + // NOTE: slack bound inequalities are not considered here. + // They should never be infeasible. Only if explicitly initialized infeasible from outside. + int constr_index, slack_index_in_ux, slack_index; + ocp_qp_dims* qp_dims = mem->qp_in->dim; + int *nb = qp_dims->nb; + int *ng = qp_dims->ng; + int *ns = qp_dims->ns; + double merit_grad_ineq = 0.0; + double slack_step; + + for (i=0; i<=N; i++) + { + tmp_vec = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); + int *idxb = mem->qp_in->idxb[i]; + if (ni[i] > 0) + { + // NOTE: loop could be simplified handling lower and upper constraints together. + for (j = 0; j < 2 * (nb[i] + ng[i]); j++) // 2 * ni + { + double constraint_val = BLASFEO_DVECEL(tmp_vec, j); + if (constraint_val > 0) + { + weight = BLASFEO_DVECEL(work->weight_merit_fun->lam+i, j); + + // find corresponding slack value + constr_index = j < nb[i]+ng[i] ? j : j-(nb[i]+ng[i]); + slack_index = mem->qp_in->idxs_rev[i][constr_index]; + // if softened: add slack contribution + if (slack_index >= 0) + { + slack_index_in_ux = j < (nb[i]+ng[i]) ? nx[i] + nu[i] + slack_index + : nx[i] + nu[i] + slack_index + ns[i]; + slack_step = BLASFEO_DVECEL(mem->qp_out->ux+i, slack_index_in_ux); + merit_grad_ineq -= weight * slack_step; + // printf("at node %d, ineq %d, idxs_rev[%d] = %d\n", i, j, constr_index, slack_index); + // printf("slack contribution: uxs[%d] = %e\n", slack_index_in_ux, slack_step); + } + + + // NOTE: the inequalities are internally organized in the following order: + // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; + // lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + // printf("constraint %d %d is active with value %e", i, j, constraint_val); + if (j < nb[i]) + { + // printf("lower idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j, idxb[j], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j]), constraint_val, nb[i]); + merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j]); + } + else if (j < nb[i] + ng[i]) + { + // merit_grad_ineq += weight * mem->qp_in->DCt_j * dux + blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - nb[i], 0, &tmp_vec_nxu, 0); + merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); + // printf("general linear constraint lower contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); + } + else if (j < 2*nb[i] + ng[i]) + { + // printf("upper idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j-nb[i]-ng[i], idxb[j-nb[i]-ng[i]], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j-nb[i]-ng[i]]), constraint_val, nb[i]); + merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j-nb[i]-ng[i]]); + } + else if (j < 2*nb[i] + 2*ng[i]) + { + blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - 2*nb[i] - ng[i], 0, &tmp_vec_nxu, 0); + merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); + // printf("general linear constraint upper contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); + } + } + } + } + } + // print_ocp_qp_dims(qp_dims); + // print_ocp_qp_in(mem->qp_in); + + merit_grad = merit_grad_cost + merit_grad_dyn + merit_grad_ineq; + if (opts->print_level > 1) + printf("computed 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; +} + +static int ocp_nlp_line_search(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 sqp_iter, double *alpha_reference) + double *alpha_reference) { ocp_nlp_globalization_merit_backtracking_opts *merit_opts = opts->globalization; ocp_nlp_globalization_opts *globalization_opts = merit_opts->globalization_opts; @@ -174,7 +316,7 @@ int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in * // } /* modify/initialize merit function weights (Leineweber1999 M5.1, p.89) */ - if (sqp_iter==0) + if (mem->iter==0) { merit_backtracking_initialize_weights(dims, work->weight_merit_fun, qp_out); } @@ -184,9 +326,6 @@ int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in * } // TODO: why does Leineweber do full step in first SQP iter? - // if (sqp_iter == 0) - // { - // } double merit_fun0 = ocp_nlp_evaluate_merit_fun(config, dims, in, out, opts, mem, work); @@ -350,7 +489,7 @@ static void copy_multipliers_qp_to_nlp(ocp_nlp_dims *dims, ocp_qp_out *from, ocp } static int ocp_nlp_line_search_merit_check_full_step(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 sqp_iter) + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work) { int N = dims->N; int *nv = dims->nv; @@ -364,9 +503,8 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp blasfeo_dveccp(nv[i], out->ux+i, 0, work->tmp_nlp_out->ux+i, 0); // NOTE: copying duals not needed, as they dont enter the merit function, see ocp_nlp_line_search - /* modify/initialize merit function weights (Leineweber1999 M5.1, p.89) */ - if (sqp_iter==0) + if (mem->iter==0) { merit_backtracking_initialize_weights(dims, work->weight_merit_fun, qp_out); } @@ -398,7 +536,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp if (isnan(merit_fun1) || isinf(merit_fun1)) { // do nothing and continue with normal line search, i.e. step reduction - if (sqp_iter != 0) + if (mem->iter != 0) { // reset merit function weights; copy_multipliers_qp_to_nlp(dims, work->tmp_qp_out, work->weight_merit_fun); @@ -414,7 +552,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp else { // trigger SOC - if (sqp_iter != 0) + if (mem->iter != 0) { // reset merit function weights; copy_multipliers_qp_to_nlp(dims, work->tmp_qp_out, work->weight_merit_fun); @@ -424,7 +562,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp } static bool ocp_nlp_soc_line_search(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, int sqp_iter) + ocp_nlp_out *nlp_out, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work) { int ii; int N = dims->N; @@ -441,7 +579,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // NOTE: the "and" is interpreted as an "or" in the current implementation // preliminary line search - int line_search_status = ocp_nlp_line_search_merit_check_full_step(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter); + int line_search_status = ocp_nlp_line_search_merit_check_full_step(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); // return bool do_line_search; if (line_search_status == ACADOS_NAN_DETECTED) @@ -539,12 +677,12 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, if (nlp_opts->print_level > 3) { - printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", nlp_mem->iter); // print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 1); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 1); #endif // solve QP @@ -562,29 +700,29 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // save statistics of last qp solver call // TODO: SOC QP solver call should be warm / hot started! - // if (sqp_iter+1 < nlp_mem->stat_m) + // if (nlp_mem->iter+1 < nlp_mem->stat_m) // { - // // mem->stat[mem->stat_n*(sqp_iter+1)+4] = qp_status; + // // mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; // // add qp_iter; should maybe be in a seperate statistic - // nlp_mem->stat[nlp_mem->stat_n*(sqp_iter+1)+5] += qp_iter; + // nlp_mem->stat[nlp_mem->stat_n*(nlp_mem->iter+1)+5] += qp_iter; // } // compute external QP residuals (for debugging) // if (nlp_opts->ext_qp_res) // { // ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); - // if (sqp_iter+1 < nlp_mem->stat_m) - // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(sqp_iter+1)+7)); + // if (nlp_mem->iter+1 < nlp_mem->stat_m) + // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(nlp_mem->iter+1)+7)); // } // if (nlp_opts->print_level > 3) // { - // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", sqp_iter); + // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", nlp_mem->iter); // print_ocp_qp_out(qp_out); // } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 1); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 1); #endif // exit conditions on QP status @@ -592,7 +730,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, { #ifndef ACADOS_SILENT printf("\nQP solver returned error status %d in SQP iteration %d for SOC QP.\n", - qp_status, sqp_iter); + qp_status, nlp_mem->iter); #endif // if (nlp_opts->print_level > 1) // { @@ -600,157 +738,13 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // if (nlp_opts->print_level > 3) // print_ocp_qp_in(qp_in); // } - nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; return true; } return true; } -double ocp_nlp_compute_merit_gradient(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) -{ - /* computes merit function gradient at iterate: out -- using already evaluated gradients of submodules - with weights: work->weight_merit_fun */ - int i, j; - - int N = dims->N; - int *nv = dims->nv; - int *nx = dims->nx; - int *nu = dims->nu; - int *ni = dims->ni; - - double merit_grad = 0.0; - double weight; - - // NOTE: step is in: mem->qp_out->ux - struct blasfeo_dvec *tmp_vec; // size nv - struct blasfeo_dvec tmp_vec_nxu = work->tmp_nv; // size nxu - struct blasfeo_dvec dxnext_dy = work->dxnext_dy; // size nx - - // cost - for (i=0; i<=N; i++) - { - tmp_vec = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); - merit_grad += blasfeo_ddot(nv[i], tmp_vec, 0, mem->qp_out->ux + i, 0); - } - double merit_grad_cost = merit_grad; - - /* dynamics */ - double merit_grad_dyn = 0.0; - for (i=0; idynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); - - /* compute directional derivative of xnext with direction y -> dxnext_dy */ - blasfeo_dgemv_t(nx[i]+nu[i], nx[i+1], 1.0, mem->qp_in->BAbt+i, 0, 0, mem->qp_out->ux+i, 0, - 0.0, &dxnext_dy, 0, &dxnext_dy, 0); - - /* add merit gradient contributions depending on sign of shooting gap */ - for (j = 0; j < nx[i+1]; j++) - { - weight = BLASFEO_DVECEL(work->weight_merit_fun->pi+i, j); - double deqj_dy = BLASFEO_DVECEL(&dxnext_dy, j) - BLASFEO_DVECEL(mem->qp_out->ux+(i+1), nu[i+1]+j); - { - if (BLASFEO_DVECEL(tmp_vec, j) > 0) - { - merit_grad_dyn += weight * deqj_dy; - // printf("\ndyn_contribution +%e, weight %e, deqj_dy %e, i %d, j %d", weight * deqj_dy, weight, deqj_dy, i, j); - } - else - { - merit_grad_dyn -= weight * deqj_dy; - // printf("\ndyn_contribution %e, weight %e, deqj_dy %e, i %d, j %d", -weight * deqj_dy, weight, deqj_dy, i, j); - } - } - } - } - - /* inequality contributions */ - // NOTE: slack bound inequalities are not considered here. - // They should never be infeasible. Only if explicitly initialized infeasible from outside. - int constr_index, slack_index_in_ux, slack_index; - ocp_qp_dims* qp_dims = mem->qp_in->dim; - int *nb = qp_dims->nb; - int *ng = qp_dims->ng; - int *ns = qp_dims->ns; - double merit_grad_ineq = 0.0; - double slack_step; - - for (i=0; i<=N; i++) - { - tmp_vec = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); - int *idxb = mem->qp_in->idxb[i]; - if (ni[i] > 0) - { - // NOTE: loop could be simplified handling lower and upper constraints together. - for (j = 0; j < 2 * (nb[i] + ng[i]); j++) // 2 * ni - { - double constraint_val = BLASFEO_DVECEL(tmp_vec, j); - if (constraint_val > 0) - { - weight = BLASFEO_DVECEL(work->weight_merit_fun->lam+i, j); - - // find corresponding slack value - constr_index = j < nb[i]+ng[i] ? j : j-(nb[i]+ng[i]); - slack_index = mem->qp_in->idxs_rev[i][constr_index]; - // if softened: add slack contribution - if (slack_index >= 0) - { - slack_index_in_ux = j < (nb[i]+ng[i]) ? nx[i] + nu[i] + slack_index - : nx[i] + nu[i] + slack_index + ns[i]; - slack_step = BLASFEO_DVECEL(mem->qp_out->ux+i, slack_index_in_ux); - merit_grad_ineq -= weight * slack_step; - // printf("at node %d, ineq %d, idxs_rev[%d] = %d\n", i, j, constr_index, slack_index); - // printf("slack contribution: uxs[%d] = %e\n", slack_index_in_ux, slack_step); - } - - - // NOTE: the inequalities are internally organized in the following order: - // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; - // lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] - // printf("constraint %d %d is active with value %e", i, j, constraint_val); - if (j < nb[i]) - { - // printf("lower idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j, idxb[j], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j]), constraint_val, nb[i]); - merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j]); - } - else if (j < nb[i] + ng[i]) - { - // merit_grad_ineq += weight * mem->qp_in->DCt_j * dux - blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - nb[i], 0, &tmp_vec_nxu, 0); - merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); - // printf("general linear constraint lower contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); - } - else if (j < 2*nb[i] + ng[i]) - { - // printf("upper idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j-nb[i]-ng[i], idxb[j-nb[i]-ng[i]], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j-nb[i]-ng[i]]), constraint_val, nb[i]); - merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j-nb[i]-ng[i]]); - } - else if (j < 2*nb[i] + 2*ng[i]) - { - blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - 2*nb[i] - ng[i], 0, &tmp_vec_nxu, 0); - merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); - // printf("general linear constraint upper contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); - } - } - } - } - } - // print_ocp_qp_dims(qp_dims); - // print_ocp_qp_in(mem->qp_in); - - merit_grad = merit_grad_cost + merit_grad_dyn + merit_grad_ineq; - if (opts->print_level > 1) - printf("computed 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; -} - double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, @@ -1018,12 +1012,11 @@ int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_c ocp_nlp_globalization_merit_backtracking_opts *merit_opts = nlp_opts->globalization; ocp_nlp_globalization_opts *globalization_opts = merit_opts->globalization_opts; - int sqp_iter = 1; // NEEDS TO BE CHANGED HERE bool do_line_search = true; if (merit_opts->globalization_opts->use_SOC) { - do_line_search = ocp_nlp_soc_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter); + do_line_search = ocp_nlp_soc_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); if (nlp_mem->status == ACADOS_QP_FAILURE) { return nlp_mem->status; @@ -1033,7 +1026,7 @@ int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_c if (do_line_search) { int line_search_status; - line_search_status = ocp_nlp_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter, &mem->alpha); + line_search_status = ocp_nlp_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, &mem->alpha); if (line_search_status == ACADOS_NAN_DETECTED) { nlp_mem->status = ACADOS_NAN_DETECTED; diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h index 263d80d77c..3d6f368c80 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h @@ -89,14 +89,7 @@ void *ocp_nlp_globalization_merit_backtracking_memory_assign(void *config, void /************************************************ * functions ************************************************/ -// -double ocp_nlp_compute_merit_gradient(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_line_search(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 sqp_iter, double *alpha_ref); + // double ocp_nlp_evaluate_merit_fun(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); diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index f792505493..9be5054f7e 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -71,6 +71,8 @@ def solve_maratos_problem_with_setting(setting): line_search_use_sufficient_descent = setting['line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] + print(f"running maratos test problem with settings {setting}") + # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -139,10 +141,10 @@ def solve_maratos_problem_with_setting(setting): if FOR_LOOPING: # call solver in for loop to get all iterates ocp.solver_options.nlp_solver_max_iter = 1 - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, verbose=False) else: ocp.solver_options.nlp_solver_max_iter = SQP_max_iter - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, verbose=False) # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 @@ -210,8 +212,8 @@ def solve_maratos_problem_with_setting(setting): if any(alphas[:iter] != 1.0): raise Exception(f"Expected all alphas = 1.0 when using full step SQP on Maratos problem") elif globalization == 'MERIT_BACKTRACKING': - if max_infeasibility > 0.5: - raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") + if max_infeasibility > 1.5: + raise Exception(f"Expected max_infeasibility < 1.5 when using globalized SQP on Maratos problem") if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") @@ -224,8 +226,8 @@ def solve_maratos_problem_with_setting(setting): # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception(f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") else: - if iter != 16: - raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") + if iter != 10: + raise Exception(f"Expected 10 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") From e2fa504f811bf8018d486b0e2dc4304852f8fe9b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 14:54:34 +0200 Subject: [PATCH 023/164] C: cleanup `qp_res` option and memory (#1494) It was already in the common workspace. - move `qp_res` memory management to common fully - remove option ext_qp_res from nlp solvers, as it was moved to common in #1254 --- acados/ocp_nlp/ocp_nlp_common.c | 21 ++++++++++ acados/ocp_nlp/ocp_nlp_ddp.c | 35 ++--------------- acados/ocp_nlp/ocp_nlp_ddp.h | 1 - acados/ocp_nlp/ocp_nlp_sqp.c | 20 ---------- acados/ocp_nlp/ocp_nlp_sqp.h | 1 - acados/ocp_nlp/ocp_nlp_sqp_rti.c | 39 +++---------------- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 1 - acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 1 - 8 files changed, 31 insertions(+), 88 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 65d6636454..2635d0f709 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -2105,6 +2105,16 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d ext_fun_workspace_size += dynamics[i]->get_external_fun_workspace_requirement(dynamics[i], dims->dynamics[i], opts->dynamics[i], in->dynamics[i]); } } + + if (opts->ext_qp_res) + { + // qp_res + size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + + // qp_res_ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + size += 64; // ext_fun_workspace_size align size += ext_fun_workspace_size; @@ -2349,6 +2359,17 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } } + if (opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + assert((char *) work + mem->workspace_size >= c_ptr); return work; diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index 13143d2a22..ba8fcec33f 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -117,8 +117,6 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_ opts->tol_comp = 1e-8; opts->tol_zero_res = 1e-12; - opts->ext_qp_res = 0; - opts->warm_start_first_qp = false; opts->warm_start_first_qp_from_nlp = false; opts->eval_residual_at_max_iter = false; @@ -195,11 +193,6 @@ void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* v // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", value); } - else if (!strcmp(field, "ext_qp_res")) - { - int* ext_qp_res = (int *) value; - opts->ext_qp_res = *ext_qp_res; - } else if (!strcmp(field, "warm_start_first_qp")) { bool* warm_start_first_qp = (bool *) value; @@ -265,7 +258,7 @@ acados_size_t ocp_nlp_ddp_memory_calculate_size(void *config_, void *dims_, void // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 7; - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) stat_n += 4; size += stat_n*stat_m*sizeof(double); @@ -332,9 +325,9 @@ void *ocp_nlp_ddp_memory_assign(void *config_, void *dims_, void *opts_, void *i // stat mem->stat = (double *) c_ptr; - mem->stat_m = opts->nlp_opts->max_iter+1; + mem->stat_m = nlp_opts->max_iter+1; mem->stat_n = 7; - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -373,15 +366,6 @@ acados_size_t ocp_nlp_ddp_workspace_calculate_size(void *config_, void *dims_, v // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -401,17 +385,6 @@ static void ocp_nlp_ddp_cast_workspace(ocp_nlp_config *config, ocp_nlp_dims *dim work->nlp_work = ocp_nlp_workspace_assign(config, dims, nlp_opts, nlp_in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_ddp_workspace_calculate_size(config, dims, opts, nlp_in) >= c_ptr); return; @@ -820,7 +793,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // compute external QP residuals (for debugging) - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(qp_in, qp_out, work->qp_res, work->qp_res_ws); if (ddp_iter+1 < mem->stat_m) diff --git a/acados/ocp_nlp/ocp_nlp_ddp.h b/acados/ocp_nlp/ocp_nlp_ddp.h index a1b0a2cb8d..a8ddafa584 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.h +++ b/acados/ocp_nlp/ocp_nlp_ddp.h @@ -61,7 +61,6 @@ typedef struct double tol_ineq; // exit tolerance on inequality constraints 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) 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 index 2b16f5a216..a824c2fffc 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -379,15 +379,6 @@ acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config_, void *dims_, v // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (nlp_opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -407,17 +398,6 @@ static void ocp_nlp_sqp_cast_workspace(ocp_nlp_config *config, ocp_nlp_dims *dim work->nlp_work = ocp_nlp_workspace_assign(config, dims, nlp_opts, in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (nlp_opts->ext_qp_res) - { - // qp res - work->nlp_work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->nlp_work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_sqp_workspace_calculate_size(config, dims, opts, in) >= c_ptr); return; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index 58cf72dc57..07ca3d5564 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -62,7 +62,6 @@ typedef struct double tol_comp; // exit tolerance on complementarity condition double tol_unbounded; // exit threshold when objective function seems to be unbounded 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 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. diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 833a06938e..945d985593 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -113,7 +113,6 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP RTI opts - opts->ext_qp_res = 0; opts->warm_start_first_qp = false; opts->warm_start_first_qp_from_nlp = true; opts->rti_phase = 0; @@ -161,12 +160,7 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, } else // nlp opts { - if (!strcmp(field, "ext_qp_res")) - { - int* ext_qp_res = (int *) value; - opts->ext_qp_res = *ext_qp_res; - } - else if (!strcmp(field, "warm_start_first_qp")) + if (!strcmp(field, "warm_start_first_qp")) { bool* warm_start_first_qp = (bool *) value; opts->warm_start_first_qp = *warm_start_first_qp; @@ -256,7 +250,7 @@ acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, int stat_n = 2; // qp_status, qp_iter if (opts->rti_log_residuals) stat_n += 4; // nlp_res - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) stat_n += 4; // qp_res size += stat_n*stat_m*sizeof(double); @@ -296,7 +290,7 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, mem->stat_n = 2; // qp_status, qp_iter if (opts->rti_log_residuals) mem->stat_n += 4; // nlp_res - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) mem->stat_n += 4; // qp_res c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -336,15 +330,6 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -366,19 +351,6 @@ static void ocp_nlp_sqp_rti_cast_workspace( config, dims, nlp_opts, nlp_in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - work->nlp_work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->nlp_work->qp_res_ws = ocp_qp_res_workspace_assign( - dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size( - dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_sqp_rti_workspace_calculate_size(config, dims, opts, nlp_in) >= c_ptr); @@ -404,9 +376,10 @@ static void rti_store_residuals_in_stats(ocp_nlp_sqp_rti_opts *opts, ocp_nlp_sqp { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_nlp_res *nlp_res = nlp_mem->nlp_res; + ocp_nlp_opts *nlp_opts = opts->nlp_opts; if (nlp_mem->iter < mem->stat_m) { - int m_offset = 2 + 4 * opts->ext_qp_res; + int m_offset = 2 + 4 * nlp_opts->ext_qp_res; // printf("storing residuals AS RTI, m_offset %d\n", m_offset); // printf("%e\t%e\t%e\t%e\n", nlp_res->inf_norm_res_stat, nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq, nlp_res->inf_norm_res_comp); mem->stat[mem->stat_n * nlp_mem->iter+0+m_offset] = nlp_res->inf_norm_res_stat; @@ -626,7 +599,7 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * } // compute external QP residuals (for debugging) - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(nlp_mem->qp_in, nlp_mem->qp_out, work->qp_res, work->qp_res_ws); ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+2)); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 8f34708cf3..40ddaa0bdf 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -80,7 +80,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) 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_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index aab1ac0886..6a68558907 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1226,7 +1226,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, if (config->globalization->needs_objective_value() == 1) { - l1_inf_QP_feasibility = calculate_qp_l1_infeasibility(dims, mem, work, opts, relaxed_qp_in, relaxed_qp_out); mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); } From 20ac01be7fc552acb0b4c341c46ce930eeab1f83 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 15:23:55 +0200 Subject: [PATCH 024/164] Step norm logging (#1495) - move primal_step_norm to common memory, as option is also there - Interfaces: fix log_primal_step_norm for SQP_WFQP and multi-phase - add option ` log_dual_step_norm` - test getting step norms in MATLAB and Python Bit unrelated: - SQP_WFQP: remove max_iter duplicated from common --- acados/ocp_nlp/ocp_nlp_common.c | 88 +++++++++++++++++++ acados/ocp_nlp/ocp_nlp_common.h | 6 ++ acados/ocp_nlp/ocp_nlp_sqp.c | 47 +++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 42 ++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 3 - .../getting_started/extensive_example_ocp.m | 9 ++ .../ocp/example_ocp_dynamics_formulations.py | 15 +++- .../acados_matlab_octave/AcadosOcpOptions.m | 2 + interfaces/acados_matlab_octave/ocp_get.c | 8 ++ .../acados_template/acados_ocp_options.py | 31 +++++-- .../acados_template/acados_ocp_solver.py | 2 +- .../c_templates_tera/acados_multi_solver.in.c | 15 +++- .../c_templates_tera/acados_solver.in.c | 5 +- 13 files changed, 190 insertions(+), 83 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 2635d0f709..893a19a3bc 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1207,6 +1207,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->log_dual_step_norm = 0; opts->max_iter = 1; /* submodules opts */ @@ -1433,6 +1434,11 @@ 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, "log_dual_step_norm")) + { + int* log_dual_step_norm = (int *) value; + opts->log_dual_step_norm = *log_dual_step_norm; + } else if (!strcmp(field, "max_iter")) { int* max_iter = (int *) value; @@ -1631,6 +1637,16 @@ acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims size += sizeof(struct ocp_nlp_timings); size += (N+1)*sizeof(bool); // set_sim_guess + // primal step norm + if (opts->log_primal_step_norm) + { + size += opts->max_iter*sizeof(double); + } + // dual step norm + if (opts->log_dual_step_norm) + { + size += opts->max_iter*sizeof(double); + } size += (N+1)*sizeof(struct blasfeo_dmat); // dzduxt size += 6*(N+1)*sizeof(struct blasfeo_dvec); // cost_grad ineq_fun ineq_adj dyn_adj sim_guess z_alg @@ -1816,6 +1832,20 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims // sim_guess assign_and_advance_blasfeo_dvec_structs(N + 1, &mem->sim_guess, &c_ptr); + // primal step norm + if (opts->log_primal_step_norm) + { + mem->primal_step_norm = (double *) c_ptr; + c_ptr += opts->max_iter*sizeof(double); + } + + // dual step norm + if (opts->log_dual_step_norm) + { + mem->dual_step_norm = (double *) c_ptr; + c_ptr += opts->max_iter*sizeof(double); + } + // set_sim_guess assign_and_advance_bool(N+1, &mem->set_sim_guess, &c_ptr); for (i = 0; i <= N; ++i) @@ -3383,6 +3413,32 @@ void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out) } +double ocp_nlp_compute_delta_dual_norm_inf(ocp_nlp_dims *dims, ocp_nlp_workspace *work, ocp_nlp_out *nlp_out, ocp_qp_out *qp_out) +{ + /* computes the inf norm of multipliers in qp_out and nlp_out */ + int N = dims->N; + int *nx = dims->nx; + int *ni = dims->ni; + double tmp; + double norm = 0.0; + // compute delta dual + for (int i = 0; i <= N; i++) + { + blasfeo_daxpy(2*ni[i], -1.0, nlp_out->lam+i, 0, qp_out->lam+i, 0, &work->tmp_2ni, 0); + blasfeo_dvecnrm_inf(2*ni[i], &work->tmp_2ni, 0, &tmp); + norm = norm > tmp ? norm : tmp; + if (i < N) + { + blasfeo_daxpy(nx[i+1], -1.0, nlp_out->pi+i, 0, qp_out->pi+i, 0, &work->tmp_nv, 0); + blasfeo_dvecnrm_inf(nx[i+1], &work->tmp_nv, 0, &tmp); + norm = norm > tmp ? norm : tmp; + } + } + return norm; +} + + + void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to) { // extract dims @@ -4106,6 +4162,38 @@ void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const c double *value = return_value_; *value = nlp_mem->cost_value; } + else if (!strcmp("primal_step_norm", field)) + { + if (nlp_mem->primal_step_norm == NULL) + { + printf("\nerror: options log_primal_step_norm was not set\n"); + exit(1); + } + else + { + double *value = return_value_; + for (int ii=0; iiiter; ii++) + { + value[ii] = nlp_mem->primal_step_norm[ii]; + } + } + } + else if (!strcmp("dual_step_norm", field)) + { + if (nlp_mem->dual_step_norm == NULL) + { + printf("\nerror: options log_dual_step_norm was not set\n"); + exit(1); + } + else + { + double *value = return_value_; + for (int ii=0; iiiter; ii++) + { + value[ii] = nlp_mem->dual_step_norm[ii]; + } + } + } else { printf("\nerror: field %s not available in ocp_nlp_memory_get\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index f1a90e5a36..c8b9e6aa43 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -299,6 +299,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 log_dual_step_norm; // compute and log the max norm of the dual steps int max_iter; // maximum number of (SQP/DDP) iterations int qp_iter_max; // maximum iter of QP solver, stored to remember. double tau_min; // minimum value of the barrier parameter, for IPMs @@ -446,6 +447,9 @@ typedef struct ocp_nlp_memory double adaptive_levenberg_marquardt_mu_bar; bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables + double *primal_step_norm; + double *dual_step_norm; + struct blasfeo_dvec *sim_guess; acados_size_t workspace_size; @@ -544,6 +548,8 @@ void ocp_nlp_initialize_qp_from_nlp(ocp_nlp_config *config, ocp_nlp_dims *dims, // void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_res *res, ocp_nlp_memory *mem, ocp_nlp_workspace *work); + +double ocp_nlp_compute_delta_dual_norm_inf(ocp_nlp_dims *dims, ocp_nlp_workspace *work, ocp_nlp_out *nlp_out, ocp_qp_out *qp_out); // void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index a824c2fffc..640fcc5a56 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -286,11 +286,6 @@ acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void // nlp mem size += ocp_nlp_memory_calculate_size(config, dims, nlp_opts, in); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - size += opts->nlp_opts->max_iter*sizeof(double); - } // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 7; @@ -332,12 +327,7 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *i mem->nlp_mem = ocp_nlp_memory_assign(config, dims, nlp_opts, in, c_ptr); c_ptr += ocp_nlp_memory_calculate_size(config, dims, nlp_opts, in); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - mem->primal_step_norm = (double *) c_ptr; - c_ptr += opts->nlp_opts->max_iter*sizeof(double); - } + // stat mem->stat = (double *) c_ptr; @@ -826,9 +816,16 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // Compute the step norm if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) { - mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nlp_mem->qp_out); + mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(qp_out); if (nlp_opts->log_primal_step_norm) - mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + } + if (nlp_opts->log_dual_step_norm) + { + if (nlp_opts->log_dual_step_norm) + { + nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); + } } /* end solve QP */ @@ -1006,12 +1003,12 @@ void ocp_nlp_sqp_eval_solution_sens_adj_p(void *config_, void *dims_, } -// TODO: write getter for things in nlp_mem void ocp_nlp_sqp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_) { ocp_nlp_config *config = config_; ocp_nlp_dims *dims = dims_; ocp_nlp_sqp_memory *mem = mem_; + ocp_nlp_memory *nlp_mem = mem->nlp_mem; char *ptr_module = NULL; int module_length = 0; @@ -1021,32 +1018,16 @@ void ocp_nlp_sqp_get(void *config_, void *dims_, void *mem_, const char *field, if ( ptr_module!=NULL && (!strcmp(ptr_module, "time")) ) { // call timings getter - ocp_nlp_timings_get(config, mem->nlp_mem->nlp_timings, field, return_value_); + ocp_nlp_timings_get(config, nlp_mem->nlp_timings, field, return_value_); } else if (!strcmp("stat", field)) { double **value = return_value_; *value = mem->stat; } - else if (!strcmp("primal_step_norm", field)) - { - if (mem->primal_step_norm == NULL) - { - printf("\nerror: options log_primal_step_norm was not set\n"); - exit(1); - } - else - { - double *value = return_value_; - for (int ii=0; iinlp_mem->iter; ii++) - { - value[ii] = mem->primal_step_norm[ii]; - } - } - } else if (!strcmp("statistics", field)) { - int n_row = mem->stat_mnlp_mem->iter+1 ? mem->stat_m : mem->nlp_mem->iter+1; + int n_row = mem->stat_miter+1 ? mem->stat_m : nlp_mem->iter+1; double *value = return_value_; for (int ii=0; iinlp_mem, field, return_value_); + ocp_nlp_memory_get(config, nlp_mem, field, return_value_); } } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 6a68558907..c78b946dd5 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -316,11 +316,6 @@ acados_size_t ocp_nlp_sqp_wfqp_memory_calculate_size(void *config_, void *dims_, size += ocp_qp_in_calculate_size(dims->relaxed_qp_solver->orig_dims); size += ocp_qp_out_calculate_size(dims->relaxed_qp_solver->orig_dims); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - size += opts->nlp_opts->max_iter*sizeof(double); - } // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 13; @@ -434,13 +429,6 @@ void *ocp_nlp_sqp_wfqp_memory_assign(void *config_, void *dims_, void *opts_, vo // RSQ_constr assign_and_advance_blasfeo_dmat_structs(N + 1, &mem->RSQ_constr, &c_ptr); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - mem->primal_step_norm = (double *) c_ptr; - c_ptr += opts->nlp_opts->max_iter*sizeof(double); - } - // stat mem->stat_m = opts->nlp_opts->max_iter+1; mem->stat_n = 13; @@ -1693,7 +1681,11 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nominal_qp_out); if (nlp_opts->log_primal_step_norm) - mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + } + if (nlp_opts->log_dual_step_norm) + { + nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); } /* globalization */ @@ -1961,7 +1953,7 @@ void ocp_nlp_sqp_wfqp_get(void *config_, void *dims_, void *mem_, const char *fi ocp_nlp_config *config = config_; ocp_nlp_dims *dims = dims_; ocp_nlp_sqp_wfqp_memory *mem = mem_; - + ocp_nlp_memory *nlp_mem = mem->nlp_mem; char module[MAX_STR_LEN]; char *ptr_module = NULL; @@ -1981,32 +1973,16 @@ void ocp_nlp_sqp_wfqp_get(void *config_, void *dims_, void *mem_, const char *fi if ( ptr_module!=NULL && (!strcmp(ptr_module, "time")) ) { // call timings getter - ocp_nlp_timings_get(config, mem->nlp_mem->nlp_timings, field, return_value_); + ocp_nlp_timings_get(config, nlp_mem->nlp_timings, field, return_value_); } else if (!strcmp("stat", field)) { double **value = return_value_; *value = mem->stat; } - else if (!strcmp("primal_step_norm", field)) - { - if (mem->primal_step_norm == NULL) - { - printf("\nerror: options log_primal_step_norm was not set\n"); - exit(1); - } - else - { - double *value = return_value_; - for (int ii=0; iinlp_mem->iter; ii++) - { - value[ii] = mem->primal_step_norm[ii]; - } - } - } else if (!strcmp("statistics", field)) { - int n_row = mem->stat_mnlp_mem->iter+1 ? mem->stat_m : mem->nlp_mem->iter+1; + int n_row = mem->stat_miter+1 ? mem->stat_m : nlp_mem->iter+1; double *value = return_value_; for (int ii=0; iinlp_mem, field, return_value_); + ocp_nlp_memory_get(config, nlp_mem, field, return_value_); } } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index 5cb6b9d360..e3542378a3 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -60,8 +60,6 @@ typedef struct double tol_comp; double tol_unbounded; // exit threshold when objective function seems to be unbounded double tol_min_step_norm; // exit tolerance for small step - int max_iter; - int log_primal_step_norm; // compute and log the max norm of the primal steps bool log_pi_norm_inf; // compute and log the max norm of the pi multipliers bool log_lam_norm_inf; // compute and log the max norm of the lam multipliers bool warm_start_first_qp; @@ -101,7 +99,6 @@ typedef struct ocp_nlp_memory *nlp_mem; double alpha; - double *primal_step_norm; int *nns; // number of non-slacked constraints in NLP int **idxns; // indices of non-slacked constraints in NLP 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 c8604484ad..443abfc937 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -83,6 +83,8 @@ ocp.solver_options.exact_hess_constr = 1; ocp.solver_options.print_level = 1; ocp.solver_options.store_iterates = true; +ocp.solver_options.log_dual_step_norm = true; +ocp.solver_options.log_primal_step_norm = true; % can vary for integrators sim_method_num_stages = 1 * ones(N,1); @@ -254,6 +256,13 @@ % get cost value cost_val_ocp = ocp_solver.get_cost(); +primal_step_norm = ocp_solver.get('primal_step_norm'); +dual_step_norm = ocp_solver.get('dual_step_norm'); +disp('primal step norms') +disp(primal_step_norm); +disp('dual step norms') +disp(dual_step_norm); + %% get QP matrices: % See https://docs.acados.org/problem_formulation % |----- dynamics -----|------ cost --------|---------------------------- constraints ------------------------| diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py index a4819f4b93..97c4ce8e32 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py @@ -46,7 +46,7 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='test Python interface on pendulum example.') - parser.add_argument('--INTEGRATOR_TYPE', dest='INTEGRATOR_TYPE', + parser.add_argument('--INTEGRATOR_TYPE', dest='INTEGRATOR_TYPE', default="ERK", help=f'INTEGRATOR_TYPE: supports {INTEGRATOR_TYPES}') parser.add_argument('--BUILD_SYSTEM', dest='BUILD_SYSTEM', default='make', @@ -62,7 +62,7 @@ build_system = args.BUILD_SYSTEM if build_system not in BUILD_SYSTEMS: - msg = f'Invalid unit test value {build_system} for parameter INTEGRATOR_TYPE. Possible values are' \ + msg = f'Invalid unit test value {build_system} for parameter BUILD_SYSTEM. Possible values are' \ f' {BUILD_SYSTEMS}, got {build_system}.' raise Exception(msg) @@ -118,6 +118,8 @@ ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = integrator_type ocp.solver_options.print_level = 1 + ocp.solver_options.log_dual_step_norm = True + ocp.solver_options.log_primal_step_norm = True if ocp.solver_options.integrator_type == 'GNSF': from acados_template import acados_dae_model_json_dump @@ -171,4 +173,13 @@ simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") + # test getting step norms + primal_step_norms = ocp_solver.get_stats('primal_step_norm') + dual_step_norms = ocp_solver.get_stats('dual_step_norm') + print(f"primal step norms: {primal_step_norms}") + print(f"dual step norms: {dual_step_norms}") + # Assert that step norms are decreasing + assert np.all(np.diff(primal_step_norms)[1:] <= 0), "Primal step norms are not decreasing." + assert np.all(np.diff(dual_step_norms) <= 0), "Dual step norms are not decreasing." + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX) diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 6b6bf79b8d..4d2465bbbc 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -116,6 +116,7 @@ adaptive_levenberg_marquardt_mu_min adaptive_levenberg_marquardt_mu0 log_primal_step_norm + log_dual_step_norm store_iterates eval_residual_at_max_iter @@ -228,6 +229,7 @@ obj.adaptive_levenberg_marquardt_mu_min = 1e-16; obj.adaptive_levenberg_marquardt_mu0 = 1e-3; obj.log_primal_step_norm = 0; + obj.log_dual_step_norm = 0; obj.store_iterates = false; obj.eval_residual_at_max_iter = []; obj.timeout_max_time = 0.; diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index a6f7af7746..977ffad633 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -514,6 +514,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) mat_ptr[ii+(jj+1)*min_size] = stat[jj+ii*stat_n]; } } + else if (!strcmp(field, "primal_step_norm") || !strcmp(field, "dual_step_norm")) + { + int nlp_iter; + ocp_nlp_get(solver, "nlp_iter", &nlp_iter); + plhs[0] = mxCreateNumericMatrix(nlp_iter, 1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + ocp_nlp_get(solver, field, mat_ptr); + } else if (!strcmp(field, "residuals")) { if (plan->nlp_solver == SQP_RTI) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index eb311c1eec..5753216d20 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -125,6 +125,7 @@ def __init__(self): self.__adaptive_levenberg_marquardt_mu_min = 1e-16 self.__adaptive_levenberg_marquardt_mu0 = 1e-3 self.__log_primal_step_norm: bool = False + self.__log_dual_step_norm: bool = False self.__store_iterates: bool = False self.__timeout_max_time = 0. self.__timeout_heuristic = 'LAST' @@ -647,16 +648,25 @@ def adaptive_levenberg_marquardt_mu0(self): return self.__adaptive_levenberg_marquardt_mu0 @property - def log_primal_step_norm(self,): + def log_primal_step_norm(self): """ Flag indicating whether the max norm of the primal steps should be logged. - This is implemented only for solver type `SQP`. + This is implemented only for solver types `SQP`, `SQP_WITH_FEASIBLE_QP`. Default: False """ return self.__log_primal_step_norm @property - def store_iterates(self,): + def log_dual_step_norm(self): + """ + Flag indicating whether the max norm of the dual steps should be logged. + This is implemented only for solver types `SQP`, `SQP_WITH_FEASIBLE_QP`. + Default: False + """ + return self.__log_dual_step_norm + + @property + def store_iterates(self): """ Flag indicating whether the intermediate primal-dual iterates should be stored. This is implemented only for solver types `SQP` and `DDP`. @@ -665,7 +675,7 @@ def store_iterates(self,): return self.__store_iterates @property - def timeout_max_time(self,): + def timeout_max_time(self): """ Maximum time before solver timeout. If 0, there is no timeout. A timeout is triggered if the condition @@ -678,7 +688,7 @@ def timeout_max_time(self,): return self.__timeout_max_time @property - def timeout_heuristic(self,): + def timeout_heuristic(self): """ Heuristic to be used for predicting the runtime of the next SQP iteration, cf. `timeout_max_time`. Possible values are "MAX_CALL", "MAX_OVERALL", "LAST", "AVERAGE", "ZERO". @@ -1728,10 +1738,15 @@ def adaptive_levenberg_marquardt_mu0(self, adaptive_levenberg_marquardt_mu0): @log_primal_step_norm.setter def log_primal_step_norm(self, val): - if isinstance(val, bool): - self.__log_primal_step_norm = val - else: + if not isinstance(val, bool): raise TypeError('Invalid log_primal_step_norm value. Expected bool.') + self.__log_primal_step_norm = val + + @log_dual_step_norm.setter + def log_dual_step_norm(self, val): + if not isinstance(val, bool): + raise TypeError('Invalid log_dual_step_norm value. Expected bool.') + self.__log_dual_step_norm = val @store_iterates.setter def store_iterates(self, val): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 903b927c4d..8493f43b16 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1611,7 +1611,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: self.__acados_lib.ocp_nlp_get(self.nlp_solver, field, out_data) return out - elif field_ == 'primal_step_norm': + elif field_ in ['primal_step_norm', 'dual_step_norm']: nlp_iter = self.get_stats("nlp_iter") out = np.zeros((nlp_iter,), dtype=np.float64, order="C") out_data = cast(out.ctypes.data, POINTER(c_double)) 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 cf44923d66..4b2e68f927 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 @@ -2291,11 +2291,22 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_adaptive_eps", ®_adaptive_eps); {%- endif %} + 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); - 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); +{%- if solver_options.nlp_solver_type == "SQP" or solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} + 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); + + int log_dual_step_norm = {{ solver_options.log_dual_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + + double nlp_solver_tol_min_step_norm = {{ solver_options.nlp_solver_tol_min_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); +{%- endif %} {%- if solver_options.qp_solver is containing("HPIPM") %} // set HPIPM mode: should be done before setting other QP solver options 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 32538cdbc6..c37da7aed1 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 @@ -2408,10 +2408,13 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps 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" %} +{%- if solver_options.nlp_solver_type == "SQP" or solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} 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); + int log_dual_step_norm = {{ solver_options.log_dual_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + double nlp_solver_tol_min_step_norm = {{ solver_options.nlp_solver_tol_min_step_norm }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); {%- endif %} From 053dbdafc4fd377cf297d14c18a602184e27892e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 10 Apr 2025 13:20:01 +0200 Subject: [PATCH 025/164] C: avoid using local variable `sqp_iter` (#1496) Follow-up of https://github.com/acados/acados/pull/1492. The changed tests in https://github.com/acados/acados/pull/1492 have been reverted as the iteration reference was still wrong there. Now I verified by printing. --- acados/ocp_nlp/ocp_nlp_sqp.c | 87 ++++++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 120 +++++++++--------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 2 +- .../non_ocp_nlp/maratos_test_problem.py | 8 +- 4 files changed, 104 insertions(+), 113 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 640fcc5a56..d582c5faaa 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -568,7 +568,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int qp_iter = 0; mem->alpha = 0.0; mem->step_norm = 0.0; - mem->nlp_mem->status = ACADOS_SUCCESS; + nlp_mem->status = ACADOS_SUCCESS; nlp_mem->objective_multiplier = 1.0; if (opts->timeout_heuristic != MAX_OVERALL) @@ -586,7 +586,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, /************************************************ * main sqp loop ************************************************/ - int sqp_iter = 0; + nlp_mem->iter = 0; double prev_levenberg_marquardt = 0.0; int globalization_status; qp_info *qp_info_; @@ -594,17 +594,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, double timeout_previous_time_tot = 0.; double timeout_time_prev_iter = 0.; - 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. + for (; nlp_mem->iter <= opts->nlp_opts->max_iter; nlp_mem->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->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->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]); + copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[nlp_mem->iter]); } /* Prepare the QP data */ // linearize NLP and update QP matrices @@ -617,7 +617,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } - ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, sqp_iter, nlp_mem->qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, nlp_mem->qp_in); nlp_timings->time_lin += acados_toc(&timer1); // compute nlp residuals @@ -626,24 +626,24 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Initialize globalization strategies (do not move outside the SQP loop) - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { config->globalization->initialize_memory(config, dims, nlp_mem, nlp_opts); } // save statistics - if (sqp_iter < mem->stat_m) + if (nlp_mem->iter < mem->stat_m) { - mem->stat[mem->stat_n*sqp_iter+0] = nlp_res->inf_norm_res_stat; - mem->stat[mem->stat_n*sqp_iter+1] = nlp_res->inf_norm_res_eq; - mem->stat[mem->stat_n*sqp_iter+2] = nlp_res->inf_norm_res_ineq; - mem->stat[mem->stat_n*sqp_iter+3] = nlp_res->inf_norm_res_comp; + mem->stat[mem->stat_n*nlp_mem->iter+0] = nlp_res->inf_norm_res_stat; + mem->stat[mem->stat_n*nlp_mem->iter+1] = nlp_res->inf_norm_res_eq; + mem->stat[mem->stat_n*nlp_mem->iter+2] = nlp_res->inf_norm_res_ineq; + mem->stat[mem->stat_n*nlp_mem->iter+3] = nlp_res->inf_norm_res_comp; } // Output if (nlp_opts->print_level > 0) { - print_iteration(sqp_iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); + print_iteration(nlp_mem->iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); } prev_levenberg_marquardt = nlp_opts->levenberg_marquardt; @@ -659,7 +659,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { nlp_timings->time_tot = acados_toc(&timer0); - if (sqp_iter > 0) + if (nlp_mem->iter > 0) { timeout_time_prev_iter = nlp_timings->time_tot - timeout_previous_time_tot; @@ -673,7 +673,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->timeout_estimated_per_iteration_time = timeout_time_prev_iter > mem->timeout_estimated_per_iteration_time ? timeout_time_prev_iter : mem->timeout_estimated_per_iteration_time; break; case AVERAGE: - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { mem->timeout_estimated_per_iteration_time = timeout_time_prev_iter; } @@ -695,21 +695,20 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Termination - if (check_termination(sqp_iter, dims, nlp_res, mem, opts)) + if (check_termination(nlp_mem->iter, dims, nlp_res, mem, opts)) { #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer0); - return mem->nlp_mem->status; + return nlp_mem->status; } /* solve QP */ // warm start of first QP - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { if (!opts->warm_start_first_qp) { @@ -727,47 +726,47 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // Show input to QP if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_in at iteration %d\n", nlp_mem->iter); print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 0); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL); // restore default warm start - if (sqp_iter==0) + if (nlp_mem->iter==0) { qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start); } if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_out at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_out at iteration %d\n", nlp_mem->iter); print_ocp_qp_out(qp_out); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 0); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 0); #endif ocp_qp_out_get(qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; // save statistics of last qp solver call - if (sqp_iter+1 < mem->stat_m) + if (nlp_mem->iter+1 < mem->stat_m) { - mem->stat[mem->stat_n*(sqp_iter+1)+4] = qp_status; - mem->stat[mem->stat_n*(sqp_iter+1)+5] = qp_iter; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+5] = qp_iter; } // compute external QP residuals (for debugging) if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); - if (sqp_iter+1 < mem->stat_m) - ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+7)); + if (nlp_mem->iter+1 < mem->stat_m) + ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, mem->stat+(mem->stat_n*(nlp_mem->iter+1)+7)); } // exit conditions on QP status @@ -775,17 +774,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { 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, + printf("%i\t%e\t%e\t%e\t%e.\n", nlp_mem->iter, nlp_res->inf_norm_res_stat, nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq, nlp_res->inf_norm_res_comp ); printf("\n\n"); } - // increment sqp_iter to return full statistics and improve output below. - sqp_iter++; + // increment nlp_mem->iter to return full statistics and improve output below. + nlp_mem->iter++; #ifndef ACADOS_SILENT printf("\nQP solver returned error status %d in SQP iteration %d, QP iteration %d.\n", - qp_status, sqp_iter, qp_iter); + qp_status, nlp_mem->iter, qp_iter); #endif #if defined(ACADOS_WITH_OPENMP) // restore number of threads @@ -798,11 +797,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, print_ocp_qp_in(qp_in); } - mem->nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; + nlp_mem->status = ACADOS_QP_FAILURE; nlp_timings->time_tot = acados_toc(&timer0); - return mem->nlp_mem->status; + return nlp_mem->status; } // Calculate optimal QP objective (needed for globalization) @@ -814,17 +812,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Compute the step norm - if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) + if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm || nlp_opts->print_level > 0) { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(qp_out); if (nlp_opts->log_primal_step_norm) - nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } if (nlp_opts->log_dual_step_norm) { if (nlp_opts->log_dual_step_norm) { - nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); } } /* end solve QP */ @@ -842,17 +840,16 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { printf("\nFailure in globalization, got status %d!\n", globalization_status); } - mem->nlp_mem->status = globalization_status; - nlp_mem->iter = sqp_iter; + nlp_mem->status = globalization_status; nlp_timings->time_tot = acados_toc(&timer0); #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - return mem->nlp_mem->status; + return nlp_mem->status; } - if (sqp_iter+1 < mem->stat_m) - mem->stat[mem->stat_n*(sqp_iter+1)+6] = mem->alpha; + if (nlp_mem->iter+1 < mem->stat_m) + mem->stat[mem->stat_n*(nlp_mem->iter+1)+6] = mem->alpha; } // end SQP loop @@ -864,7 +861,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - return mem->nlp_mem->status; + return nlp_mem->status; } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index c78b946dd5..f44acb229a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -917,7 +917,7 @@ static void set_pointers_for_hessian_evaluation(ocp_nlp_config *config, static void setup_hessian_matrices_for_qps(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_sqp_wfqp_opts *opts, - ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work, int sqp_iter) + ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; @@ -951,7 +951,7 @@ static void setup_hessian_matrices_for_qps(ocp_nlp_config *config, blasfeo_dveccpsc(2*ns[i], 1.0, mem->Z_cost_module+i, 0, nominal_qp_in->Z+i, 0); } // Levenberg Marquardt term for nominal QP - ocp_nlp_add_levenberg_marquardt_term(config, dims, in, out, opts->nlp_opts, nlp_mem, work, mem->alpha, sqp_iter, nlp_mem->qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, in, out, opts->nlp_opts, nlp_mem, work, mem->alpha, nlp_mem->iter, nlp_mem->qp_in); } /* @@ -959,7 +959,7 @@ Solves the QP. Either solves feasibility QP or nominal QP */ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* opts, ocp_qp_in* qp_in, ocp_qp_out* qp_out, ocp_nlp_dims *dims, ocp_nlp_sqp_wfqp_memory* mem, ocp_nlp_in* nlp_in, ocp_nlp_out* nlp_out, - ocp_nlp_memory* nlp_mem, ocp_nlp_workspace* nlp_work, int sqp_iter, bool solve_feasibility_qp, + ocp_nlp_memory* nlp_mem, ocp_nlp_workspace* nlp_work, bool solve_feasibility_qp, acados_timer timer_tot) { acados_timer timer_qp; @@ -969,7 +969,7 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o int qp_status = ACADOS_SUCCESS; // warm start of first QP - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { if (!opts->warm_start_first_qp) { @@ -985,12 +985,12 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o } } - if (mem->qps_solved_in_sqp_iter < 2 && (!solve_feasibility_qp || opts->use_constraint_hessian_in_feas_qp)) + if (mem->qps_solved_in_iter < 2 && (!solve_feasibility_qp || opts->use_constraint_hessian_in_feas_qp)) { if (solve_feasibility_qp && opts->use_constraint_hessian_in_feas_qp) { // LM for feasibility QP - ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work, mem->alpha, sqp_iter, qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, qp_in); } // regularize Hessian @@ -1002,13 +1002,13 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o // Show input to QP if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_in at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_in->dim); print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 0); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif if (solve_feasibility_qp) @@ -1032,31 +1032,30 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o nlp_mem, nlp_work, false, NULL, NULL, NULL); } - - mem->qps_solved_in_sqp_iter += 1; + mem->qps_solved_in_iter += 1; // restore default warm start - if (sqp_iter==0) + if (nlp_mem->iter==0) { qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start); } if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_out at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_out at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_out->dim); print_ocp_qp_out(qp_out); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 0); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 0); #endif // exit conditions on QP status if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) { - // increment sqp_iter to return full statistics and improve output below. - sqp_iter++; + // increment nlp_mem->iter to return full statistics and improve output below. + nlp_mem->iter++; if (nlp_opts->print_level > 1) { @@ -1066,7 +1065,6 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o } mem->nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return mem->nlp_mem->status; @@ -1074,43 +1072,45 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o return qp_status; } -static void log_qp_stats(ocp_nlp_sqp_wfqp_memory *mem, int sqp_iter, bool solve_feasibility_qp, +static void log_qp_stats(ocp_nlp_sqp_wfqp_memory *mem, bool solve_feasibility_qp, int qp_status, int qp_iter) { - if (sqp_iter < mem->stat_m) + int nlp_iter = mem->nlp_mem->iter; + if (nlp_iter < mem->stat_m) { if (mem->search_direction_mode == NOMINAL_QP) { - mem->stat[mem->stat_n*(sqp_iter)+4] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+5] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+4] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+5] = qp_iter; } else if (mem->search_direction_mode == BYRD_OMOJOKUN) { if (solve_feasibility_qp) { - mem->stat[mem->stat_n*(sqp_iter)+6] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+7] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+6] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+7] = qp_iter; } else { - mem->stat[mem->stat_n*(sqp_iter)+8] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+9] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+8] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+9] = qp_iter; } } } } -static void log_multiplier_norms(int sqp_iter, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_opts *opts, ocp_nlp_out *nlp_out, ocp_nlp_dims *dims) +static void log_multiplier_norms(ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_opts *opts, ocp_nlp_out *nlp_out, ocp_nlp_dims *dims) { + int nlp_iter = mem->nlp_mem->iter; if (opts->log_pi_norm_inf) { mem->norm_inf_pi = ocp_nlp_compute_dual_pi_norm_inf(dims, nlp_out); - mem->stat[mem->stat_n*(sqp_iter)+11] = mem->norm_inf_pi; + mem->stat[mem->stat_n*(nlp_iter)+11] = mem->norm_inf_pi; } if (opts->log_lam_norm_inf) { mem->norm_inf_lam = ocp_nlp_compute_dual_lam_norm_inf(dims, nlp_out); - mem->stat[mem->stat_n*(sqp_iter)+12] = mem->norm_inf_lam; + mem->stat[mem->stat_n*(nlp_iter)+12] = mem->norm_inf_lam; } } @@ -1175,7 +1175,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_workspace *work, - int sqp_iter, acados_timer timer_tot) { ocp_nlp_memory* nlp_mem = mem->nlp_mem; @@ -1196,10 +1195,10 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, /* Solve Feasibility QP: Objective: Only constraint Hessian/Identity AND only gradient of slack variables */ print_debug_output("Solve Feasibility QP!\n", nlp_opts->print_level, 2); qp_status = prepare_and_solve_QP(config, opts, relaxed_qp_in, relaxed_qp_out, dims, mem, nlp_in, nlp_out, - nlp_mem, nlp_work, sqp_iter, true, timer_tot); + nlp_mem, nlp_work, true, timer_tot); ocp_qp_out_get(relaxed_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, true, qp_status, qp_iter); + log_qp_stats(mem, true, qp_status, qp_iter); if (qp_status != ACADOS_SUCCESS) { if (nlp_opts->print_level >=1) @@ -1207,7 +1206,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, printf("\nError in feasibility QP in iteration %d, got qp_status %d!\n", qp_iter, qp_status); } nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return nlp_mem->status; } @@ -1223,10 +1221,10 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, setup_byrd_omojokun_bounds(dims, nlp_mem, mem, work, opts); // solve_feasibility_qp --> false in prepare_and_solve_QP qp_status = prepare_and_solve_QP(config, opts, nominal_qp_in, nominal_qp_out, dims, mem, nlp_in, nlp_out, - nlp_mem, nlp_work, sqp_iter, false, timer_tot); + nlp_mem, nlp_work, false, timer_tot); ocp_qp_out_get(nominal_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, false, qp_status, qp_iter); + log_qp_stats(mem, false, qp_status, qp_iter); if (qp_status != ACADOS_SUCCESS) { @@ -1235,7 +1233,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, printf("\nError in nominal QP in iteration %d, got qp_status %d!\n", qp_iter, qp_status); } nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return nlp_mem->status; } @@ -1310,7 +1307,7 @@ update QP rhs for feasibility QP (step prim var, abs dual var) */ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, - ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work, int sqp_iter) + ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; @@ -1332,7 +1329,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf blasfeo_dveccp(ns[i], nominal_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i], relaxed_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i]+nns[i]); } // setup d_mask; TODO: this is only needed at the start of each NLP solve - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { int offset_dmask; for (int i=0; i<=dims->N; i++) @@ -1436,14 +1433,13 @@ static int calculate_search_direction(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_workspace *work, - int sqp_iter, acados_timer timer_tot) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; qp_info* qp_info_; int qp_iter = 0; int search_direction_status; - mem->qps_solved_in_sqp_iter = 0; + mem->qps_solved_in_iter = 0; if (mem->search_direction_mode == NOMINAL_QP) { @@ -1451,10 +1447,10 @@ static int calculate_search_direction(ocp_nlp_dims *dims, // otherwise, we change the mode to Byrd-Omojokun and we continue. search_direction_status = prepare_and_solve_QP(config, opts, nlp_mem->qp_in, nlp_mem->qp_out, dims, mem, nlp_in, nlp_out, nlp_mem, work->nlp_work, - sqp_iter, false, timer_tot); + false, timer_tot); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, false, search_direction_status, qp_iter); + log_qp_stats(mem, false, search_direction_status, qp_iter); if (search_direction_status != ACADOS_SUCCESS) { if (nlp_opts->print_level >1) @@ -1482,7 +1478,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, { // We solve two QPs and return the search direction that we found! // if the second QP is feasible, we change back to nominal QP mode. - if (mem->qps_solved_in_sqp_iter == 1) + if (mem->qps_solved_in_iter == 1) { mem->search_direction_type = "NFN"; } @@ -1490,7 +1486,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, { mem->search_direction_type = "FN"; } - search_direction_status = byrd_omojokun_direction_computation(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, sqp_iter, timer_tot); + search_direction_status = byrd_omojokun_direction_computation(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); double l1_inf = calculate_qp_l1_infeasibility(dims, mem, work, opts, mem->relaxed_qp_in, mem->relaxed_qp_out); if (l1_inf/(fmax(1.0, (double) mem->absolute_nns)) < opts->tol_ineq) { @@ -1578,7 +1574,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, /************************************************ * main sqp loop ************************************************/ - int sqp_iter = 0; + nlp_mem->iter = 0; double prev_levenberg_marquardt = 0.0; int search_direction_status = 0; @@ -1587,17 +1583,17 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, print_indices(dims, work, mem); } - 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. + for (; nlp_mem->iter <= opts->nlp_opts->max_iter; nlp_mem->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->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->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]); + copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[nlp_mem->iter]); } /* Prepare the QP data */ // linearize NLP and update QP matrices @@ -1609,14 +1605,14 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // relaxed QP solver // matrices for relaxed QP solver evaluated in nominal QP solver - ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work, sqp_iter); + ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work); if (nlp_opts->with_adaptive_levenberg_marquardt || config->globalization->needs_objective_value() == 1) { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } - setup_hessian_matrices_for_qps(config, dims, nlp_in, nlp_out, opts, mem, nlp_work, sqp_iter); + setup_hessian_matrices_for_qps(config, dims, nlp_in, nlp_out, opts, mem, nlp_work); // nlp_timings->time_lin += acados_toc(&timer1); // compute nlp residuals @@ -1625,37 +1621,36 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Initialize the memory for different globalization strategies - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { config->globalization->initialize_memory(config, dims, nlp_mem, nlp_opts); } // save statistics - if (sqp_iter < mem->stat_m) + if (nlp_mem->iter < mem->stat_m) { - mem->stat[mem->stat_n*sqp_iter+0] = nlp_res->inf_norm_res_stat; - mem->stat[mem->stat_n*sqp_iter+1] = nlp_res->inf_norm_res_eq; - mem->stat[mem->stat_n*sqp_iter+2] = nlp_res->inf_norm_res_ineq; - mem->stat[mem->stat_n*sqp_iter+3] = nlp_res->inf_norm_res_comp; + mem->stat[mem->stat_n*nlp_mem->iter+0] = nlp_res->inf_norm_res_stat; + mem->stat[mem->stat_n*nlp_mem->iter+1] = nlp_res->inf_norm_res_eq; + mem->stat[mem->stat_n*nlp_mem->iter+2] = nlp_res->inf_norm_res_ineq; + mem->stat[mem->stat_n*nlp_mem->iter+3] = nlp_res->inf_norm_res_comp; } - log_multiplier_norms(sqp_iter, mem, opts, nlp_out, dims); + log_multiplier_norms(mem, opts, nlp_out, dims); /* Output */ if (nlp_opts->print_level > 0) { - print_iteration(sqp_iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); + print_iteration(nlp_mem->iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); } prev_levenberg_marquardt = nlp_opts->levenberg_marquardt; /* Termination */ - if (check_termination(sqp_iter, dims, nlp_res, mem, opts)) + if (check_termination(nlp_mem->iter, dims, nlp_res, mem, opts)) { #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return mem->nlp_mem->status; } @@ -1666,7 +1661,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } /* Search Direction Computation */ - search_direction_status = calculate_search_direction(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, sqp_iter, timer_tot); + search_direction_status = calculate_search_direction(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); if (search_direction_status != ACADOS_SUCCESS) { #if defined(ACADOS_WITH_OPENMP) @@ -1681,11 +1676,11 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nominal_qp_out); if (nlp_opts->log_primal_step_norm) - nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } if (nlp_opts->log_dual_step_norm) { - nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); } /* globalization */ @@ -1711,7 +1706,6 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, printf("\nFailure in globalization, got status %d!\n", globalization_status); } nlp_mem->status = globalization_status; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); #if defined(ACADOS_WITH_OPENMP) // restore number of threads @@ -1720,7 +1714,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, return nlp_mem->status; } - mem->stat[mem->stat_n*(sqp_iter+1)+10] = mem->alpha; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+10] = mem->alpha; nlp_timings->time_glob += acados_toc(&timer1); } // end SQP loop diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index e3542378a3..39a9efbdf2 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -124,7 +124,7 @@ typedef struct int watchdog_zero_slacks_counter; // counts number of consecutive BYRD_OMOJOKUN iter with slack sum == 0 int absolute_nns; // sum of all nns[i] - int qps_solved_in_sqp_iter; + int qps_solved_in_iter; // QP solver with always feasible QPs ocp_qp_xcond_solver relaxed_qp_solver; diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index 9be5054f7e..13a3561525 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -212,8 +212,8 @@ def solve_maratos_problem_with_setting(setting): if any(alphas[:iter] != 1.0): raise Exception(f"Expected all alphas = 1.0 when using full step SQP on Maratos problem") elif globalization == 'MERIT_BACKTRACKING': - if max_infeasibility > 1.5: - raise Exception(f"Expected max_infeasibility < 1.5 when using globalized SQP on Maratos problem") + if max_infeasibility > 0.5: + raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") @@ -226,8 +226,8 @@ def solve_maratos_problem_with_setting(setting): # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception(f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") else: - if iter != 10: - raise Exception(f"Expected 10 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") + if iter != 16: + raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") From 0e55eeecc62cd4fd4a25de3647a961cb9ec754cf Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 11 Apr 2025 17:59:24 +0200 Subject: [PATCH 026/164] Template: avoid openmp dependency when not batched (#1497) If got lost in #1483 --- .../acados_template/c_templates_tera/acados_solver.in.c | 2 ++ 1 file changed, 2 insertions(+) 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 c37da7aed1..676d3970dc 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 @@ -37,8 +37,10 @@ #include "acados_c/ocp_nlp_interface.h" #include "acados_c/external_function_interface.h" +{%- if solver_options.with_batch_functionality %} // openmp #include +{%- endif %} // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" From 0f042a191d8fea0cbcfcf9044dc28a31c67f9c6f Mon Sep 17 00:00:00 2001 From: Confectio <30325218+Confectio@users.noreply.github.com> Date: Mon, 14 Apr 2025 09:38:34 +0200 Subject: [PATCH 027/164] Allow `solver_options.N_horizon=0` (#1498) This PR modifies the Matlab and Python interfaces to allow formulating optimization problems without any shooting-structure and `solver_options.N_horizon=0` . If `solver_options.N_horizon=0`, only the terminal costs and constraints will be regarded. In particular, also no dynamics, other costs, and constraints have to be defined. We have also updated some examples to use this feature instead of the workarounds. --------- Co-authored-by: Jonathan Frey --- .../convex_problem_globalization_necessary.m | 8 +- .../env.sh | 75 +++ .../acados_matlab_octave/generic_nlp/env.sh | 75 +++ .../acados_matlab_octave/generic_nlp/main.m | 18 +- .../non_ocp_nlp/maratos_test_problem.py | 153 +++-- .../non_ocp_example.py | 18 +- external/hpipm | 2 +- interfaces/acados_matlab_octave/AcadosOcp.m | 583 +++++++++++------- .../acados_template/acados_dims.py | 2 +- .../acados_template/acados_ocp.py | 538 ++++++++++------ .../acados_template/acados_ocp_options.py | 6 +- .../acados_template/acados_ocp_solver.py | 3 - .../c_templates_tera/CMakeLists.in.txt | 23 +- .../c_templates_tera/acados_solver.in.c | 72 ++- 14 files changed, 1017 insertions(+), 559 deletions(-) create mode 100644 examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh create mode 100644 examples/acados_matlab_octave/generic_nlp/env.sh diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m b/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m index fe0252eb53..0797a11e2b 100644 --- a/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m +++ b/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m @@ -48,13 +48,11 @@ x = SX.sym('x'); % dynamics: identity - model.disc_dyn_expr = x; model.x = x; model.name = strcat('convex_problem_', globalization); %% solver settings - T = 1; - N_horizon = 1; + N_horizon = 0; %% OCP formulation object ocp = AcadosOcp(); @@ -63,11 +61,8 @@ % terminal cost term ocp.cost.cost_type_e = 'EXTERNAL'; ocp.model.cost_expr_ext_cost_e = log(exp(model.x) + exp(-model.x)); - ocp.cost.cost_type = 'EXTERNAL'; - ocp.model.cost_expr_ext_cost = 0; % define solver options - ocp.solver_options.tf = T; ocp.solver_options.N_horizon = N_horizon; ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'; ocp.solver_options.hessian_approx = 'EXACT'; @@ -87,7 +82,6 @@ % set trajectory initialization ocp_solver.set('init_x', xinit, 0); - ocp_solver.set('init_x', xinit, 1); % solve ocp_solver.solve(); diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh b/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh new file mode 100644 index 0000000000..f37d215665 --- /dev/null +++ b/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh @@ -0,0 +1,75 @@ +#! /usr/bin/bash +# +# 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.; +# + + +if [[ "${BASH_SOURCE[0]}" != "${0}" ]] +then + echo "Script is being sourced" +else + echo "ERROR: Script is a subshell" + echo "To affect your current shell enviroment source this script with:" + echo "source env.sh" + exit +fi + +# check that this file is run +export ENV_RUN=true + +# if acados folder not specified assume parent of the folder of the single examples +ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} +export ACADOS_INSTALL_DIR +echo +echo "ACADOS_INSTALL_DIR=$ACADOS_INSTALL_DIR" + +# export casadi folder and matlab/octave mex folder +# MATLAB case +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/external/casadi-matlab/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ + +echo +echo "MATLABPATH=$MATLABPATH" +# Octave case +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ +echo +echo "OCTAVE_PATH=$OCTAVE_PATH" + +# export acados mex flags +#export ACADOS_MEX_FLAGS="GCC=/usr/bin/gcc-4.9" + +# if model folder not specified assume this folder +MODEL_FOLDER=${MODEL_FOLDER:-"./build"} +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ACADOS_INSTALL_DIR/lib:$MODEL_FOLDER +export LD_RUN_PATH="$(pwd)"/c_generated_code +echo +echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" diff --git a/examples/acados_matlab_octave/generic_nlp/env.sh b/examples/acados_matlab_octave/generic_nlp/env.sh new file mode 100644 index 0000000000..f37d215665 --- /dev/null +++ b/examples/acados_matlab_octave/generic_nlp/env.sh @@ -0,0 +1,75 @@ +#! /usr/bin/bash +# +# 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.; +# + + +if [[ "${BASH_SOURCE[0]}" != "${0}" ]] +then + echo "Script is being sourced" +else + echo "ERROR: Script is a subshell" + echo "To affect your current shell enviroment source this script with:" + echo "source env.sh" + exit +fi + +# check that this file is run +export ENV_RUN=true + +# if acados folder not specified assume parent of the folder of the single examples +ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} +export ACADOS_INSTALL_DIR +echo +echo "ACADOS_INSTALL_DIR=$ACADOS_INSTALL_DIR" + +# export casadi folder and matlab/octave mex folder +# MATLAB case +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/external/casadi-matlab/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ + +echo +echo "MATLABPATH=$MATLABPATH" +# Octave case +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ +echo +echo "OCTAVE_PATH=$OCTAVE_PATH" + +# export acados mex flags +#export ACADOS_MEX_FLAGS="GCC=/usr/bin/gcc-4.9" + +# if model folder not specified assume this folder +MODEL_FOLDER=${MODEL_FOLDER:-"./build"} +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ACADOS_INSTALL_DIR/lib:$MODEL_FOLDER +export LD_RUN_PATH="$(pwd)"/c_generated_code +echo +echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" diff --git a/examples/acados_matlab_octave/generic_nlp/main.m b/examples/acados_matlab_octave/generic_nlp/main.m index b111b6a582..926497bcc8 100644 --- a/examples/acados_matlab_octave/generic_nlp/main.m +++ b/examples/acados_matlab_octave/generic_nlp/main.m @@ -47,7 +47,6 @@ model.name = 'generic_nlp'; model.x = x; model.p = p; -model.f_expl_expr = casadi.SX.zeros(length(model.x),1); %% acados ocp formulation ocp = AcadosOcp(); @@ -66,19 +65,10 @@ % initial parameter values ocp.parameter_values = zeros(length(model.p),1); -% set additional fields to prevent errors/warnings -ocp.cost.cost_type_0 = 'EXTERNAL'; -ocp.model.cost_expr_ext_cost_0 = 0; -ocp.cost.cost_type = 'EXTERNAL'; -ocp.model.cost_expr_ext_cost = 0; - %% solver options -ocp.solver_options.tf = 1; -ocp.solver_options.N_horizon = 1; +ocp.solver_options.N_horizon = 0; ocp.solver_options.nlp_solver_type = 'SQP'; -ocp.solver_options.integrator_type = 'ERK'; -ocp.solver_options.sim_method_num_stages = 1; -ocp.solver_options.sim_method_num_steps = 1; +ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'; %TODO: Change after PARTIAL_CONDENSING_HPIPM fix %% create the solver ocp_solver = AcadosOcpSolver(ocp); @@ -86,7 +76,7 @@ %% solve the NLP % initial guess init_x = [2.5; 3.0]; -ocp_solver.set('init_x', repmat(init_x,1,2)); +ocp_solver.set('init_x', init_x); % set the parameters p_value = [1;1]; @@ -104,7 +94,7 @@ end % display results -x_opt = ocp_solver.get('x',1); +x_opt = ocp_solver.get('x', 0); disp('Optimal solution:') % should be [1;1] for p = [1;1] disp(x_opt) disp(['Total time: ', num2str(1e3*total_time), ' ms']) diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index 13a3561525..8ed7512374 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -41,7 +41,6 @@ # Settings PLOT = False -FOR_LOOPING = False # call solver in for loop to get all iterates TOL = 1e-6 def main(): @@ -63,6 +62,12 @@ def main(): pass else: solve_maratos_problem_with_setting(setting) + # exit(1) + # pass + # setting = {"globalization": "MERIT_BACKTRACKING", + # "line_search_use_sufficient_descent": 0, + # "globalization_use_SOC": 1} + # solve_maratos_problem_with_setting(setting) def solve_maratos_problem_with_setting(setting): @@ -77,57 +82,66 @@ def solve_maratos_problem_with_setting(setting): ocp = AcadosOcp() # set model - model = AcadosModel() x1 = SX.sym('x1') x2 = SX.sym('x2') x = vertcat(x1, x2) # dynamics: identity - model.disc_dyn_expr = x - model.x = x - model.u = SX.sym('u', 0, 0) # [] / None doesnt work - model.p = [] - model.name = f'maratos_problem' - ocp.model = model + ocp.model.x = x + ocp.model.name = f'maratos_problem' # discretization - Tf = 1 N = 1 ocp.solver_options.N_horizon = N - ocp.solver_options.tf = Tf - # cost - ocp.cost.cost_type_e = 'EXTERNAL' - ocp.model.cost_expr_ext_cost_e = x1 - - # constarints - ocp.model.con_h_expr_0 = x1 ** 2 + x2 ** 2 - ocp.constraints.lh_0 = np.array([1.0]) - ocp.constraints.uh_0 = np.array([1.0]) + if N == 0: + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = x1 + + # constraints + ocp.model.con_h_expr_e = x1 ** 2 + x2 ** 2 + ocp.constraints.lh_e = np.array([1.0]) + ocp.constraints.uh_e = np.array([1.0]) + elif N == 1: + # dynamics: identity + ocp.model.disc_dyn_expr = x + ocp.model.u = SX.sym('u', 0, 0) # [] / None doesnt work + + # discretization + ocp.solver_options.tf = 1.0 + ocp.solver_options.integrator_type = 'DISCRETE' + + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = x1 + + # constarints + ocp.model.con_h_expr_0 = x1 ** 2 + x2 ** 2 + ocp.constraints.lh_0 = np.array([1.0]) + ocp.constraints.uh_0 = np.array([1.0]) + else: + raise NotImplementedError('N > 1 not implemented') # # soften - # ocp.constraints.idxsh = np.array([0]) - # ocp.cost.zl = 1e5 * np.array([1]) - # ocp.cost.zu = 1e5 * np.array([1]) - # ocp.cost.Zl = 1e5 * np.array([1]) - # ocp.cost.Zu = 1e5 * np.array([1]) + # ocp.constraints.idxsh_e = np.array([0]) + # ocp.cost.zl_e = 1e5 * np.array([1]) + # ocp.cost.zu_e = 1e5 * np.array([1]) + # ocp.cost.Zl_e = 1e5 * np.array([1]) + # ocp.cost.Zu_e = 1e5 * np.array([1]) # add bounds on x # nx = 2 - # ocp.constraints.idxbx_0 = np.array(range(nx)) - # ocp.constraints.lbx_0 = -2 * np.ones((nx)) - # ocp.constraints.ubx_0 = 2 * np.ones((nx)) + # ocp.constraints.idxbx_e = np.array(range(nx)) + # ocp.constraints.lbx_e = -2 * np.ones((nx)) + # ocp.constraints.ubx_e = 2 * np.ones((nx)) # 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.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # TODO: check difference wrt FULL_CONDENSING ocp.solver_options.hessian_approx = 'EXACT' - ocp.solver_options.integrator_type = 'DISCRETE' - if globalization == 'FUNNEL_L1PEN_LINESEARCH': - ocp.solver_options.print_level = 1 + # ocp.solver_options.print_level = 2 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP - ocp.solver_options.levenberg_marquardt = 1e-1 + ocp.solver_options.levenberg_marquardt = 1e-1 # / (N+1) SQP_max_iter = 300 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.qp_tol = 5e-7 @@ -138,13 +152,10 @@ def solve_maratos_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 = 1e-1 + ocp.solver_options.store_iterates = True - if FOR_LOOPING: # call solver in for loop to get all iterates - ocp.solver_options.nlp_solver_max_iter = 1 - ocp_solver = AcadosOcpSolver(ocp, verbose=False) - else: - ocp.solver_options.nlp_solver_max_iter = SQP_max_iter - ocp_solver = AcadosOcpSolver(ocp, verbose=False) + ocp.solver_options.nlp_solver_max_iter = SQP_max_iter + ocp_solver = AcadosOcpSolver(ocp, verbose=False) # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 @@ -153,34 +164,18 @@ def solve_maratos_problem_with_setting(setting): [ocp_solver.set(i, "x", xinit) for i in range(N+1)] # solve - if FOR_LOOPING: # call solver in for loop to get all iterates - iterates = np.zeros((SQP_max_iter+1, 2)) - iterates[0, :] = xinit - alphas = np.zeros((SQP_max_iter,)) - qp_iters = np.zeros((SQP_max_iter,)) - iter = SQP_max_iter - residuals = np.zeros((4, SQP_max_iter)) - - # solve - for i in range(SQP_max_iter): - status = ocp_solver.solve() - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # print(f'acados returned status {status}.') - iterates[i+1, :] = ocp_solver.get(0, "x") - if status in [0, 4]: - iter = i - break - alphas[i] = ocp_solver.get_stats('alpha')[1] - qp_iters[i] = ocp_solver.get_stats('qp_iter')[1] - residuals[:, i] = ocp_solver.get_stats('residuals') - - else: - ocp_solver.solve() - ocp_solver.print_statistics() - iter = ocp_solver.get_stats('sqp_iter') - alphas = ocp_solver.get_stats('alpha')[1:] - qp_iters = ocp_solver.get_stats('qp_iter') - residuals = ocp_solver.get_stats('statistics')[1:5,1:iter] + ocp_solver.solve() + ocp_solver.print_statistics() + iter = ocp_solver.get_stats('sqp_iter') + alphas = ocp_solver.get_stats('alpha')[1:] + qp_iters = ocp_solver.get_stats('qp_iter') + residuals = ocp_solver.get_stats('statistics')[1:5,1:iter] + iterates = ocp_solver.get_iterates() + x_iterates = iterates.as_array('x') + if N > 0: + xdiff = x_iterates[:, 0, :] - x_iterates[:, 1, :] + xdiff = np.linalg.norm(xdiff, axis=1) + print(f"xdiff = {xdiff}") # get solution solution = ocp_solver.get(0, "x") @@ -199,7 +194,7 @@ def solve_maratos_problem_with_setting(setting): # checks if sol_err > TOL*1e1: - raise Exception(f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}") + print(f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}") else: print(f"matched analytical solution with tolerance {TOL}") @@ -214,9 +209,9 @@ def solve_maratos_problem_with_setting(setting): elif globalization == 'MERIT_BACKTRACKING': if max_infeasibility > 0.5: raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") - if globalization_use_SOC == 0: - if FOR_LOOPING and iter != 57: - raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") + elif globalization_use_SOC == 0: + if iter not in range(56, 61): + raise Exception(f"Expected 56 to 60 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") elif line_search_use_sufficient_descent == 1: if iter not in range(29, 37): # NOTE: got 29 locally and 36 on Github actions. @@ -230,13 +225,12 @@ def solve_maratos_problem_with_setting(setting): raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: - raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") + raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") except Exception as inst: - if FOR_LOOPING and globalization == "MERIT_BACKTRACKING": - print("\nAcados globalized OCP solver behaves different when for looping due to different merit function weights.", - "Following exception is not raised\n") - print(inst, "\n") + if N == 0: + print(f"Exceptions in this file are tailored to formulation with N=1, difference should be investigated.") + print(f"got Exception {inst} in test with settings {setting}") else: raise(inst) @@ -244,10 +238,9 @@ def solve_maratos_problem_with_setting(setting): plt.figure() axs = plt.plot(solution[0], solution[1], 'x', label='solution') - if FOR_LOOPING: # call solver in for loop to get all iterates - cm = plt.cm.get_cmap('RdYlBu') - axs = plt.scatter(iterates[:iter+1,0], iterates[:iter+1,1], c=range(iter+1), s=35, cmap=cm, label='iterates') - plt.colorbar(axs) + cm = plt.cm.get_cmap('RdYlBu') + axs = plt.scatter(x_iterates[:iter+1, 0, 0], x_iterates[:iter+1, 0, 1], c=range(iter+1), s=35, cmap=cm, label='iterates') + plt.colorbar(axs) ts = np.linspace(0,2*np.pi,100) plt.plot(1 * np.cos(ts)+0,1 * np.sin(ts)-0, 'r') diff --git a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py index f208fff186..66346fadc4 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py @@ -40,23 +40,19 @@ def export_parametric_ocp() -> AcadosOcp: model = AcadosModel() model.x = ca.SX.sym("x", 1) model.p_global = ca.SX.sym("p_global", 1) - model.disc_dyn_expr = model.x - model.cost_expr_ext_cost = (model.x - model.p_global**2)**2 - model.cost_expr_ext_cost_e = 0 + model.cost_expr_ext_cost_e = (model.x - model.p_global**2)**2 model.name = "non_ocp" ocp = AcadosOcp() ocp.model = model - ocp.constraints.lbx_0 = np.array([-1.0]) - ocp.constraints.ubx_0 = np.array([1.0]) - ocp.constraints.idxbx_0 = np.array([0]) + ocp.constraints.lbx_e = np.array([-1.0]) + ocp.constraints.ubx_e = np.array([1.0]) + ocp.constraints.idxbx_e = np.array([0]) - ocp.cost.cost_type = "EXTERNAL" - ocp.solver_options.integrator_type = "DISCRETE" + ocp.cost.cost_type_e = "EXTERNAL" ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" ocp.solver_options.hessian_approx = "EXACT" - ocp.solver_options.N_horizon = 1 - ocp.solver_options.tf = 1.0 + ocp.solver_options.N_horizon = 0 ocp.p_global_values = np.zeros((1,)) ocp.solver_options.with_solution_sens_wrt_params = True @@ -92,7 +88,7 @@ def solve_and_compute_sens(p_test, tau): # breakpoint() # Calculate the policy gradient - out_dict = ocp_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=True) + out_dict = ocp_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=True, return_sens_u=False) sens_x[i] = out_dict['sens_x'].item() return solution, sens_x diff --git a/external/hpipm b/external/hpipm index e0f5466418..aebf3bcfb4 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit e0f5466418db49a44141b0afc4c58938ec8fb67d +Subproject commit aebf3bcfb4082a7b0d0c20ccea2f8a7dc00049ff diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 79b638c958..e958c4b740 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -91,99 +91,13 @@ end end - function make_consistent(self, is_mocp_phase) - if nargin < 2 - is_mocp_phase = false; - end - self.model.make_consistent(self.dims); - - model = self.model; - dims = self.dims; + function make_consistent_cost_initial(self) cost = self.cost; - constraints = self.constraints; - opts = self.solver_options; - - N = opts.N_horizon; - self.detect_cost_and_constraints(); - - % check if nx != nx_next - if ~is_mocp_phase && dims.nx ~= dims.nx_next && opts.N_horizon > 1 - error(['nx_next = ', num2str(dims.nx_next), ' must be equal to nx = ', num2str(dims.nx), ' if more than one shooting interval is used.']); - end - - % detect GNSF structure - if strcmp(opts.integrator_type, 'GNSF') - if dims.gnsf_nx1 + dims.gnsf_nx2 ~= dims.nx - % TODO: properly interface those. - gnsf_transcription_opts = struct(); - detect_gnsf_structure(model, dims, gnsf_transcription_opts); - else - warning('No GNSF model detected, assuming all required fields are set.') - end - end - - % sanity checks on options, which are done in setters in Python - qp_solvers = {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'}; - if ~ismember(opts.qp_solver, qp_solvers) - error(['Invalid qp_solver: ', opts.qp_solver, '. Available options are: ', strjoin(qp_solvers, ', ')]); - end - - regularize_methods = {'NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'}; - if ~ismember(opts.regularize_method, regularize_methods) - error(['Invalid regularize_method: ', opts.regularize_method, '. Available options are: ', strjoin(regularize_methods, ', ')]); - end - hpipm_modes = {'BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'}; - if ~ismember(opts.hpipm_mode, hpipm_modes) - error(['Invalid hpipm_mode: ', opts.hpipm_mode, '. Available options are: ', strjoin(hpipm_modes, ', ')]); - end - INTEGRATOR_TYPES = {'ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'}; - if ~ismember(opts.integrator_type, INTEGRATOR_TYPES) - error(['Invalid integrator_type: ', opts.integrator_type, '. Available options are: ', strjoin(INTEGRATOR_TYPES, ', ')]); - end - - COLLOCATION_TYPES = {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}; - if ~ismember(opts.collocation_type, COLLOCATION_TYPES) - error(['Invalid collocation_type: ', opts.collocation_type, '. Available options are: ', strjoin(COLLOCATION_TYPES, ', ')]); - end - - COST_DISCRETIZATION_TYPES = {'EULER', 'INTEGRATOR'}; - if ~ismember(opts.cost_discretization, COST_DISCRETIZATION_TYPES) - error(['Invalid cost_discretization: ', opts.cost_discretization, '. Available options are: ', strjoin(COST_DISCRETIZATION_TYPES, ', ')]); - end - - search_direction_modes = {'NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP'}; - if ~ismember(opts.search_direction_mode, search_direction_modes) - error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); - end - - % OCP name - self.name = model.name; - - % parameters - if isempty(self.parameter_values) - if dims.np > 0 - warning(['self.parameter_values are not set.', ... - 10 'Using zeros(np,1) by default.' 10 'You can update them later using set().']); - end - self.parameter_values = zeros(self.dims.np,1); - elseif length(self.parameter_values) ~= self.dims.np - error(['parameter_values has the wrong shape. Expected: ' num2str(self.dims.np)]) - end - - - % parameters - if isempty(self.p_global_values) - if dims.np_global > 0 - warning(['self.p_global_values are not set.', ... - 10 'Using zeros(np_global,1) by default.' 10 'You can update them later using set().']); - end - self.p_global_values = zeros(self.dims.np_global,1); - elseif length(self.p_global_values) ~= self.dims.np_global - error(['p_global_values has the wrong shape. Expected: ' num2str(self.dims.np_global)]) + dims = self.dims; + model = self.model; + if self.solver_options.N_horizon == 0 + return end - - %% cost - % initial if strcmp(cost.cost_type_0, 'LINEAR_LS') if ~isempty(cost.W_0) && ~isempty(cost.Vx_0) && ~isempty(cost.Vu_0) ny = length(cost.W_0); @@ -214,8 +128,15 @@ function make_consistent(self, is_mocp_phase) end dims.ny_0 = ny; end + end - % path + function make_consistent_cost_path(self) + cost = self.cost; + dims = self.dims; + model = self.model; + if self.solver_options.N_horizon == 0 + return + end if strcmp(cost.cost_type, 'LINEAR_LS') if ~isempty(cost.W) && ~isempty(cost.Vx) && ~isempty(cost.Vu) ny = length(cost.W); @@ -245,8 +166,13 @@ function make_consistent(self, is_mocp_phase) end dims.ny = ny; end + end + + function make_consistent_cost_terminal(self) + cost = self.cost; + dims = self.dims; + model = self.model; - % terminal if strcmp(cost.cost_type_e, 'LINEAR_LS') if ~isempty(cost.W_e) && ~isempty(cost.Vx_e) ny_e = length(cost.W_e); @@ -279,20 +205,16 @@ function make_consistent(self, is_mocp_phase) end dims.ny_e = ny_e; end + end - % cost integration - if strcmp(opts.cost_discretization, "INTEGRATOR") - if ~(strcmp(cost.cost_type, "NONLINEAR_LS") || strcmp(cost.cost_type, "CONVEX_OVER_NONLINEAR")) - error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for path cost.') - end - if ~(strcmp(cost.cost_type_0, "NONLINEAR_LS") || strcmp(cost.cost_type_0, "CONVEX_OVER_NONLINEAR")) - error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for initial cost.') - end + function make_consistent_constraints_initial(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; + if self.solver_options.N_horizon == 0 + return end - - %% constraints - % initial if ~isempty(constraints.idxbx_0) && ~isempty(constraints.lbx_0) && ~isempty(constraints.ubx_0) nbx_0 = length(constraints.lbx_0); if nbx_0 ~= length(constraints.ubx_0) || nbx_0 ~= length(constraints.idxbx_0) @@ -312,7 +234,29 @@ function make_consistent(self, is_mocp_phase) dims.nbxe_0 = length(constraints.idxbxe_0); - % path + if ~isempty(model.con_h_expr_0) && ... + ~isempty(constraints.lh_0) && ~isempty(constraints.uh_0) + nh_0 = length(constraints.lh_0); + if nh_0 ~= length(constraints.uh_0) || nh_0 ~= length(model.con_h_expr_0) + error('inconsistent dimension nh_0, regarding expr_h_0, lh_0, uh_0.'); + end + elseif ~isempty(model.con_h_expr_0) || ... + ~isempty(constraints.lh_0) || ~isempty(constraints.uh_0) + error('setting external constraint function h: need expr_h_0, lh_0, uh_0 at least one missing.'); + else + nh_0 = 0; + end + dims.nh_0 = nh_0; + end + + function make_consistent_constraints_path(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; + if self.solver_options.N_horizon == 0 + return + end + if ~isempty(constraints.idxbx) && ~isempty(constraints.lbx) && ~isempty(constraints.ubx) nbx = length(constraints.lbx); if nbx ~= length(constraints.ubx) || nbx ~= length(constraints.idxbx) @@ -358,7 +302,7 @@ function make_consistent(self, is_mocp_phase) dims.ng = ng; if ~isempty(model.con_h_expr) && ... - ~isempty(constraints.lh) && ~isempty(constraints.uh) + ~isempty(constraints.lh) && ~isempty(constraints.uh) nh = length(constraints.lh); if nh ~= length(constraints.uh) || nh ~= length(model.con_h_expr) error('inconsistent dimension nh, regarding expr_h, lh, uh.'); @@ -370,22 +314,13 @@ function make_consistent(self, is_mocp_phase) nh = 0; end dims.nh = nh; + end - if ~isempty(model.con_h_expr_0) && ... - ~isempty(constraints.lh_0) && ~isempty(constraints.uh_0) - nh_0 = length(constraints.lh_0); - if nh_0 ~= length(constraints.uh_0) || nh_0 ~= length(model.con_h_expr_0) - error('inconsistent dimension nh_0, regarding expr_h_0, lh_0, uh_0.'); - end - elseif ~isempty(model.con_h_expr_0) || ... - ~isempty(constraints.lh_0) || ~isempty(constraints.uh_0) - error('setting external constraint function h: need expr_h_0, lh_0, uh_0 at least one missing.'); - else - nh_0 = 0; - end - dims.nh_0 = nh_0; + function make_consistent_constraints_terminal(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; - % terminal if ~isempty(constraints.idxbx_e) && ~isempty(constraints.lbx_e) && ~isempty(constraints.ubx_e) nbx_e = length(constraints.lbx_e); if nbx_e ~= length(constraints.ubx_e) || nbx_e ~= length(constraints.idxbx_e) @@ -416,7 +351,7 @@ function make_consistent(self, is_mocp_phase) dims.ng_e = ng_e; if ~isempty(model.con_h_expr_e) && ... - ~isempty(constraints.lh_e) && ~isempty(constraints.uh_e) + ~isempty(constraints.lh_e) && ~isempty(constraints.uh_e) nh_e = length(constraints.lh_e); if nh_e ~= length(constraints.uh_e) || nh_e ~= length(model.con_h_expr_e) error('inconsistent dimension nh_e, regarding expr_h_e, lh_e, uh_e.'); @@ -428,8 +363,16 @@ function make_consistent(self, is_mocp_phase) nh_e = 0; end dims.nh_e = nh_e; + end + + function make_consistent_slack_dimensions_path(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + if self.solver_options.N_horizon == 0 + return + end - %% slack dimensions nsbx = length(constraints.idxsbx); nsbu = length(constraints.idxsbu); nsg = length(constraints.idxsg); @@ -481,8 +424,18 @@ function make_consistent(self, is_mocp_phase) dims.nsg = nsg; dims.nsh = nsh; dims.nsphi = nsphi; + end + + function make_consistent_slack_dimensions_initial(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + nsbu = dims.nsbu; + nsg = dims.nsg; + if self.solver_options.N_horizon == 0 + return + end - % slacks at initial stage nsh_0 = length(constraints.idxsh_0); nsphi_0 = length(constraints.idxsphi_0); @@ -522,8 +475,13 @@ function make_consistent(self, is_mocp_phase) dims.ns_0 = ns_0; dims.nsh_0 = nsh_0; dims.nsphi_0 = nsphi_0; + end - %% terminal slack dimensions + function make_consistent_slack_dimensions_terminal(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + nsbx_e = length(constraints.idxsbx_e); nsg_e = length(constraints.idxsg_e); nsh_e = length(constraints.idxsh_e); @@ -572,46 +530,27 @@ function make_consistent(self, is_mocp_phase) dims.nsh_e = nsh_e; dims.nsphi_e = nsphi_e; - % check for ACADOS_INFTY - if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) - ACADOS_INFTY = get_acados_infty(); - % loop over all bound vectors - fields = {'lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'}; - for i = 1:length(fields) - field = fields{i}; - bound = constraints.(field); - if any(bound >= ACADOS_INFTY) || any(bound <= -ACADOS_INFTY) - error(['Field ', field, ' contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = ', num2str(ACADOS_INFTY, '%.2e'), '. One-sided constraints are not supported by the chosen QP solver ', opts.qp_solver, '.']); - end - end - end + end - % shooting nodes -> time_steps - % discretization - if isempty(opts.N_horizon) && isempty(dims.N) - error('N_horizon not provided.'); - elseif isempty(opts.N_horizon) && ~isempty(dims.N) - opts.N_horizon = dims.N; - disp(['field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon.',... - ' setting AcadosOcpOptions.N_horizon = N.',... - ' For future comppatibility, please use AcadosOcpOptions.N_horizon directly.']); - elseif ~isempty(opts.N_horizon) && ~isempty(dims.N) && opts.N_horizon ~= dims.N - error(['Inconsistent dimension N, regarding N = ', num2str(dims.N),... - ', N_horizon = ', num2str(opts.N_horizon), '.']); - else - dims.N = opts.N_horizon; + function make_consistent_discretization(self) + dims = self.dims; + opts = self.solver_options; + + if opts.N_horizon == 0 + opts.shooting_nodes = zeros(1, 1); + opts.time_steps = ones(1, 1); + return end - N = opts.N_horizon; if length(opts.tf) ~= 1 || opts.tf < 0 error('time horizon tf should be a nonnegative number'); end if ~isempty(opts.shooting_nodes) - if N + 1 ~= length(opts.shooting_nodes) + if opts.N_horizon + 1 ~= length(opts.shooting_nodes) error('inconsistent dimension N regarding shooting nodes.'); end - for i=1:N + for i=1:opts.N_horizon opts.time_steps(i) = opts.shooting_nodes(i+1) - opts.shooting_nodes(i); end sum_time_steps = sum(opts.time_steps); @@ -620,7 +559,7 @@ function make_consistent(self, is_mocp_phase) opts.time_steps = opts.time_steps * opts.tf / sum_time_steps; end elseif ~isempty(opts.time_steps) - if N ~= length(opts.time_steps) + if opts.N_horizon ~= length(opts.time_steps) error('inconsistent dimension N regarding time steps.'); end sum_time_steps = sum(opts.time_steps); @@ -629,12 +568,12 @@ function make_consistent(self, is_mocp_phase) 'got tf = ' num2str(opts.tf) '; sum(time_steps) = ' num2str(sum_time_steps) '.']); end else - opts.time_steps = opts.tf/N * ones(N,1); + opts.time_steps = opts.tf/opts.N_horizon * ones(opts.N_horizon,1); end % add consistent shooting_nodes e.g. for plotting; if isempty(opts.shooting_nodes) - opts.shooting_nodes = zeros(N+1, 1); - for i = 1:N + opts.shooting_nodes = zeros(opts.N_horizon+1, 1); + for i = 1:opts.N_horizon opts.shooting_nodes(i+1) = sum(opts.time_steps(1:i)); end end @@ -642,12 +581,12 @@ function make_consistent(self, is_mocp_phase) error(['ocp discretization: time_steps between shooting nodes must all be > 0', ... ' got: ' num2str(opts.time_steps)]) end + end - % cost_scaling - if isempty(opts.cost_scaling) - opts.cost_scaling = [opts.time_steps(:); 1.0]; - elseif length(opts.cost_scaling) ~= N+1 - error(['cost_scaling must have length N+1 = ', num2str(N+1)]); + function make_consistent_simulation(self) + opts = self.solver_options + if opts.N_horizon == 0 + return end % set integrator time automatically @@ -662,29 +601,191 @@ function make_consistent(self, is_mocp_phase) end end - % qpdunes - if ~isempty(strfind(opts.qp_solver,'qpdunes')) - constraints.idxbxe_0 = []; - dims.nbxe_0 = 0; - end - %% options sanity checks if length(opts.sim_method_num_steps) == 1 - opts.sim_method_num_steps = opts.sim_method_num_steps * ones(1, N); - elseif length(opts.sim_method_num_steps) ~= N + opts.sim_method_num_steps = opts.sim_method_num_steps * ones(1, opts.N_horizon); + elseif length(opts.sim_method_num_steps) ~= opts.N_horizon error('sim_method_num_steps must be a scalar or a vector of length N'); end if length(opts.sim_method_num_stages) == 1 - opts.sim_method_num_stages = opts.sim_method_num_stages * ones(1, N); - elseif length(opts.sim_method_num_stages) ~= N + opts.sim_method_num_stages = opts.sim_method_num_stages * ones(1, opts.N_horizon); + elseif length(opts.sim_method_num_stages) ~= opts.N_horizon error('sim_method_num_stages must be a scalar or a vector of length N'); end if length(opts.sim_method_jac_reuse) == 1 - opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * ones(1, N); - elseif length(opts.sim_method_jac_reuse) ~= N + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * ones(1, opts.N_horizon); + elseif length(opts.sim_method_jac_reuse) ~= opts.N_horizon error('sim_method_jac_reuse must be a scalar or a vector of length N'); end + + end + + function make_consistent(self, is_mocp_phase) + if nargin < 2 + is_mocp_phase = false; + end + self.model.make_consistent(self.dims); + + model = self.model; + dims = self.dims; + cost = self.cost; + constraints = self.constraints; + opts = self.solver_options; + + self.detect_cost_and_constraints(); + + if isempty(opts.N_horizon) && isempty(dims.N) + error('N_horizon not provided.'); + elseif isempty(opts.N_horizon) && ~isempty(dims.N) + opts.N_horizon = dims.N; + disp(['field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon.',... + ' setting AcadosOcpOptions.N_horizon = N.',... + ' For future comppatibility, please use AcadosOcpOptions.N_horizon directly.']); + elseif ~isempty(opts.N_horizon) && ~isempty(dims.N) && opts.N_horizon ~= dims.N + error(['Inconsistent dimension N, regarding N = ', num2str(dims.N),... + ', N_horizon = ', num2str(opts.N_horizon), '.']); + else + dims.N = opts.N_horizon; + end + + % check if nx != nx_next + if ~is_mocp_phase && dims.nx ~= dims.nx_next && opts.N_horizon > 1 + error(['nx_next = ', num2str(dims.nx_next), ' must be equal to nx = ', num2str(dims.nx), ' if more than one shooting interval is used.']); + end + + % detect GNSF structure + if strcmp(opts.integrator_type, 'GNSF') && opts.N_horizon > 0 + if dims.gnsf_nx1 + dims.gnsf_nx2 ~= dims.nx + % TODO: properly interface those. + gnsf_transcription_opts = struct(); + detect_gnsf_structure(model, dims, gnsf_transcription_opts); + else + warning('No GNSF model detected, assuming all required fields are set.') + end + end + + % sanity checks on options, which are done in setters in Python + qp_solvers = {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'}; + if ~ismember(opts.qp_solver, qp_solvers) + error(['Invalid qp_solver: ', opts.qp_solver, '. Available options are: ', strjoin(qp_solvers, ', ')]); + end + + regularize_methods = {'NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'}; + if ~ismember(opts.regularize_method, regularize_methods) + error(['Invalid regularize_method: ', opts.regularize_method, '. Available options are: ', strjoin(regularize_methods, ', ')]); + end + hpipm_modes = {'BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'}; + if ~ismember(opts.hpipm_mode, hpipm_modes) + error(['Invalid hpipm_mode: ', opts.hpipm_mode, '. Available options are: ', strjoin(hpipm_modes, ', ')]); + end + INTEGRATOR_TYPES = {'ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'}; + if ~ismember(opts.integrator_type, INTEGRATOR_TYPES) + error(['Invalid integrator_type: ', opts.integrator_type, '. Available options are: ', strjoin(INTEGRATOR_TYPES, ', ')]); + end + + COLLOCATION_TYPES = {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}; + if ~ismember(opts.collocation_type, COLLOCATION_TYPES) + error(['Invalid collocation_type: ', opts.collocation_type, '. Available options are: ', strjoin(COLLOCATION_TYPES, ', ')]); + end + + COST_DISCRETIZATION_TYPES = {'EULER', 'INTEGRATOR'}; + if ~ismember(opts.cost_discretization, COST_DISCRETIZATION_TYPES) + error(['Invalid cost_discretization: ', opts.cost_discretization, '. Available options are: ', strjoin(COST_DISCRETIZATION_TYPES, ', ')]); + end + + search_direction_modes = {'NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP'}; + if ~ismember(opts.search_direction_mode, search_direction_modes) + error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); + end + + % OCP name + self.name = model.name; + + % parameters + if isempty(self.parameter_values) + if dims.np > 0 + warning(['self.parameter_values are not set.', ... + 10 'Using zeros(np,1) by default.' 10 'You can update them later using set().']); + end + self.parameter_values = zeros(self.dims.np,1); + elseif length(self.parameter_values) ~= self.dims.np + error(['parameter_values has the wrong shape. Expected: ' num2str(self.dims.np)]) + end + + + % parameters + if isempty(self.p_global_values) + if dims.np_global > 0 + warning(['self.p_global_values are not set.', ... + 10 'Using zeros(np_global,1) by default.' 10 'You can update them later using set().']); + end + self.p_global_values = zeros(self.dims.np_global,1); + elseif length(self.p_global_values) ~= self.dims.np_global + error(['p_global_values has the wrong shape. Expected: ' num2str(self.dims.np_global)]) + end + + %% cost + self.make_consistent_cost_initial(); + self.make_consistent_cost_path(); + self.make_consistent_cost_terminal(); + + % cost integration + if strcmp(opts.cost_discretization, "INTEGRATOR") && opts.N_horizon > 0 + if ~(strcmp(cost.cost_type, "NONLINEAR_LS") || strcmp(cost.cost_type, "CONVEX_OVER_NONLINEAR")) + error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for path cost.') + end + if ~(strcmp(cost.cost_type_0, "NONLINEAR_LS") || strcmp(cost.cost_type_0, "CONVEX_OVER_NONLINEAR")) + error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for initial cost.') + end + end + + + %% constraints + self.make_consistent_constraints_initial(); + self.make_consistent_constraints_path(); + self.make_consistent_constraints_terminal(); + + %% slack dimensions + self.make_consistent_slack_dimensions_path(); + self.make_consistent_slack_dimensions_initial(); + self.make_consistent_slack_dimensions_terminal(); + + % check for ACADOS_INFTY + if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) + ACADOS_INFTY = get_acados_infty(); + % loop over all bound vectors + if opts.N_horizon > 0 + fields = {'lbx_e', 'ubx_e', 'lg_e', 'ug_e', 'lh_e', 'uh_e', 'lphi_e', 'uphi_e'}; + else + fields = {'lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'}; + end + for i = 1:length(fields) + field = fields{i}; + bound = constraints.(field); + if any(bound >= ACADOS_INFTY) || any(bound <= -ACADOS_INFTY) + error(['Field ', field, ' contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = ', num2str(ACADOS_INFTY, '%.2e'), '. One-sided constraints are not supported by the chosen QP solver ', opts.qp_solver, '.']); + end + end + end + + self.make_consistent_discretization(); + + % cost_scaling + if isempty(opts.cost_scaling) + opts.cost_scaling = [opts.time_steps(:); 1.0]; + elseif length(opts.cost_scaling) ~= opts.N_horizon+1 + error(['cost_scaling must have length N+1 = ', num2str(N+1)]); + end + + self.make_consistent_simulation(); + + % qpdunes + if ~isempty(strfind(opts.qp_solver,'qpdunes')) + constraints.idxbxe_0 = []; + dims.nbxe_0 = 0; + end + if strcmp(opts.qp_solver, "PARTIAL_CONDENSING_HPMPC") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_QPDUNES") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_OOQP") @@ -704,8 +805,14 @@ function make_consistent(self, is_mocp_phase) if opts.hessian_approx == 'EXACT' error('fixed_hess and hessian_approx = EXACT are incompatible') end - if ~(strcmp(cost.cost_type_0, "LINEAR_LS") && strcmp(cost.cost_type, "LINEAR_LS") && strcmp(cost.cost_type_e, "LINEAR_LS")) - error('fixed_hess requires LINEAR_LS cost type') + if ~strcmp(cost.cost_type_0, "LINEAR_LS") && opts.N_horizon > 0 + error('fixed_hess requires LINEAR_LS cost_type_0') + end + if ~strcmp(cost.cost_type, "LINEAR_LS") && opts.N_horizon > 0 + error('fixed_hess requires LINEAR_LS cost_type') + end + if ~strcmp(cost.cost_type_e, "LINEAR_LS") + error('fixed_hess requires LINEAR_LS cost_type_e') end end @@ -713,11 +820,14 @@ function make_consistent(self, is_mocp_phase) % check if qp_solver_cond_N is set if isempty(opts.qp_solver_cond_N) - opts.qp_solver_cond_N = N; + opts.qp_solver_cond_N = opts.N_horizon; + end + if opts.qp_solver_cond_N > opts.N_horizon + error('qp_solver_cond_N > N_horizon is not supported.'); end if ~isempty(opts.qp_solver_cond_block_size) - if sum(opts.qp_solver_cond_block_size) ~= N + if sum(opts.qp_solver_cond_block_size) ~= opts.N_horizon error(['sum(qp_solver_cond_block_size) =', num2str(sum(opts.qp_solver_cond_block_size)), ' != N = {opts.N_horizon}.']); end if length(opts.qp_solver_cond_block_size) ~= opts.qp_solver_cond_N+1 @@ -726,8 +836,11 @@ function make_consistent(self, is_mocp_phase) end if strcmp(opts.nlp_solver_type, "DDP") + if opts.N_horizon == 0 + error('DDP solver only supported for N_horizon > 0.'); + end if ~strcmp(opts.qp_solver, "PARTIAL_CONDENSING_HPIPM") || (opts.qp_solver_cond_N ~= opts.N_horizon) - error('DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.'); + error('DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N_horizon.'); end if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]) error('DDP only supports initial state constraints, got path constraints.') @@ -745,9 +858,14 @@ function make_consistent(self, is_mocp_phase) error('tau_min > 0 is only compatible with HPIPM.'); end - if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any([strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]) + if opts.N_horizon == 0 + cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + else + cost_types_to_check = [strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + end + if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any(cost_types_to_check) error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); end @@ -861,12 +979,26 @@ function make_consistent(self, is_mocp_phase) end if isa(self.zoro_description, 'ZoroDescription') + if opts.N_horizon == 0 + error('ZORO only supported for N_horizon > 0.'); + end self.zoro_description.process(); end end function [] = detect_cost_and_constraints(self) % detect cost type + N = self.solver_options.N_horizon + if N == 0 + if strcmp(self.cost.cost_type_e, 'AUTO') + detect_cost_type(self.model, self.cost, self.dims, 'terminal'); + end + if strcmp(self.constraints.constr_type_e, 'AUTO') + detect_constraint_structure(self.model, self.constraints, 'terminal'); + end + return + end + stage_types = {'initial', 'path', 'terminal'}; cost_types = {self.cost.cost_type_0, self.cost.cost_type, self.cost.cost_type_e}; @@ -941,44 +1073,20 @@ function make_consistent(self, is_mocp_phase) cost = ocp.cost; dims = ocp.dims; - % dynamics - model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [ocp.name '_model']); + setup_code_generation_context_dynamics(ocp, context); - if strcmp(ocp.model.dyn_ext_fun_type, 'generic') - check_dir_and_create(model_dir); - copyfile(fullfile(pwd, ocp.model.dyn_generic_source), model_dir); - context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); - elseif strcmp(ocp.model.dyn_ext_fun_type, 'casadi') - check_casadi_version(); - switch solver_opts.integrator_type - case 'ERK' - generate_c_code_explicit_ode(context, ocp.model, model_dir); - case 'IRK' - generate_c_code_implicit_ode(context, ocp.model, model_dir); - case 'LIFTED_IRK' - if ~(isempty(ocp.model.t) || length(ocp.model.t) == 0) - error('NOT LIFTED_IRK with time-varying dynamics not implemented yet.') - end - generate_c_code_implicit_ode(context, ocp.model, model_dir); - case 'GNSF' - generate_c_code_gnsf(context, ocp.model, model_dir); - case 'DISCRETE' - generate_c_code_discrete_dynamics(context, ocp.model, model_dir); - otherwise - error('Unknown integrator type.') - end - else - error('Unknown dyn_ext_fun_type.') - end - - if ignore_initial && ignore_terminal - stage_type_indices = [2]; - elseif ignore_terminal - stage_type_indices = [1, 2]; - elseif ignore_initial - stage_type_indices = [2, 3]; + if solver_opts.N_horizon == 0 + stage_type_indices = [3]; else - stage_type_indices = [1, 2, 3]; + if ignore_initial && ignore_terminal + stage_type_indices = [2]; + elseif ignore_terminal + stage_type_indices = [1, 2]; + elseif ignore_initial + stage_type_indices = [2, 3]; + else + stage_type_indices = [1, 2, 3]; + end end stage_types = {'initial', 'path', 'terminal'}; @@ -1030,6 +1138,43 @@ function make_consistent(self, is_mocp_phase) end end + function setup_code_generation_context_dynamics(ocp, context) + code_gen_opts = context.opts; + solver_opts = ocp.solver_options; + if solver_opts.N_horizon == 0 + return + end + + model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [ocp.name '_model']); + + if strcmp(ocp.model.dyn_ext_fun_type, 'generic') + check_dir_and_create(model_dir); + copyfile(fullfile(pwd, ocp.model.dyn_generic_source), model_dir); + context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); + elseif strcmp(ocp.model.dyn_ext_fun_type, 'casadi') + check_casadi_version(); + switch solver_opts.integrator_type + case 'ERK' + generate_c_code_explicit_ode(context, ocp.model, model_dir); + case 'IRK' + generate_c_code_implicit_ode(context, ocp.model, model_dir); + case 'LIFTED_IRK' + if ~(isempty(ocp.model.t) || length(ocp.model.t) == 0) + error('NOT LIFTED_IRK with time-varying dynamics not implemented yet.') + end + generate_c_code_implicit_ode(context, ocp.model, model_dir); + case 'GNSF' + generate_c_code_gnsf(context, ocp.model, model_dir); + case 'DISCRETE' + generate_c_code_discrete_dynamics(context, ocp.model, model_dir); + otherwise + error('Unknown integrator type.') + end + else + error('Unknown dyn_ext_fun_type.') + end + end + function render_templates(self) %% render templates diff --git a/interfaces/acados_template/acados_template/acados_dims.py b/interfaces/acados_template/acados_template/acados_dims.py index 1ca68b8103..9b6056d2a8 100644 --- a/interfaces/acados_template/acados_template/acados_dims.py +++ b/interfaces/acados_template/acados_template/acados_dims.py @@ -597,5 +597,5 @@ def ng_e(self, ng_e): @N.setter def N(self, N): - check_int_value("N", N, positive=True) + check_int_value("N", N, nonnegative=True) self.__N = N diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 2be43613e5..85c3228d20 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -163,35 +163,16 @@ def json_file(self): def json_file(self, json_file): self.__json_file = json_file - def make_consistent(self, is_mocp_phase=False) -> None: - """ - Detect dimensions, perform sanity checks - """ + + def _make_consistent_cost_initial(self): dims = self.dims cost = self.cost - constraints = self.constraints model = self.model opts = self.solver_options + if opts.N_horizon == 0: + return - model.make_consistent(dims) - self.name = model.name - - # check if nx != nx_next - if not is_mocp_phase and dims.nx != dims.nx_next and opts.N_horizon > 1: - raise ValueError('nx_next should be equal to nx if more than one shooting interval is used.') - - # parameters - if self.parameter_values.shape[0] != dims.np: - raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ - f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n') - - # p_global_values - if self.p_global_values.shape[0] != dims.np_global: - raise ValueError('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ - f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n') - - ## cost - # initial stage - if not set, copy fields from path constraints + # if not set, copy fields from path constraints if cost.cost_type_0 is None: self.copy_path_cost_to_stage_0() @@ -259,26 +240,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: if model.cost_expr_ext_cost_custom_hess_0.shape != (dims.nx+dims.nu, dims.nx+dims.nu): raise ValueError('cost_expr_ext_cost_custom_hess_0 should have shape (nx+nu, nx+nu).') - # GN check - gn_warning_0 = (cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) - gn_warning_path = (cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) - gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) - if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): - external_cost_types = [] - if gn_warning_0: - external_cost_types.append('cost_type_0') - if gn_warning_path: - external_cost_types.append('cost_type') - if gn_warning_terminal: - external_cost_types.append('cost_type_e') - print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" - f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" - "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" - "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" - "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" - "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") - # path + def _make_consistent_cost_path(self): + dims = self.dims + cost = self.cost + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + if cost.cost_type == 'AUTO': self.detect_cost_type(model, cost, dims, "path") @@ -342,7 +312,14 @@ def make_consistent(self, is_mocp_phase=False) -> None: if not is_empty(model.cost_expr_ext_cost_custom_hess): if model.cost_expr_ext_cost_custom_hess.shape != (dims.nx+dims.nu, dims.nx+dims.nu): raise ValueError('cost_expr_ext_cost_custom_hess should have shape (nx+nu, nx+nu).') - # terminal + + + def _make_consistent_cost_terminal(self): + dims = self.dims + cost = self.cost + model = self.model + opts = self.solver_options + if cost.cost_type_e == 'AUTO': self.detect_cost_type(model, cost, dims, "terminal") @@ -401,14 +378,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: if model.cost_expr_ext_cost_custom_hess_e.shape != (dims.nx, dims.nx): raise ValueError('cost_expr_ext_cost_custom_hess_e should have shape (nx, nx).') - # cost integration - supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] - if opts.cost_discretization == 'INTEGRATOR' and \ - any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): - raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') - ## constraints - # initial + def _make_consistent_constraints_initial(self): + constraints = self.constraints + dims = self.dims + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + nbx_0 = constraints.idxbx_0.shape[0] if constraints.ubx_0.shape[0] != nbx_0 or constraints.lbx_0.shape[0] != nbx_0: raise ValueError('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') @@ -447,7 +425,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: dims.nr_0 = casadi_length(model.con_r_expr_0) - # path + + def _make_consistent_constraints_path(self): + constraints = self.constraints + dims = self.dims + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + nbx = constraints.idxbx.shape[0] if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx: raise ValueError('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') @@ -500,7 +486,11 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.nr = casadi_length(model.con_r_expr) - # terminal + def _make_consistent_constraints_terminal(self): + dims = self.dims + constraints = self.constraints + model = self.model + nbx_e = constraints.idxbx_e.shape[0] if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e: raise ValueError('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') @@ -536,7 +526,101 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: dims.nr_e = casadi_length(model.con_r_expr_e) - # Slack dimensions + + def _make_consistent_slacks_initial(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + if opts.N_horizon == 0: + return + + nh_0 = dims.nh_0 + nsbu = dims.nsbu + nsg = dims.nsg + ns = dims.ns + nsh_0 = constraints.idxsh_0.shape[0] + if nsh_0 > nh_0: + raise ValueError(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') + if any(constraints.idxsh_0 >= nh_0): + raise ValueError(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') + if is_empty(constraints.lsh_0): + constraints.lsh_0 = np.zeros((nsh_0,)) + elif constraints.lsh_0.shape[0] != nsh_0: + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') + if is_empty(constraints.ush_0): + constraints.ush_0 = np.zeros((nsh_0,)) + elif constraints.ush_0.shape[0] != nsh_0: + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') + dims.nsh_0 = nsh_0 + + nsphi_0 = constraints.idxsphi_0.shape[0] + if nsphi_0 > dims.nphi_0: + raise ValueError(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') + if any(constraints.idxsphi_0 >= dims.nphi_0): + raise ValueError(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') + if is_empty(constraints.lsphi_0): + constraints.lsphi_0 = np.zeros((nsphi_0,)) + elif constraints.lsphi_0.shape[0] != nsphi_0: + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') + if is_empty(constraints.usphi_0): + constraints.usphi_0 = np.zeros((nsphi_0,)) + elif constraints.usphi_0.shape[0] != nsphi_0: + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') + dims.nsphi_0 = nsphi_0 + + # Note: at stage 0 bounds on x are not slacked! + ns_0 = nsbu + nsg + nsphi_0 + nsh_0 # NOTE: nsbx not supported at stage 0 + + if cost.zl_0 is None and cost.zu_0 is None and cost.Zl_0 is None and cost.Zu_0 is None: + if ns_0 == 0: + cost.zl_0 = np.array([]) + cost.zu_0 = np.array([]) + cost.Zl_0 = np.array([]) + cost.Zu_0 = np.array([]) + elif ns_0 == ns: + cost.zl_0 = cost.zl + cost.zu_0 = cost.zu + cost.Zl_0 = cost.Zl + cost.Zu_0 = cost.Zu + print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") + print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") + else: + raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") + + wrong_fields = [] + if cost.Zl_0.shape[0] != ns_0: + wrong_fields += ["Zl_0"] + dim = cost.Zl_0.shape[0] + elif cost.Zu_0.shape[0] != ns_0: + wrong_fields += ["Zu_0"] + dim = cost.Zu_0.shape[0] + elif cost.zl_0.shape[0] != ns_0: + wrong_fields += ["zl_0"] + dim = cost.zl_0.shape[0] + elif cost.zu_0.shape[0] != ns_0: + wrong_fields += ["zu_0"] + dim = cost.zu_0.shape[0] + + if wrong_fields != []: + raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') + dims.ns_0 = ns_0 + + + def _make_consistent_slacks_path(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + if opts.N_horizon == 0: + return + + nbx = dims.nbx + nbu = dims.nbu + nh = dims.nh + ng = dims.ng nsbx = constraints.idxsbx.shape[0] if nsbx > nbx: raise ValueError(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') @@ -633,77 +717,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns - # slack dimensions at initial node - nsh_0 = constraints.idxsh_0.shape[0] - if nsh_0 > nh_0: - raise ValueError(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') - if any(constraints.idxsh_0 >= nh_0): - raise ValueError(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') - if is_empty(constraints.lsh_0): - constraints.lsh_0 = np.zeros((nsh_0,)) - elif constraints.lsh_0.shape[0] != nsh_0: - raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') - if is_empty(constraints.ush_0): - constraints.ush_0 = np.zeros((nsh_0,)) - elif constraints.ush_0.shape[0] != nsh_0: - raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') - dims.nsh_0 = nsh_0 - - nsphi_0 = constraints.idxsphi_0.shape[0] - if nsphi_0 > dims.nphi_0: - raise ValueError(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') - if any(constraints.idxsphi_0 >= dims.nphi_0): - raise ValueError(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') - if is_empty(constraints.lsphi_0): - constraints.lsphi_0 = np.zeros((nsphi_0,)) - elif constraints.lsphi_0.shape[0] != nsphi_0: - raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') - if is_empty(constraints.usphi_0): - constraints.usphi_0 = np.zeros((nsphi_0,)) - elif constraints.usphi_0.shape[0] != nsphi_0: - raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') - dims.nsphi_0 = nsphi_0 - - # Note: at stage 0 bounds on x are not slacked! - ns_0 = nsbu + nsg + nsphi_0 + nsh_0 # NOTE: nsbx not supported at stage 0 - if cost.zl_0 is None and cost.zu_0 is None and cost.Zl_0 is None and cost.Zu_0 is None: - if ns_0 == 0: - cost.zl_0 = np.array([]) - cost.zu_0 = np.array([]) - cost.Zl_0 = np.array([]) - cost.Zu_0 = np.array([]) - elif ns_0 == ns: - cost.zl_0 = cost.zl - cost.zu_0 = cost.zu - cost.Zl_0 = cost.Zl - cost.Zu_0 = cost.Zu - print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") - print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") - else: - raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") - - wrong_fields = [] - if cost.Zl_0.shape[0] != ns_0: - wrong_fields += ["Zl_0"] - dim = cost.Zl_0.shape[0] - elif cost.Zu_0.shape[0] != ns_0: - wrong_fields += ["Zu_0"] - dim = cost.Zu_0.shape[0] - elif cost.zl_0.shape[0] != ns_0: - wrong_fields += ["zl_0"] - dim = cost.zl_0.shape[0] - elif cost.zu_0.shape[0] != ns_0: - wrong_fields += ["zu_0"] - dim = cost.zu_0.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' - + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ - + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') - dims.ns_0 = ns_0 + def _make_consistent_slacks_terminal(self): + constraints = self.constraints + dims = self.dims + cost = self.cost - # slacks at terminal node + nbx_e = dims.nbx_e + nh_e = dims.nh_e + ng_e = dims.ng_e nsbx_e = constraints.idxsbx_e.shape[0] if nsbx_e > nbx_e: raise ValueError(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') @@ -787,24 +809,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.ns_e = ns_e - # check for ACADOS_INFTY - if opts.qp_solver not in ["PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM", "FULL_CONDENSING_DAQP"]: - # loop over all bound vectors - for field in ['lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e']: - bound = getattr(constraints, field) - if any(bound >= ACADOS_INFTY) or any(bound <= -ACADOS_INFTY): - raise ValueError(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") - # discretization - if opts.N_horizon is None and dims.N is None: - raise ValueError('N_horizon not provided.') - elif opts.N_horizon is None and dims.N is not None: - opts.N_horizon = dims.N - print("field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon. setting AcadosOcpOptions.N_horizon = N. For future comppatibility, please use AcadosOcpOptions.N_horizon directly.") - elif opts.N_horizon is not None and dims.N is not None and opts.N_horizon != dims.N: - raise ValueError(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') - else: - dims.N = opts.N_horizon + def _make_consistent_discretization(self): + opts = self.solver_options + if opts.N_horizon == 0: + opts.shooting_nodes = np.array([0.]) + opts.time_steps = np.array([]) + return if not isinstance(opts.tf, (float, int)): raise TypeError(f'Time horizon tf should be float provided, got tf = {opts.tf}.') @@ -841,11 +852,11 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise ValueError(f'Inconsistent discretization: {opts.tf}' f' = tf != sum(opts.time_steps) = {tf}.') - # cost scaling - if opts.cost_scaling is None: - opts.cost_scaling = np.append(opts.time_steps, 1.0) - if opts.cost_scaling.shape[0] != opts.N_horizon + 1: - raise ValueError(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') + + def _make_consistent_simulation(self): + opts = self.solver_options + if opts.N_horizon == 0: + return # set integrator time automatically opts.Tsim = opts.time_steps[0] @@ -886,32 +897,141 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: raise ValueError("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") + + def make_consistent(self, is_mocp_phase=False) -> None: + """ + Detect dimensions, perform sanity checks + """ + dims = self.dims + cost = self.cost + constraints = self.constraints + model = self.model + opts = self.solver_options + + model.make_consistent(dims) + self.name = model.name + + if opts.N_horizon is None and dims.N is None: + raise ValueError('N_horizon not provided.') + elif opts.N_horizon is None and dims.N is not None: + opts.N_horizon = dims.N + print("field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon. setting AcadosOcpOptions.N_horizon = N. For future comppatibility, please use AcadosOcpOptions.N_horizon directly.") + elif opts.N_horizon is not None and dims.N is not None and opts.N_horizon != dims.N: + raise ValueError(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') + else: + dims.N = opts.N_horizon + + # check if nx != nx_next + if not is_mocp_phase and dims.nx != dims.nx_next and opts.N_horizon > 1: + raise ValueError('nx_next should be equal to nx if more than one shooting interval is used.') + + # parameters + if self.parameter_values.shape[0] != dims.np: + raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ + f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n') + + # p_global_values + if self.p_global_values.shape[0] != dims.np_global: + raise ValueError('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ + f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n') + + ## cost + self._make_consistent_cost_initial() + self._make_consistent_cost_path() + self._make_consistent_cost_terminal() + + # GN check + gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) + gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) + gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) + if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): + external_cost_types = [] + if gn_warning_0: + external_cost_types.append('cost_type_0') + if gn_warning_path: + external_cost_types.append('cost_type') + if gn_warning_terminal: + external_cost_types.append('cost_type_e') + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" + f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" + "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" + "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" + "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + + # cost integration + if opts.N_horizon > 0: + supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] + if opts.cost_discretization == 'INTEGRATOR' and \ + any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): + raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') + + ## constraints + self._make_consistent_constraints_initial() + self._make_consistent_constraints_path() + self._make_consistent_constraints_terminal() + + self._make_consistent_slacks_path() + self._make_consistent_slacks_initial() + self._make_consistent_slacks_terminal() + + # check for ACADOS_INFTY + if opts.qp_solver not in ["PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM", "FULL_CONDENSING_DAQP"]: + # loop over all bound vectors + if opts.N_horizon > 0: + fields_to_check = ['lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'] + else: + fields_to_check = ['lbx_0', 'ubx_0', 'lbx_e', 'ubx_e', 'lg_e', 'ug_e', 'lh_e', 'uh_e''lphi_e', 'uphi_e'] + for field in fields_to_check: + bound = getattr(constraints, field) + if any(bound >= ACADOS_INFTY) or any(bound <= -ACADOS_INFTY): + raise ValueError(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") + + self._make_consistent_discretization() + + # cost scaling + if opts.cost_scaling is None: + opts.cost_scaling = np.append(opts.time_steps, 1.0) + if opts.cost_scaling.shape[0] != opts.N_horizon + 1: + raise ValueError(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') + + self._make_consistent_simulation() + + if opts.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + self.remove_x0_elimination() + # fixed hessian if opts.fixed_hess: if opts.hessian_approx == 'EXACT': raise ValueError('fixed_hess is not compatible with hessian_approx == EXACT.') - if cost.cost_type != "LINEAR_LS": + if cost.cost_type != "LINEAR_LS" and opts.N_horizon > 0: raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type.') - if cost.cost_type_0 != "LINEAR_LS": + if cost.cost_type_0 != "LINEAR_LS" and opts.N_horizon > 0: raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_0.') if cost.cost_type_e != "LINEAR_LS": raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_e.') # solution sensitivities - bgp_type_constraint_pairs = [ - ("path", model.con_phi_expr), ("initial", model.con_phi_expr_0), ("terminal", model.con_phi_expr_e), - ("path", model.con_r_expr), ("initial", model.con_r_expr_0), ("terminal", model.con_r_expr_e) - ] - bgh_type_constraint_pairs = [ - ("path", model.con_h_expr), ("initial", model.con_h_expr_0), ("terminal", model.con_h_expr_e), - ] + if opts.N_horizon > 0: + bgp_type_constraint_pairs = [ + ("path", model.con_phi_expr), ("initial", model.con_phi_expr_0), ("terminal", model.con_phi_expr_e), + ("path", model.con_r_expr), ("initial", model.con_r_expr_0), ("terminal", model.con_r_expr_e) + ] + cost_types_to_check = [cost.cost_type, cost.cost_type_0, cost.cost_type_e] + suffix = f", got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}." + else: + bgp_type_constraint_pairs = [ + ("terminal", model.con_phi_expr_e), ("terminal", model.con_r_expr_e) + ] + cost_types_to_check = [cost.cost_type_e] + suffix = f", got cost_type_e {cost.cost_type_e}." if opts.with_solution_sens_wrt_params: if dims.np_global == 0: raise ValueError('with_solution_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') - if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise ValueError(f'with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type, got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}.') - if opts.integrator_type != "DISCRETE": + if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in cost_types_to_check]): + raise ValueError('with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type' + suffix) + if opts.N_horizon > 0 and opts.integrator_type != "DISCRETE": raise NotImplementedError('with_solution_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): @@ -920,19 +1040,21 @@ def make_consistent(self, is_mocp_phase=False) -> None: if opts.with_value_sens_wrt_params: if dims.np_global == 0: raise ValueError('with_value_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') - if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise ValueError('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type.') - if opts.integrator_type != "DISCRETE": + if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in cost_types_to_check]): + raise ValueError('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type' + suffix) + if opts.N_horizon > 0 and opts.integrator_type != "DISCRETE": raise NotImplementedError('with_value_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): raise NotImplementedError(f"with_value_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + if opts.tau_min > 0 and "HPIPM" not in opts.qp_solver: + raise ValueError('tau_min > 0 is only compatible with HPIPM.') + if opts.qp_solver_cond_N is None: opts.qp_solver_cond_N = opts.N_horizon - - if opts.tau_min > 0 and not "HPIPM" in opts.qp_solver: - raise ValueError('tau_min > 0 is only compatible with HPIPM.') + if opts.qp_solver_cond_N > opts.N_horizon: + raise ValueError("qp_solver_cond_N > N_horizon is not supported.") if opts.qp_solver_cond_block_size is not None: if sum(opts.qp_solver_cond_block_size) != opts.N_horizon: @@ -941,6 +1063,8 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise ValueError(f'qp_solver_cond_block_size = {opts.qp_solver_cond_block_size} should have length qp_solver_cond_N+1 = {opts.qp_solver_cond_N+1}.') if opts.nlp_solver_type == "DDP": + if opts.N_horizon == 0: + raise ValueError("DDP solver only supported for N_horizon > 0.") if opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon: raise ValueError(f'DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N, got qp solver {opts.qp_solver} and qp_solver_cond_N {opts.qp_solver_cond_N}, N {opts.N_horizon}.') if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]): @@ -981,7 +1105,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: opts.globalization_full_step_dual = 0 # AS-RTI - if opts.as_rti_level in [1, 2] and any([cost.cost_type.endswith('LINEAR_LS'), cost.cost_type_0.endswith('LINEAR_LS'), cost.cost_type_e.endswith('LINEAR_LS')]): + if opts.as_rti_level in [1, 2] and any([cost_type.endswith("LINEAR_LS") for cost_type in cost_types_to_check]): raise NotImplementedError('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.') # sanity check for Funnel globalization and SQP @@ -989,7 +1113,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise NotImplementedError('FUNNEL_L1PEN_LINESEARCH only supports SQP.') # termination - if opts.nlp_solver_tol_min_step_norm == None: + if opts.nlp_solver_tol_min_step_norm is None: if ddp_with_merit_or_funnel: opts.nlp_solver_tol_min_step_norm = 1e-12 else: @@ -997,6 +1121,8 @@ def make_consistent(self, is_mocp_phase=False) -> None: # zoRO if self.zoro_description is not None: + if opts.N_horizon == 0: + raise ValueError('zoRO only supported for N_horizon > 0.') if not isinstance(self.zoro_description, ZoroDescription): raise TypeError('zoro_description should be of type ZoroDescription or None') else: @@ -1011,11 +1137,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: def _get_external_function_header_templates(self, ) -> list: dims = self.dims name = self.model.name + opts = self.solver_options template_list = [] # dynamics - model_dir = os.path.join(self.code_export_directory, f'{name}_model') - template_list.append(('model.in.h', f'{name}_model.h', model_dir)) + if opts.N_horizon > 0: + model_dir = os.path.join(self.code_export_directory, f'{name}_model') + template_list.append(('model.in.h', f'{name}_model.h', model_dir)) # constraints if any(np.array([dims.nh, dims.nh_e, dims.nh_0, dims.nphi, dims.nphi_e, dims.nphi_0]) > 0): constraints_dir = os.path.join(self.code_export_directory, f'{name}_constraints') @@ -1036,6 +1164,7 @@ def __get_template_list(self, cmake_builder=None) -> list: (input_filename, output_filname, output_directory) """ name = self.model.name + opts = self.solver_options template_list = [] template_list.append(('main.in.c', f'main_{name}.c')) @@ -1048,7 +1177,7 @@ def __get_template_list(self, cmake_builder=None) -> list: template_list.append(('Makefile.in', 'Makefile')) # sim - if self.solver_options.integrator_type != 'DISCRETE': + if opts.N_horizon > 0 and self.solver_options.integrator_type != 'DISCRETE': template_list.append(('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c')) template_list.append(('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h')) template_list.append(('main_sim.in.c', f'main_sim_{name}.c')) @@ -1156,6 +1285,50 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia model = self.model constraints = self.constraints + opts = self.solver_options + + check_casadi_version() + self._setup_code_generation_context_dynamics(context) + + if opts.N_horizon > 0: + if ignore_initial and ignore_terminal: + stage_type_indices = [1] + elif ignore_initial: + stage_type_indices = [1, 2] + elif ignore_terminal: + stage_type_indices = [0, 1] + else: + stage_type_indices = [0, 1, 2] + else: + stage_type_indices = [2] + + stage_types = [val for i, val in enumerate(['initial', 'path', 'terminal']) if i in stage_type_indices] + nhs = [val for i, val in enumerate(['nh_0', 'nh', 'nh_e']) if i in stage_type_indices] + nphis = [val for i, val in enumerate(['nphi_0', 'nphi', 'nphi_e']) if i in stage_type_indices] + cost_types = [val for i, val in enumerate(['cost_type_0', 'cost_type', 'cost_type_e']) if i in stage_type_indices] + + for attr_nh, attr_nphi, stage_type in zip(nhs, nphis, stage_types): + if getattr(self.dims, attr_nh) > 0 or getattr(self.dims, attr_nphi) > 0: + generate_c_code_constraint(context, model, constraints, stage_type) + + for attr, stage_type in zip(cost_types, stage_types): + if getattr(self.cost, attr) == 'NONLINEAR_LS': + generate_c_code_nls_cost(context, model, stage_type) + elif getattr(self.cost, attr) == 'CONVEX_OVER_NONLINEAR': + generate_c_code_conl_cost(context, model, stage_type) + elif getattr(self.cost, attr) == 'EXTERNAL': + generate_c_code_external_cost(context, model, stage_type) + # TODO: generic + + return context + + + def _setup_code_generation_context_dynamics(self, context: GenerateContext): + opts = self.solver_options + model = self.model + + if opts.N_horizon == 0: + return code_gen_opts = context.opts @@ -1164,7 +1337,6 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia if not os.path.exists(model_dir): os.makedirs(model_dir) - check_casadi_version() if self.model.dyn_ext_fun_type == 'casadi': if self.solver_options.integrator_type == 'ERK': generate_c_code_explicit_ode(context, model, model_dir) @@ -1186,34 +1358,6 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia shutil.copyfile(model.dyn_generic_source, target_location) context.add_external_function_file(model.dyn_generic_source, target_dir) - if ignore_initial and ignore_terminal: - stage_type_indices = [1] - elif ignore_initial: - stage_type_indices = [1, 2] - elif ignore_terminal: - stage_type_indices = [0, 1] - else: - stage_type_indices = [0, 1, 2] - - stage_types = [val for i, val in enumerate(['initial', 'path', 'terminal']) if i in stage_type_indices] - nhs = [val for i, val in enumerate(['nh_0', 'nh', 'nh_e']) if i in stage_type_indices] - nphis = [val for i, val in enumerate(['nphi_0', 'nphi', 'nphi_e']) if i in stage_type_indices] - cost_types = [val for i, val in enumerate(['cost_type_0', 'cost_type', 'cost_type_e']) if i in stage_type_indices] - - for attr_nh, attr_nphi, stage_type in zip(nhs, nphis, stage_types): - if getattr(self.dims, attr_nh) > 0 or getattr(self.dims, attr_nphi) > 0: - generate_c_code_constraint(context, model, constraints, stage_type) - - for attr, stage_type in zip(cost_types, stage_types): - if getattr(self.cost, attr) == 'NONLINEAR_LS': - generate_c_code_nls_cost(context, model, stage_type) - elif getattr(self.cost, attr) == 'CONVEX_OVER_NONLINEAR': - generate_c_code_conl_cost(context, model, stage_type) - elif getattr(self.cost, attr) == 'EXTERNAL': - generate_c_code_external_cost(context, model, stage_type) - # TODO: generic - - return context def remove_x0_elimination(self) -> None: """Remove the elimination of x0 from the constraints, bounds on x0 are handled as general bounds on x.""" diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 5753216d20..8855b42355 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -1118,7 +1118,7 @@ def tf(self): def N_horizon(self): """ Number of shooting intervals. - Type: int > 0 + Type: int >= 0 Default: :code:`None` """ return self.__N_horizon @@ -1384,10 +1384,10 @@ def tf(self, tf): @N_horizon.setter def N_horizon(self, N_horizon): - if isinstance(N_horizon, int) and N_horizon > 0: + if isinstance(N_horizon, int) and N_horizon >= 0: self.__N_horizon = N_horizon else: - raise ValueError('Invalid N_horizon value, expected positive integer.') + raise ValueError('Invalid N_horizon value, expected non-negative integer.') @time_steps.setter def time_steps(self, time_steps): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 8493f43b16..eb8f585b5d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -121,9 +121,6 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: else: detect_gnsf_structure(acados_ocp) - if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': - acados_ocp.remove_x0_elimination() - if acados_ocp.solver_options.qp_solver in ['FULL_CONDENSING_QPOASES', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP']: print(f"NOTE: The selected QP solver {acados_ocp.solver_options.qp_solver} does not support one-sided constraints yet.") diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 64923291dd..d9c4d56609 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -108,12 +108,15 @@ endif() # object target names +{%- if problem_class == "SIM" or solver_options.N_horizon > 0 %} set(MODEL_OBJ model_{{ model.name }}) +{%- endif %} set(OCP_OBJ ocp_{{ model.name }}) {%- if solver_options.integrator_type != "DISCRETE" %} set(SIM_OBJ sim_{{ model.name }}) {%- endif %} +{%- if problem_class == "SIM" or solver_options.N_horizon > 0 %} # model set(MODEL_SRC {%- for filename in external_function_files_model %} @@ -121,7 +124,7 @@ set(MODEL_SRC {%- endfor %} ) add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) - +{%- endif %} {% if problem_class != "SIM" %} # optimal control problem - mostly CasADi exports @@ -223,7 +226,11 @@ endif() # bundled_shared_lib if(${BUILD_ACADOS_SOLVER_LIB}) set(LIB_ACADOS_SOLVER acados_solver_{{ model.name }}) - add_library(${LIB_ACADOS_SOLVER} SHARED $ $ + add_library(${LIB_ACADOS_SOLVER} SHARED + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $ {%- if solver_options.integrator_type != "DISCRETE" %} $ {%- endif -%} @@ -234,7 +241,11 @@ endif(${BUILD_ACADOS_SOLVER_LIB}) # ocp_shared_lib if(${BUILD_ACADOS_OCP_SOLVER_LIB}) set(LIB_ACADOS_OCP_SOLVER acados_ocp_solver_{{ model.name }}) - add_library(${LIB_ACADOS_OCP_SOLVER} SHARED $ $) + add_library(${LIB_ACADOS_OCP_SOLVER} SHARED + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $) # Specify libraries or flags to use when linking a given target and/or its dependents. target_link_libraries(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_LIB}) target_link_directories(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_DIR}) @@ -243,7 +254,11 @@ endif(${BUILD_ACADOS_OCP_SOLVER_LIB}) # example if(${BUILD_EXAMPLE}) - add_executable(${EX_EXE} ${EX_SRC} $ $ + add_executable(${EX_EXE} ${EX_SRC} + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $ {%- if solver_options.integrator_type != "DISCRETE" %} $ {%- endif -%} 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 676d3970dc..f1ce6e7f33 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 @@ -43,7 +43,9 @@ {%- endif %} // example specific +{% if solver_options.N_horizon > 0 %} #include "{{ model.name }}_model/{{ model.name }}_model.h" +{%- endif %} {% if dims.n_global_data > 0 %} #include "{{ name }}_p_global_precompute_fun.h" @@ -142,6 +144,10 @@ int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule) int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) { +{% if solver_options.N_horizon == 0 %} + printf("\nacados_update_time_steps() not implemented, since N_horizon = 0!\n\n"); + exit(1); +{% else %} if (N != capsule->nlp_solver_plan->N) { fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \ "differs from the currently allocated number of " \ @@ -161,6 +167,7 @@ int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* c ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); } return 0; +{% endif %} } /** @@ -179,9 +186,11 @@ void {{ model.name }}_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, co nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; + {%- if solver_options.N_horizon > 0 %} nlp_solver_plan->nlp_cost[0] = {{ cost.cost_type_0 }}; for (int i = 1; i < N; i++) nlp_solver_plan->nlp_cost[i] = {{ cost.cost_type }}; + {%- endif %} nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }}; @@ -271,7 +280,9 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na nbx[0] = NBX0; nsbx[0] = 0; ns[0] = NS0; + {% if solver_options.N_horizon > 0 %} nbxe[0] = {{ dims.nbxe_0 }}; + {% endif %} ny[0] = NY0; nh[0] = NH0; nsh[0] = NSH0; @@ -321,12 +332,12 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); } - -{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR"%} +{%- if solver_options.N_horizon > 0 %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); {%- endif %} -{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR"%} +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR" %} for (int i = 1; i < N; i++) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} @@ -351,6 +362,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]); {%- endif %} } +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); @@ -364,6 +376,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type == "GNSF" -%} // GNSF specific dimensions int gnsf_nx1 = {{ dims.gnsf_nx1 }}; @@ -386,7 +399,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na for (int i = 0; i < N; i++) ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} - +{%- endif %}{# solver_options.N_horizon > 0 #} free(intNp1mem); return nlp_dims; @@ -453,7 +466,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps {%- endif %} ext_fun_opts.external_workspace = true; - +{%- if solver_options.N_horizon > 0 %} {%- if constraints.constr_type_0 == "BGH" and dims.nh_0 > 0 %} MAP_CASADI_FNC(nl_constr_h_0_fun_jac, {{ model.name }}_constr_h_0_fun_jac_uxt_zt); MAP_CASADI_FNC(nl_constr_h_0_fun, {{ model.name }}_constr_h_0_fun); @@ -811,6 +824,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps } {%- endif %} {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} @@ -886,7 +900,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps /** - * Internal function for {{ model.name }}_acados_create: step 4 + * Internal function for {{ model.name }}_acados_create: step 5 */ void {{ model.name }}_acados_create_set_default_parameters({{ model.name }}_solver_capsule* capsule) { @@ -947,14 +961,16 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps and cost_scaling - {%- set all_equal = true -%} - {%- set val = solver_options.time_steps[0] %} - {%- for j in range(start=1, end=solver_options.N_horizon) %} - {%- if val != solver_options.time_steps[j] %} - {%- set_global all_equal = false %} - {%- break %} - {%- endif %} - {%- endfor %} + {%- if solver_options.N_horizon > 0 -%} + {%- set all_equal = true -%} + {%- set val = solver_options.time_steps[0] %} + {%- for j in range(start=1, end=solver_options.N_horizon) %} + {%- if val != solver_options.time_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + {%- endif %} if (new_time_steps) { @@ -964,6 +980,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu else { // set time_steps + {%- if solver_options.N_horizon > 0 %} {% if all_equal == true -%}{# all time_steps are identical #} double time_step = {{ solver_options.time_steps[0] }}; for (int i = 0; i < N; i++) @@ -981,6 +998,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(time_steps); {%- endif %} + {%- endif %}{# solver_options.N_horizon > 0 #} // set cost scaling double* cost_scaling = malloc((N+1)*sizeof(double)); {%- for j in range(end=solver_options.N_horizon+1) %} @@ -994,6 +1012,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } +{% if solver_options.N_horizon > 0 %} /**** Dynamics ****/ for (int i = 0; i < N; i++) { @@ -1226,6 +1245,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endif %}{# LINEAR LS #} {%- endif %}{# ny != 0 #} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if dims.ny_e != 0 %} @@ -1270,6 +1290,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endif %}{# ny_e != 0 #} +{%- if solver_options.N_horizon > 0 %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); @@ -1320,6 +1341,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {% endif %} } {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if cost.cost_type_e == "NONLINEAR_LS" %} ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); @@ -1345,6 +1367,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} +{% if solver_options.N_horizon > 0 %} {% if dims.ns_0 > 0 %} // slacks initial double* zlu0_mem = calloc(4*NS0, sizeof(double)); @@ -1427,6 +1450,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(zlumem); {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {% if dims.ns_e > 0 %} // slacks terminal @@ -1471,6 +1495,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu /**** Constraints ****/ // bounds for initial stage +{%- if solver_options.N_horizon > 0 %} {%- if dims.nbx_0 > 0 %} // x0 int* idxbx0 = malloc(NBX0 * sizeof(int)); @@ -1937,6 +1962,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(luphi); {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} /* terminal constraints */ {% if dims.nbx_e > 0 %} @@ -2249,6 +2275,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps } {%- endif %} +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type != "DISCRETE" %} // set collocation type (relevant for implicit integrators) @@ -2351,6 +2378,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps } {%- endif %} {%- endif %}{# solver_options.integrator_type != "DISCRETE" #} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if solver_options.nlp_solver_warm_start_first_qp %} int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }}; @@ -2378,7 +2406,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {%- endif %} ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); - {%- if solver_options.qp_solver_cond_block_size -%} + {%- if solver_options.qp_solver_cond_block_size and solver_options.N_horizon > 0 -%} int* qp_solver_cond_block_size = malloc((qp_solver_cond_N+1) * sizeof(int)); {%- for i in range(end=solver_options.qp_solver_cond_N+1) %} qp_solver_cond_block_size[{{ i }}] = {{ solver_options.qp_solver_cond_block_size[i] }}; @@ -2548,7 +2576,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {% endif %} int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; -{%- if cost.cost_type == "EXTERNAL" %} +{%- if cost.cost_type == "EXTERNAL" and solver_options.N_horizon > 0 %} for (int i = 0; i < N; i++) { ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess); @@ -2574,7 +2602,7 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); double* x0 = xu0; -{% if dims.nbx_0 == dims.nx %} +{% if dims.nbx_0 == dims.nx and solver_options.N_horizon > 0 %} // initialize with x0 {%- for item in constraints.lbx_0 %} {%- if item != 0 %} @@ -2675,7 +2703,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c */ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule* capsule, int qp_solver_cond_N) { -{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} +{%- if solver_options.N_horizon == 0 %} + printf("\nacados_update_qp_solver_cond_N() not implemented, since N_horizon = 0!\n\n"); + exit(1); +{%- elif solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} // 1) destroy solver ocp_nlp_solver_destroy(capsule->nlp_solver); @@ -2695,7 +2726,6 @@ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_caps {%- else %} printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); exit(1); - return -1; {%- endif %} } @@ -3000,6 +3030,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) /* free external function */ // dynamics +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type == "IRK" %} for (int i = 0; i < N; i++) { @@ -3160,6 +3191,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) free(capsule->ext_cost_grad_p); {%- endif %} {%- endif %} +{%- endif %}{# if solver_options.N_horizon > 0 #} {%- if cost.cost_type_e == "NONLINEAR_LS" %} external_function_external_param_casadi_free(&capsule->cost_y_e_fun); external_function_external_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); @@ -3182,6 +3214,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- endif %} // constraints +{%- if solver_options.N_horizon > 0 %} {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} for (int i = 0; i < N-1; i++) { @@ -3229,6 +3262,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_external_param_casadi_free(&capsule->phi_0_constraint_fun); external_function_external_param_casadi_free(&capsule->phi_0_constraint_fun_jac_hess); {%- endif %} +{%- endif %}{# if solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} external_function_external_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); From bcf01c1d110c3235508465d79fd37935d032f02a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 15 Apr 2025 13:31:51 +0200 Subject: [PATCH 028/164] Modify `AcadosOcpBatchSolver` to use flexible batch size (#1460) --- .../ocp/minimal_example_batch_ocp_solver.py | 22 ++- ...ch_adjoint_solution_sensitivity_example.py | 4 +- .../acados_ocp_batch_solver.py | 135 ++++++++++-------- 3 files changed, 96 insertions(+), 65 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py index e23efe7170..adb708d83c 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py @@ -122,8 +122,10 @@ def main_batch(Xinit, simU, tol, num_threads_in_batch_solve=1): N_batch = Xinit.shape[0] - 1 ocp = setup_ocp(tol) - batch_solver = AcadosOcpBatchSolver(ocp, N_batch, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) - + N_batch_max = N_batch + 3 # to test with more than N_batch + + batch_solver = AcadosOcpBatchSolver(ocp, N_batch_max, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) + assert batch_solver.num_threads_in_batch_solve == num_threads_in_batch_solve batch_solver.num_threads_in_batch_solve = 1337 assert batch_solver.num_threads_in_batch_solve == 1337 @@ -144,16 +146,24 @@ def main_batch(Xinit, simU, tol, num_threads_in_batch_solve=1): # solve t0 = time.time() - batch_solver.solve() + batch_solver.solve(N_batch) t_elapsed = 1e3 * (time.time() - t0) print(f"main_batch: with {num_threads_in_batch_solve} threads, solve: {t_elapsed:.3f}ms") - U_batch = batch_solver.get_flat("u") + U_batch = batch_solver.get_flat("u", N_batch) for n in range(N_batch): - if not np.linalg.norm(U_batch[n, :ocp.dims.nu] -simU[n]) < tol*10: - raise Exception(f"solution should match sequential call up to {tol*10} got error {np.linalg.norm(u-simU[n])} for {n}th batch solve") + if not np.linalg.norm(U_batch[n, :ocp.dims.nu] - simU[n]) < tol*10: + raise Exception(f"solution should match sequential call up to {tol*10} got error {np.linalg.norm(U_batch[n, :ocp.dims.nu] - simU[n])} for {n}th batch solve") + + # test N_batch_max is respected + try: + batch_solver.get_flat("x", N_batch_max+1) + except Exception as e: + print(f"error raised correctly: {e}") + else: + raise Exception("using n_batch > N_batch_max should raise an error") if __name__ == "__main__": diff --git a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py index e18d60c615..fe1dbc925a 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py @@ -99,7 +99,7 @@ def main_batch(Xinit, simU, param_vals, adjoints_ref, tol, num_threads_in_batch_ # solve t0 = time.time() - batch_solver.solve() + batch_solver.solve(N_batch) t_elapsed = 1e3 * (time.time() - t0) print(f"main_batch: with {num_threads_in_batch_solve} threads, solve: {t_elapsed:.3f} ms") @@ -111,7 +111,7 @@ def main_batch(Xinit, simU, param_vals, adjoints_ref, tol, num_threads_in_batch_ raise Exception(f"solution should match sequential call up to {tol} got error {diff} for {n}th batch solve") # actually not needed for convex problem but we want to test it - batch_solver.setup_qp_matrices_and_factorize() + batch_solver.setup_qp_matrices_and_factorize(N_batch) # eval adjoint t0 = time.time() diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index e4855ff3f9..6f75d164e2 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -42,8 +42,8 @@ class AcadosOcpBatchSolver(): Batch OCP solver for parallel solves. :param ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` - :param N_batch: batch size, positive integer :param num_threads_in_batch_solve: number of threads used for parallelizing the batch methods. Default: 1 + :param N_batch_max: maximum batch size, positive integer :param json_file: Default: 'acados_ocp.json' :param build: Flag indicating whether solver should be (re)compiled. If False an attempt is made to load an already compiled shared library for the solver. Default: True :param generate: Flag indicating whether problem functions should be code generated. Default: True @@ -52,10 +52,13 @@ class AcadosOcpBatchSolver(): __ocp_solvers : List[AcadosOcpSolver] - def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None, json_file: str = 'acados_ocp.json', build: bool = True, generate: bool = True, verbose: bool=True): + def __init__(self, ocp: AcadosOcp, N_batch_max: int, + num_threads_in_batch_solve: Union[int, None] = None, + json_file: str = 'acados_ocp.json', + build: bool = True, generate: bool = True, verbose: bool=True): - if not isinstance(N_batch, int) or N_batch <= 0: - raise ValueError("AcadosOcpBatchSolver: argument N_batch should be a positive integer.") + if not isinstance(N_batch_max, int) or N_batch_max <= 0: + raise ValueError("AcadosOcpBatchSolver: argument N_batch_max should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = ocp.solver_options.num_threads_in_batch_solve print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead.") @@ -66,27 +69,27 @@ def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Uni print("Warning: Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False.") print("Attempting to compile with openmp nonetheless.") ocp.solver_options.with_batch_functionality = True - + self.__num_threads_in_batch_solve = num_threads_in_batch_solve - self.__N_batch = N_batch + self.__N_batch_max = N_batch_max self.__ocp_solvers = [AcadosOcpSolver(ocp, json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, verbose=verbose) - for n in range(self.N_batch)] + for n in range(self.N_batch_max)] self.__shared_lib = self.ocp_solvers[0].shared_lib self.__acados_lib = self.ocp_solvers[0].acados_lib self.__name = self.ocp_solvers[0].name - self.__ocp_solvers_pointer = (c_void_p * self.N_batch)() + self.__ocp_solvers_pointer = (c_void_p * self.N_batch_max)() - for i in range(self.N_batch): + for i in range(self.N_batch_max): self.__ocp_solvers_pointer[i] = self.ocp_solvers[i].capsule # out data for solve - self.__status = np.zeros((self.N_batch,), dtype=np.intc, order="C") + self.__status = np.zeros((self.N_batch_max,), dtype=np.intc, order="C") self.__status_p = cast(self.__status.ctypes.data, POINTER(c_int)) getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve").argtypes = [POINTER(c_void_p), POINTER(c_int), c_int, c_int] @@ -119,40 +122,41 @@ def ocp_solvers(self): """List of AcadosOcpSolvers.""" return self.__ocp_solvers - @property - def N_batch(self): - """Batch size.""" - return self.__N_batch - + def N_batch_max(self): + """Maximum batch size.""" + return self.__N_batch_max + @property def num_threads_in_batch_solve(self): """Number of threads used for parallelizing the batch methods.""" return self.__num_threads_in_batch_solve - + @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): self.__num_threads_in_batch_solve = num_threads_in_batch_solve - def solve(self): + def solve(self, n_batch: Optional[int] = None) -> None: """ - Call solve for all `N_batch` solvers. + Call solve for the first `n_batch` solvers. Or `N_batch_max` if `n_batch` is None. """ + n_batch = self.__check_n_batch(n_batch) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, n_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): solver.status = s - def setup_qp_matrices_and_factorize(self): + def setup_qp_matrices_and_factorize(self, n_batch: Optional[int] = None) -> None: """ - Call setup_qp_matrices_and_factorize for all `N_batch` solvers. + Call setup_qp_matrices_and_factorize for the first `n_batch` solvers. """ + n_batch = self.__check_n_batch(n_batch) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, n_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): @@ -168,12 +172,12 @@ def eval_adjoint_solution_sensitivity(self, """ Evaluate the adjoint sensitivity of the solution with respect to the parameters. :param seed_x : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (N_batch, nx, n_seeds) + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (n_batch, nx, n_seeds) :param seed_u : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (N_batch, nu, n_seeds) + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (n_batch, nu, n_seeds) :param with_respect_to : string in ["p_global"] :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True - :returns : np.ndarray of shape (N_batch, n_seeds, np_global) + :returns : np.ndarray of shape (n_batch, n_seeds, np_global) """ if seed_x is None: @@ -196,6 +200,7 @@ def eval_adjoint_solution_sensitivity(self, if not isinstance(s, np.ndarray): raise TypeError(f"seed_x[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_x[0][1].shape[2] + n_batch = seed_x[0][1].shape[0] if len(seed_u) > 0: if not isinstance(seed_u[0], tuple) or len(seed_u[0]) != 2: @@ -204,11 +209,14 @@ def eval_adjoint_solution_sensitivity(self, if not isinstance(s, np.ndarray): raise TypeError(f"seed_u[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_u[0][1].shape[2] + n_batch = seed_u[0][1].shape[0] + + n_batch = self.__check_n_batch(n_batch) if sanity_checks: N_horizon = self.__ocp_solvers[0].acados_ocp.solver_options.N_horizon - for n in range(self.N_batch): + for n in range(n_batch): self.__ocp_solvers[n]._sanity_check_solution_sensitivities() nx = self.__acados_lib.ocp_nlp_dims_get_from_attr( @@ -223,8 +231,8 @@ def eval_adjoint_solution_sensitivity(self, raise ValueError(f"AcadosOcpBatchSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") if not isinstance(seed_stage, np.ndarray): raise TypeError(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") - if seed_stage.shape != (self.N_batch, dim, n_seeds): - raise ValueError(f"{name} for stage {stage} should have shape (N_batch, dim, n_seeds) = ({self.N_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") + if seed_stage.shape != (n_batch, dim, n_seeds): + raise ValueError(f"{name} for stage {stage} should have shape (n_batch, dim, n_seeds) = ({n_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") if with_respect_to == "p_global": field = "p_global".encode('utf-8') @@ -234,20 +242,20 @@ def eval_adjoint_solution_sensitivity(self, # compute jacobian wrt params t0 = time.time() - getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, n_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_lin = time.time() - t0 t1 = time.time() - grad_p = np.zeros((self.N_batch, n_seeds, np_global), order="C", dtype=np.float64) + grad_p = np.zeros((n_batch, n_seeds, np_global), order="C", dtype=np.float64) offset = n_seeds*np_global for i_seed in range(n_seeds): # set seed: - self.reset_sens_out() + self._reset_sens_out(n_batch) # TODO this could be a batch_set - for m in range(self.N_batch): + for m in range(n_batch): for (stage, sx) in seed_x: self.ocp_solvers[m].set(stage, 'sens_x', sx[m, :, i_seed]) for (stage, su) in seed_u: @@ -257,7 +265,7 @@ def eval_adjoint_solution_sensitivity(self, # solve adjoint sensitivities getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p")( - self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, self.__N_batch, self.__num_threads_in_batch_solve) + self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, n_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_solve = time.time() - t1 @@ -266,19 +274,19 @@ def eval_adjoint_solution_sensitivity(self, raise NotImplementedError(f"with_respect_to {with_respect_to} not implemented.") - def reset_sens_out(self, ): + def _reset_sens_out(self, n_batch: int) -> None: # TODO batch this - for n in range(self.N_batch): + for n in range(n_batch): self.__acados_lib.ocp_nlp_out_set_values_to_zero(self.__ocp_solvers[n].nlp_config, self.__ocp_solvers[n].nlp_dims, self.__ocp_solvers[n].sens_out) def set_flat(self, field_: str, value_: np.ndarray) -> None: """ - Set concatenation solver initialization for all `N_batch` solvers. + Set concatenation solver initialization for the first `n_batch` solvers. :param field_: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] - :param value_: np.array of shape (N_batch, n_field_total) + :param value_: np.array of shape (n_batch, n_field_total) """ field = field_.encode('utf-8') @@ -286,9 +294,12 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') dim = self.ocp_solvers[0].get_dim_flat(field_) + n_batch = value_.shape[0] - if value_.shape != (self.N_batch, dim): - raise ValueError(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({self.N_batch}, {dim}), got {value_.shape}.') + n_batch = self.__check_n_batch(n_batch) + + if value_.shape != (n_batch, dim): + raise ValueError(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({n_batch}, {dim}), got {value_.shape}.') value_ = value_.reshape((-1,), order='C') N_data = value_.shape[0] @@ -296,10 +307,10 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: value_ = value_.astype(float) value_data = cast(value_.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, n_batch, self.__num_threads_in_batch_solve) - def get_flat(self, field_: str) -> np.ndarray: + def get_flat(self, field_: str, n_batch: Optional[int] = None) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. @@ -308,40 +319,43 @@ def get_flat(self, field_: str) -> np.ndarray: """ if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + n_batch = self.__check_n_batch(n_batch) field = field_.encode('utf-8') dim = self.ocp_solvers[0].get_dim_flat(field_) - out = np.ascontiguousarray(np.zeros((self.N_batch, dim,)), dtype=np.float64) + out = np.ascontiguousarray(np.zeros((n_batch, dim,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, self.N_batch*dim, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, n_batch*dim, n_batch, self.__num_threads_in_batch_solve) return out - def store_iterate_to_flat_obj(self) -> AcadosOcpFlattenedBatchIterate: + def store_iterate_to_flat_obj(self, n_batch: Optional[int] = None) -> AcadosOcpFlattenedBatchIterate: """ - Returns the current iterate of the OCP solvers as an AcadosOcpFlattenedBatchIterate. + Returns the current iterate of the first `n_batch` OCP solvers as an AcadosOcpFlattenedBatchIterate. """ - return AcadosOcpFlattenedBatchIterate(x = self.get_flat("x"), - u = self.get_flat("u"), - z = self.get_flat("z"), - sl = self.get_flat("sl"), - su = self.get_flat("su"), - pi = self.get_flat("pi"), - lam = self.get_flat("lam"), - N_batch=self.N_batch) + n_batch = self.__check_n_batch(n_batch) + return AcadosOcpFlattenedBatchIterate(x = self.get_flat("x", n_batch), + u = self.get_flat("u", n_batch), + z = self.get_flat("z", n_batch), + sl = self.get_flat("sl", n_batch), + su = self.get_flat("su", n_batch), + pi = self.get_flat("pi", n_batch), + lam = self.get_flat("lam", n_batch), + N_batch=n_batch) def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedBatchIterate) -> None: """ - Loads the provided iterate into the OCP solvers. + Loads the provided iterate into the first `n_batch` OCP solvers. + n_batch is determined by the iterate object. + Note: The iterate object does not contain the the parameters. """ - - if self.N_batch != iterate.N_batch: - raise ValueError(f"Wrong batch dimension. Expected {self.N_batch}, got {iterate.N_batch}") + n_batch = iterate.N_batch + n_batch = self.__check_n_batch(n_batch) self.set_flat("x", iterate.x) self.set_flat("u", iterate.u) @@ -350,3 +364,10 @@ def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedBatchIterate) -> self.set_flat("su", iterate.su) self.set_flat("pi", iterate.pi) self.set_flat("lam", iterate.lam) + + def __check_n_batch(self, n_batch: Optional[int]) -> int: + if n_batch is None: + n_batch = self.N_batch_max + if n_batch > self.N_batch_max: + raise Exception(f"AcadosOcpBatchSolver: n_batch {n_batch} is larger than N_batch_max {self.N_batch_max}.") + return n_batch From c9b5d614abbe2f5408a66eb59328790cb497986f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 15 Apr 2025 17:42:51 +0200 Subject: [PATCH 029/164] Fix typo: intial -> initial (#1500) - NOTE: function `AcadosOcp.translate_initial_cost_term_to_external` recently introduced has now a new name. --- examples/acados_matlab_octave/race_cars/main.m | 4 ++-- .../test/create_ocp_qp_solver_formulation.m | 2 +- .../acados_matlab_octave/test/create_parametric_ocp_qp.m | 2 +- examples/acados_python/race_cars/acados_settings.py | 4 ++-- examples/acados_python/race_cars/acados_settings_dev.py | 4 ++-- interfaces/acados_template/acados_template/acados_ocp.py | 6 +++--- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/examples/acados_matlab_octave/race_cars/main.m b/examples/acados_matlab_octave/race_cars/main.m index ce3c0f658b..32a091cfbc 100644 --- a/examples/acados_matlab_octave/race_cars/main.m +++ b/examples/acados_matlab_octave/race_cars/main.m @@ -149,7 +149,7 @@ ocp_model.set('cost_Zl', eye(nsh,nsh)); ocp_model.set('cost_Zu', eye(nsh,nsh)); -% set intial condition +% set initial condition ocp_model.set('constr_x0', model.x0); % cost = define linear cost on x and u @@ -188,7 +188,7 @@ ocp_model.set('cost_W', W); ocp_model.set('cost_W_e', W_e); -% set intial references +% set initial references y_ref = zeros(ny,1); y_ref_e = zeros(ny_e,1); y_ref(1) = 1; % set reference on 's' to 1 to push the car forward (progress) diff --git a/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m b/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m index 2603534f1c..a7c403f73f 100644 --- a/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m +++ b/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m @@ -36,7 +36,7 @@ Vx_e = zeros(ny_e,nx); Vx_e(1:nx,:) = eye(nx); % state-to-output matrix in mayer term W = eye(ny); W_e = 5 * W(1:ny_e,1:ny_e); % cost weights in mayer term -y_ref = zeros(ny,1); % set intial references +y_ref = zeros(ny,1); % set initial references y_ref_e = zeros(ny_e,1); % cost diff --git a/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m b/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m index 00446c39c3..d96ea92922 100644 --- a/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m +++ b/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m @@ -33,7 +33,7 @@ Vx_e = zeros(ny_e,nx); Vx_e(1:nx,:) = eye(nx); % state-to-output matrix in mayer term W = eye(ny); W_e = 5 * W(1:ny_e,1:ny_e); % cost weights in mayer term - y_ref = zeros(ny,1); % set intial references + y_ref = zeros(ny,1); % set initial references y_ref_e = zeros(ny_e,1); ocp.cost.cost_type = cost_type; diff --git a/examples/acados_python/race_cars/acados_settings.py b/examples/acados_python/race_cars/acados_settings.py index bd2b65b1e1..bb51001162 100644 --- a/examples/acados_python/race_cars/acados_settings.py +++ b/examples/acados_python/race_cars/acados_settings.py @@ -102,7 +102,7 @@ def acados_settings(Tf, N, track_file): ocp.cost.zu = 100 * np.ones((ns,)) ocp.cost.Zu = 0 * np.ones((ns,)) - # set intial references + # set initial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) @@ -138,7 +138,7 @@ def acados_settings(Tf, N, track_file): ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2]) - # set intial condition + # set initial condition ocp.constraints.x0 = model.x0 # set QP solver and integration diff --git a/examples/acados_python/race_cars/acados_settings_dev.py b/examples/acados_python/race_cars/acados_settings_dev.py index c7604afdb4..256242694b 100644 --- a/examples/acados_python/race_cars/acados_settings_dev.py +++ b/examples/acados_python/race_cars/acados_settings_dev.py @@ -106,7 +106,7 @@ def acados_settings(Tf, N, track_file): ocp.cost.Zl = 1 * np.ones((ns,)) ocp.cost.Zu = 1 * np.ones((ns,)) - # set intial references + # set initial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) @@ -144,7 +144,7 @@ def acados_settings(Tf, N, track_file): ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array(range(nsh)) - # set intial condition + # set initial condition ocp.constraints.x0 = model.x0 # set QP solver and integration diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 85c3228d20..f1be8b93b5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -584,7 +584,7 @@ def _make_consistent_slacks_initial(self): cost.Zl_0 = cost.Zl cost.Zu_0 = cost.Zu print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") - print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") + print("Using entries [zl, zu, Zl, Zu] at initial node for slack penalties.\n") else: raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") @@ -1500,12 +1500,12 @@ def translate_cost_to_external_cost(self, self.model.p_global = ca.vertcat(self.model.p_global, p_global) self.p_global_values = np.concatenate((self.p_global_values, p_global_values)) - self.translate_intial_cost_term_to_external(yref_0, W_0, cost_hessian) + self.translate_initial_cost_term_to_external(yref_0, W_0, cost_hessian) self.translate_intermediate_cost_term_to_external(yref, W, cost_hessian) self.translate_terminal_cost_term_to_external(yref_e, W_e, cost_hessian) - def translate_intial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") From 9ab52ee00bf7993e5695a4bcd520beaa12aff189 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 17 Apr 2025 18:20:20 +0200 Subject: [PATCH 030/164] Python verbosity (#1501) - add verbose to `make_consistent()` for `AcadosOcp` and `AcadosMultiphaseOcp` - for batch solvers: only use verbose when creating the first solver --- .../acados_template/acados_multiphase_ocp.py | 4 +- .../acados_template/acados_ocp.py | 37 ++++++++++--------- .../acados_ocp_batch_solver.py | 3 +- .../acados_template/acados_ocp_solver.py | 9 +++-- .../acados_sim_batch_solver.py | 3 +- 5 files changed, 30 insertions(+), 26 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index 6df5df8176..9282eb8528 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -256,7 +256,7 @@ def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None: return - def make_consistent(self) -> None: + def make_consistent(self, verbose: bool = True) -> None: self.N_horizon = sum(self.N_list) self.solver_options.N_horizon = self.N_horizon # NOTE: to not change options when making ocp consistent @@ -330,7 +330,7 @@ def make_consistent(self) -> None: print(f"Phase {i} contains non-default initial fields: {nondefault_fields}, which will be ignored.") print(f"Calling make_consistent for phase {i}.") - ocp.make_consistent(is_mocp_phase=True) + ocp.make_consistent(is_mocp_phase=True, verbose=verbose) self.dummy_ocp_list.append(ocp) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index f1be8b93b5..4aa465568e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -898,7 +898,7 @@ def _make_consistent_simulation(self): raise ValueError("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") - def make_consistent(self, is_mocp_phase=False) -> None: + def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None: """ Detect dimensions, perform sanity checks """ @@ -941,23 +941,24 @@ def make_consistent(self, is_mocp_phase=False) -> None: self._make_consistent_cost_terminal() # GN check - gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) - gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) - gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) - if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): - external_cost_types = [] - if gn_warning_0: - external_cost_types.append('cost_type_0') - if gn_warning_path: - external_cost_types.append('cost_type') - if gn_warning_terminal: - external_cost_types.append('cost_type_e') - print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" - f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" - "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" - "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" - "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" - "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + if verbose: + gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) + gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) + gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) + if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): + external_cost_types = [] + if gn_warning_0: + external_cost_types.append('cost_type_0') + if gn_warning_path: + external_cost_types.append('cost_type') + if gn_warning_terminal: + external_cost_types.append('cost_type_e') + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" + f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" + "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" + "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" + "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") # cost integration if opts.N_horizon > 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 6f75d164e2..6f9068ed58 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -77,7 +77,8 @@ def __init__(self, ocp: AcadosOcp, N_batch_max: int, json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, - verbose=verbose) + verbose=verbose if n==0 else False, + ) for n in range(self.N_batch_max)] self.__shared_lib = self.ocp_solvers[0].shared_lib diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index eb8f585b5d..2f443f0377 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -90,7 +90,7 @@ def shared_lib(self,): return self.__shared_lib @classmethod - def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None): + def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None, verbose=True): """ Generates the code for an acados OCP solver, given the description in acados_ocp. :param acados_ocp: type Union[AcadosOcp, AcadosMultiphaseOcp] - description of the OCP for acados @@ -100,6 +100,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with `MS Visual Studio`); default: `None` + :param verbose: indicating if warnings are printed """ acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) @@ -112,7 +113,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: acados_ocp.simulink_opts = simulink_opts # make consistent - acados_ocp.make_consistent() + acados_ocp.make_consistent(verbose=verbose) # module dependent post processing if acados_ocp.solver_options.integrator_type == 'GNSF': @@ -225,11 +226,11 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json if generate: if json_file is not None: acados_ocp.json_file = json_file - self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) + self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder, verbose=verbose) json_file = acados_ocp.json_file else: if acados_ocp is not None: - acados_ocp.make_consistent() + acados_ocp.make_consistent(verbose=verbose) # load json, store options in object with open(json_file, 'r') as f: diff --git a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py index 5d0ac0d466..0893f8d731 100644 --- a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py @@ -70,7 +70,8 @@ def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Uni json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, - verbose=verbose) + verbose=verbose if n==0 else False, + ) for n in range(self.N_batch)] self.__shared_lib = self.sim_solvers[0].shared_lib From 39fb391fff862251126ae2dd6951b8131b8de753 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Apr 2025 19:05:52 +0200 Subject: [PATCH 031/164] Interfaces: check for u, z dependencies of terminal cost and constraints (#1502) --- .../generate_c_code_ext_cost.m | 6 +++++ .../generate_c_code_nonlinear_constr.m | 6 +++++ .../generate_c_code_nonlinear_least_squares.m | 6 +++++ .../casadi_function_generation.py | 26 ++++++++++++++++--- 4 files changed, 41 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m index b01b0e5ae3..732b3c43e1 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m +++ b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m @@ -80,6 +80,12 @@ function generate_c_code_ext_cost(context, model, target_dir, stage_type) error('Field `cost_expr_ext_cost_e` is required for cost_type_e == EXTERNAL.') end ext_cost_e = model.cost_expr_ext_cost_e; + if any(which_depends(ext_cost_e, model.u)) + error('terminal cost cannot depend on u.'); + end + if any(which_depends(ext_cost_e, model.z)) + error('terminal cost cannot depend on z.'); + end % generate jacobians jac_x_e = jacobian(ext_cost_e, x); % generate hessians diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m index cc20f83d25..9df8ed0754 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m @@ -110,6 +110,12 @@ function generate_c_code_nonlinear_constr(context, model, target_dir, stage_type elseif strcmp(stage_type, 'terminal') % NOTE: terminal node has no u, z h_e = model.con_h_expr_e; + if any(which_depends(h_e, model.u)) + error('terminal constraints cannot depend on u.'); + end + if any(which_depends(h_e, model.z)) + error('terminal constraints cannot depend on z.'); + end % multipliers for hessian nh_e = length(h_e); if isSX diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m index 4fae2178fc..3e3c20f5a2 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m @@ -118,6 +118,12 @@ function generate_c_code_nonlinear_least_squares(context, model, target_dir, sta dy_dz = jacobian(fun, z); % output symbolics ny_e = length(fun); + if any(which_depends(fun, model.u)) + error('terminal cost cannot depend on u.'); + end + if any(which_depends(fun, model.z)) + error('terminal cost cannot depend on z.'); + end if isSX y_e = SX.sym('y', ny_e, 1); u = SX.sym('u', 0, 0); diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 56eb08390f..4f379376e1 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -459,6 +459,11 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, ext_cost = model.cost_expr_ext_cost_e custom_hess = model.cost_expr_ext_cost_custom_hess_e # Last stage cannot depend on u and z + if any(ca.which_depends(ext_cost, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(ext_cost, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z u = symbol("u", 0, 0) z = symbol("z", 0, 0) @@ -532,8 +537,14 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage if stage_type == 'terminal': middle_name = '_cost_y_e' - u = symbol('u', 0, 0) y_expr = model.cost_y_expr_e + if any(ca.which_depends(y_expr, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(y_expr, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z + u = symbol("u", 0, 0) + z = symbol("z", 0, 0) elif stage_type == 'initial': middle_name = '_cost_y_0' @@ -589,7 +600,6 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag p_global = symbol('p_global', 0, 0) if stage_type == 'terminal': - u = symbol('u', 0, 0) yref = model.cost_r_in_psi_expr_e inner_expr = model.cost_y_expr_e - yref @@ -600,7 +610,13 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' custom_hess = model.cost_conl_custom_outer_hess_e - + if any(ca.which_depends(inner_expr, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(inner_expr, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z + u = symbol("u", 0, 0) + z = symbol("z", 0, 0) elif stage_type == 'initial': u = model.u @@ -688,6 +704,10 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con constr_type = constraints.constr_type_e con_h_expr = model.con_h_expr_e con_phi_expr = model.con_phi_expr_e + if any(ca.which_depends(con_h_expr, model.u)) or any(ca.which_depends(con_phi_expr, model.u)): + raise ValueError("terminal constraints cannot depend on u.") + if any(ca.which_depends(con_h_expr, model.z)) or any(ca.which_depends(con_phi_expr, model.z)): + raise ValueError("terminal constraints cannot depend on z.") # create dummy u, z u = symbol('u', 0, 0) z = symbol('z', 0, 0) From a7ea2eb9dc770aae205e81ae61afa04c33560a22 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 09:57:01 +0200 Subject: [PATCH 032/164] Condense `qp_out` (#1474) Closes https://github.com/acados/acados/issues/1433 Also: - fix setting warm_start_first_qp_from_nlp in RTI - Python: allow updating warm start options on the fly - update documentation and requirements for solution sensitivity computation --- .github/workflows/full_build.yml | 1 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 5 + acados/ocp_qp/ocp_qp_full_condensing.c | 13 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 60 +------- .../ocp/initialization_test.py | 130 ++++++++++++++++++ .../sensitivity_utils.py | 1 + .../smooth_policy_gradients.py | 3 + external/hpipm | 2 +- .../acados_template/acados_ocp.py | 3 + .../acados_template/acados_ocp_solver.py | 13 +- 10 files changed, 166 insertions(+), 65 deletions(-) create mode 100644 examples/acados_python/pendulum_on_cart/ocp/initialization_test.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 59b63508d1..cae5cddaed 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -207,6 +207,7 @@ jobs: python batch_adjoint_solution_sensitivity_example.py python non_ocp_example.py cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + python initialization_test.py python ocp_example_cost_formulations.py diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 945d985593..2c0d8793c1 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -191,6 +191,11 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, int* rti_log_only_available_residuals = (int *) value; opts->rti_log_only_available_residuals = *rti_log_only_available_residuals; } + else if (!strcmp(field, "warm_start_first_qp_from_nlp")) + { + bool* warm_start_first_qp_from_nlp = (bool *) value; + opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; + } else if (!strcmp(field, "as_rti_level")) { int* as_rti_level = (int *) value; diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index 7ae076e1ad..72f229e9c8 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -486,9 +486,16 @@ int ocp_qp_full_condensing(void *qp_in_, void *fcond_qp_in_, void *opts_, void * int ocp_qp_full_condensing_condense_qp_out(void *qp_in_, void *fcond_qp_in_, void *qp_out_, void *fcond_qp_out_, void *opts_, void *mem_, void *work) { - printf("ocp_qp_full_condensing_condense_qp_out: not implemented\n"); - printf("what about implementing it? :) do it for acados!\n"); - exit(1); + ocp_qp_in *qp_in = qp_in_; + ocp_qp_out *qp_out = qp_out_; + dense_qp_out *fcond_qp_out = fcond_qp_out_; + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + d_ocp_qp_reduce_eq_dof_sol(qp_in, qp_out, mem->red_sol, opts->hpipm_red_opts, mem->hpipm_red_work); + d_cond_qp_cond_sol(mem->red_qp, mem->red_sol, fcond_qp_out, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + return ACADOS_SUCCESS; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index ae4f538a42..0ff4ab68e2 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -541,67 +541,15 @@ int ocp_qp_partial_condensing(void *qp_in_, void *pcond_qp_in_, void *opts_, voi int ocp_qp_partial_condensing_condense_qp_out(void *qp_in_, void *pcond_qp_in_, void *qp_out_, void *pcond_qp_out_, void *opts_, void *mem_, void *work) { - // ocp_qp_in *qp_in = qp_in_; - // ocp_qp_in *pcond_qp_in = pcond_qp_in_; + ocp_qp_in *qp_in = qp_in_; + ocp_qp_in *pcond_qp_in = pcond_qp_in_; ocp_qp_out *qp_out = qp_out_; ocp_qp_out *pcond_qp_out = pcond_qp_out_; ocp_qp_partial_condensing_opts *opts = opts_; ocp_qp_partial_condensing_memory *mem = mem_; - assert(opts->N2 == opts->N2_bkp); - ocp_qp_dims *orig_dims = mem->dims->orig_dims; - - if (opts->N2 != mem->dims->orig_dims->N) - { - printf("\nocp_qp_partial_condensing_condense_qp_out: only works if N==N2 for now.\n"); - exit(1); - } - if (orig_dims->nbxe[0] != 0 && (orig_dims->nbxe[0] != orig_dims->nbx[0] || orig_dims->nx[0] != orig_dims->nbx[0])) - { - printf("\nocp_qp_partial_condensing_condense_qp_out: only works if nbxe[0] == nbx[0] == nx[0], or nbxe[0] == 0 for now.\n"); - exit(1); - } - - int N = orig_dims->N; - int *nx = orig_dims->nx; - int *nu = orig_dims->nu; - int *nbu = orig_dims->nbu; - int *nbx = orig_dims->nbx; - int *ng = orig_dims->ng; - int *ns = orig_dims->ns; - int i; - - if (orig_dims->nbxe[0] != 0) - { - // uxs 0 - blasfeo_dveccp(nu[0], qp_out->ux+0, 0, pcond_qp_out->ux+0, 0); - blasfeo_dveccp(2 * ns[0], qp_out->ux+0, nu[0]+nx[0], pcond_qp_out->ux+0, nu[0]); - // lam 0 - blasfeo_dveccp(nbu[0], qp_out->lam+0, 0, pcond_qp_out->lam+0, 0); - blasfeo_dveccp(ng[0], qp_out->lam+0, nbu[0]+nbx[0], pcond_qp_out->lam+0, nbu[0]); - blasfeo_dveccp(nbu[0], qp_out->lam+0, nbu[0]+nbx[0]+ng[0], pcond_qp_out->lam+0, nbu[0]+ng[0]); - blasfeo_dveccp(ng[0], qp_out->lam+0, 2*(nbu[0]+nbx[0])+ng[0], pcond_qp_out->lam+0, 2*nbu[0]+ng[0]); - // t 0 - blasfeo_dveccp(nbu[0], qp_out->t+0, 0, pcond_qp_out->t+0, 0); - blasfeo_dveccp(ng[0], qp_out->t+0, nbu[0]+nbx[0], pcond_qp_out->t+0, nbu[0]); - blasfeo_dveccp(nbu[0], qp_out->t+0, nbu[0]+nbx[0]+ng[0], pcond_qp_out->t+0, nbu[0]+ng[0]); - blasfeo_dveccp(ng[0], qp_out->t+0, 2*(nbu[0]+nbx[0])+ng[0], pcond_qp_out->t+0, 2*nbu[0]+ng[0]); - } - else - { - i = 0; - blasfeo_dveccp(nx[i] + nu[i] + 2 * ns[i], qp_out->ux+i, 0, pcond_qp_out->ux+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->lam+i, 0, pcond_qp_out->lam+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->t+i, 0, pcond_qp_out->t+i, 0); - } - for (i = 1; i<=N; i++) - { - blasfeo_dveccp(nx[i] + nu[i] + 2 * ns[i], qp_out->ux+i, 0, pcond_qp_out->ux+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->lam+i, 0, pcond_qp_out->lam+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->t+i, 0, pcond_qp_out->t+i, 0); - } - for (i = 0; ipi+i, 0, pcond_qp_out->pi+i, 0); + d_ocp_qp_reduce_eq_dof_sol(qp_in, qp_out, mem->red_sol, opts->hpipm_red_opts, mem->hpipm_red_work); + d_part_cond_qp_cond_sol(mem->red_qp, pcond_qp_in, mem->red_sol, pcond_qp_out, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); return ACADOS_SUCCESS; } diff --git a/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py b/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py new file mode 100644 index 0000000000..f2259acdaa --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py @@ -0,0 +1,130 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +import sys +sys.path.insert(0, '../common') + +from acados_template import AcadosOcp, AcadosOcpSolver +from pendulum_model import export_pendulum_ode_model +import numpy as np +from utils import plot_pendulum + +import casadi as ca + + +def main(qp_solver: str = 'PARTIAL_CONDENSING_HPIPM'): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + Tf = 1.0 + nx = model.x.rows() + nu = model.u.rows() + N = 20 + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + # set options + ocp.solver_options.qp_solver = qp_solver + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'IRK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' + ocp.solver_options.qp_tol = 1e-8 + + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + status = ocp_solver.solve() + ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") + + if status != 0: + raise Exception(f'acados returned status {status}.') + + sol = ocp_solver.store_iterate_to_obj() + qp_iter = ocp_solver.get_stats("qp_iter") + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 10, f"cold start should require 10 iterations, got {nlp_iter}" + + ocp_solver.load_iterate_from_obj(sol) + ocp_solver.solve() + ocp_solver.print_statistics() + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 0, f"hot start should require 0 iterations, got {nlp_iter}" + + disturbed_sol = sol + disturbed_sol.x_traj[0] += 0.1 * disturbed_sol.x_traj[0] + ocp_solver.load_iterate_from_obj(disturbed_sol) + status = ocp_solver.solve() + ocp_solver.print_statistics() + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 6, f"warm start should require 6 iterations, got {nlp_iter}" + + ocp_solver.load_iterate_from_obj(disturbed_sol) + ocp_solver.options_set("qp_warm_start", 1) + ocp_solver.options_set("warm_start_first_qp_from_nlp", True) + ocp_solver.options_set("warm_start_first_qp", True) + # ocp_solver.options_set("qp_mu0", 1e-3) + status = ocp_solver.solve() + ocp_solver.print_statistics() + + +if __name__ == '__main__': + main('FULL_CONDENSING_HPIPM') + main('PARTIAL_CONDENSING_HPIPM') diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py index a8b8da621a..59daada8a4 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py @@ -167,6 +167,7 @@ def export_parametric_ocp( ocp.solver_options.tf = T_horizon ocp.solver_options.qp_solver_ric_alg = qp_solver_ric_alg + ocp.solver_options.qp_solver_cond_ric_alg = qp_solver_ric_alg ocp.solver_options.qp_solver_mu0 = 1e3 # makes HPIPM converge more robustly ocp.solver_options.hessian_approx = hessian_approx ocp.solver_options.nlp_solver_max_iter = 400 diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py index e44bc8acd3..255d780656 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py @@ -103,6 +103,9 @@ def create_solvers(x0, use_cython=False, qp_solver_ric_alg=0, # create sensitivity solver ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, hessian_approx='EXACT', qp_solver_ric_alg=qp_solver_ric_alg, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) + # test with QP solver that does condensing: not recommended for sensitivtity solver + ocp.solver_options.qp_solver_cond_N = int(N_horizon/4) + ocp.model.name = 'sensitivity_solver' ocp.code_export_directory = f'c_generated_code_{ocp.model.name}' if use_cython: diff --git a/external/hpipm b/external/hpipm index aebf3bcfb4..185517cf53 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit aebf3bcfb4082a7b0d0c20ccea2f8a7dc00049ff +Subproject commit 185517cf539598cf173b45fd4d4ea2a74eed3f38 diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 4aa465568e..229f0dc1c9 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1037,6 +1037,9 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): raise NotImplementedError(f"with_solution_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + if opts.qp_solver_cond_N != opts.N_horizon or opts.qp_solver.startswith("FULL_CONDENSING"): + if opts.qp_solver_cond_ric_alg != 0: + print("Warning: Parametric sensitivities with condensing should be used with qp_solver_cond_ric_alg=0, as otherwise the full space Hessian needs to be factorized and the algorithm cannot handle indefinite ones.") if opts.with_value_sens_wrt_params: if dims.np_global == 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 2f443f0377..03cfc6e1aa 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -497,8 +497,8 @@ def setup_qp_matrices_and_factorize(self) -> int: This is only implemented for HPIPM QP solver without condensing. """ - if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM' or self.__solver_options["qp_solver_cond_N"] != self.N: - raise NotImplementedError('This function is only implemented for HPIPM QP solver without condensing!') + if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM': + raise NotImplementedError('This function is only implemented for PARTIAL_CONDENSING_HPIPM!') self.status = getattr(self.shared_lib, f"{self.name}_acados_setup_qp_matrices_and_factorize")(self.capsule) @@ -742,11 +742,12 @@ def eval_solution_sensitivity(self, (3) positive definiteness of the full-space Hessian if the square-root version of the Riccati recursion is used OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), \n - (4) the solution of at least one QP in advance to evaluation of the sensitivities as the factorization is reused. + (4) the last interaction before calling this function should involve the solution of the QP at the NLP solution. + This can happen as call to `solve()` with at least 1 QP being solved or `setup_qp_matrices_and_factorize()`, \n .. note:: Timing of the sensitivities computation consists of time_solution_sens_lin, time_solution_sens_solve. .. note:: Solution sensitivities with respect to parameters are currently implemented assuming the parameter vector p is global within the OCP, i.e. p=p_i with i=0, ..., N. - .. note:: Solution sensitivities with respect to parameters are currently implemented only for parametric discrete dynamics and parametric external costs (in particular, parametric constraints are not covered). + .. note:: Solution sensitivities with respect to parameters are currently implemented only for parametric discrete dynamics, parametric external costs and parametric nonlinear constraints (h). """ if with_respect_to == "params_global": @@ -881,6 +882,8 @@ def eval_adjoint_solution_sensitivity(self, The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (nu, n_seeds). :param with_respect_to : string in ["p_global"] :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True + + The correct computation of solution of adjoint sensitivities has the same requirements as the computation of solution sensitivities, see the documentation of `eval_solution_sensitivity`. """ # get n_seeds @@ -2268,7 +2271,7 @@ def options_set(self, field_, value_): 'qp_tau_min', 'qp_mu0'] string_fields = [] - bool_fields = ['with_adaptive_levenberg_marquardt'] + bool_fields = ['with_adaptive_levenberg_marquardt', 'warm_start_first_qp_from_nlp', 'warm_start_first_qp'] # check field availability and type if field_ in int_fields: From 3cb5f3102060b51aaecfc9abd7a42a0ea411a1d9 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 09:59:35 +0200 Subject: [PATCH 033/164] Document cmake options (#1503) Plus: set CMake default `HPIPM_TARGET` to `GENERIC`, previously used `X64_AUTOMATIC` default, does actually not exist, see https://github.com/giaf/hpipm/pull/184 --- CMakeLists.txt | 2 +- docs/installation/index.md | 38 ++++++++++++++++++++++++++++++++++---- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 17fc936b3e..f7e6bf8aaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,8 +64,8 @@ if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64") else() set(BLASFEO_TARGET "X64_AUTOMATIC" CACHE STRING "BLASFEO Target architecture") endif() -set(HPIPM_TARGET "X64_AUTOMATIC" CACHE STRING "HPIPM Target architecture") set(LA "HIGH_PERFORMANCE" CACHE STRING "Linear algebra optimization level") +set(HPIPM_TARGET "GENERIC" CACHE STRING "HPIPM Target architecture") if (CMAKE_SYSTEM_NAME MATCHES "Windows") set(BUILD_SHARED_LIBS OFF CACHE STRING "Build shared libraries") diff --git a/docs/installation/index.md b/docs/installation/index.md index fbcb942b2c..ca54cba5e6 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -24,12 +24,42 @@ Install `acados` as follows: mkdir -p build cd build cmake -DACADOS_WITH_QPOASES=ON .. -# add more optional arguments e.g. -DACADOS_WITH_OSQP=OFF/ON -DACADOS_INSTALL_DIR= above +# add more optional arguments e.g. -DACADOS_WITH_DAQP=ON, a list of CMake options is provided below make install -j4 ``` -NOTE: you can set the `BLASFEO_TARGET` in `/CMakeLists.txt`. -For a list of supported targets, we refer to https://github.com/giaf/blasfeo/blob/master/README.md . -The default is `X64_AUTOMATIC`, which attempts to determine the best available target for your machine. + +#### CMake options: +Below is a list of CMake options available for configuring the `acados` build. +These options can be passed to the `cmake` command using the `-D` flag, e.g., `cmake -DOPTION_NAME=VALUE ..`. +Adjust these options based on your requirements. + +| **Option Name** | **Description** | **Default Value** | +|--------------------------------|------------------------------------------------------------|-------------------| +| `ACADOS_WITH_QPOASES` | Compile acados with optional QP solver qpOASES | `OFF` | +| `ACADOS_WITH_DAQP` | Compile acados with optional QP solver DAQP | `OFF` | +| `ACADOS_WITH_QPDUNES` | Compile acados with optional QP solver qpDUNES | `OFF` | +| `ACADOS_WITH_OSQP` | Compile acados with optional QP solver OSQP | `OFF` | +| `ACADOS_WITH_HPMPC` | Compile acados with optional QP solver HPMPC | `OFF` | +| `ACADOS_WITH_QORE` | Compile acados with optional QP solver QORE (experimental) | `OFF` | +| `ACADOS_WITH_OOQP` | Compile acados with optional QP solver OOQP (experimental) | `OFF` | +| `BLASFEO_TARGET` | BLASFEO Target architecture, see BLASFEO repository for more information. Possible values include: `X64_AUTOMATIC`, `GENERIC`, `X64_INTEL_SKYLAKE_X`, `X64_INTEL_HASWELL`, `X64_INTEL_SANDY_BRIDGE`, `X64_INTEL_CORE`, `X64_AMD_BULLDOZER`, `ARMV8A_APPLE_M1`, `ARMV8A_ARM_CORTEX_A76`, `ARMV8A_ARM_CORTEX_A73`, `ARMV8A_ARM_CORTEX_A57`, `ARMV8A_ARM_CORTEX_A55`, `ARMV8A_ARM_CORTEX_A53`, `ARMV7A_ARM_CORTEX_A15`, `ARMV7A_ARM_CORTEX_A9`, `ARMV7A_ARM_CORTEX_A7` | `X64_AUTOMATIC` | +| `LA` | Linear algebra optimization level for BLASFEO | `HIGH_PERFORMANCE`| +| `ACADOS_WITH_SYSTEM_BLASFEO` | Use BLASFEO found via `find_package(blasfeo)` instead of compiling it | `OFF` | +| `HPIPM_TARGET` | HPIPM Target architecture. Possible values: `AVX`, `GENERIC` | `GENERIC` | +| `ACADOS_WITH_OPENMP` | OpenMP parallelization | `OFF` | +| `ACADOS_NUM_THREADS` | Number of threads for OpenMP parallelization within one NLP solver. If not set, `omp_get_max_threads` will be used to determine the number of threads. If multiple solves should be parallelized, e.g. with an `AcadosOcpBatchSolver` or `AcadosSimBatchSolver`, set this to 1. | Not set | +| `ACADOS_SILENT` | No console status output | `OFF` | +| `ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE` | Print QP inputs and outputs to file in SQP | `OFF` | +| `CMAKE_BUILD_TYPE` | Build type (e.g., Release, Debug, etc.) | `Release` | +| `ACADOS_UNIT_TESTS` | Compile unit tests | `OFF` | +| `ACADOS_EXAMPLES` | Compile C examples | `OFF` | +| `ACADOS_OCTAVE` | Octave interface CMake tests | `OFF` | +| `ACADOS_PYTHON` | Python interface CMake tests (Note: Python interface installation is independent of this) | `OFF` | +| `BUILD_SHARED_LIBS` | Build shared libraries | `ON` (non-Windows)| + + + +For more details on specific options, refer to the comments in the `CMakeLists.txt` file. #### **Make** (not recommended) NOTE: This build system is not actively tested and might be removed in the future! It is strongly recommended to use the `CMake` build system. From 2be594dad9fe97336b17f9294dec48d75552f9e8 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 10:18:24 +0200 Subject: [PATCH 034/164] Update README to highlight the most important acados features (#1504) Co-authored-by: sandmaennchen --- README.md | 47 ++++++++++++++++++++++++++++++++++++++++----- docs/index.md | 53 +++++++++++++++++++++++++++++++++++++++++---------- 2 files changed, 85 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 4ffd7aa7ff..383364a6a6 100644 --- a/README.md +++ b/README.md @@ -6,14 +6,28 @@ ![Github actions full build workflow](https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg) -`acados` provides fast and embedded solvers for nonlinear optimal control. +`acados` provides fast and embedded solvers for nonlinear optimal control, specifically designed for real-time applications and embedded systems. It is written in `C` and offers interfaces to the programming languages `Python`, `MATLAB` and `Octave`. ## General -- `acados` implements - 1. fast SQP-type solvers for Nonlinear Programming (NLP) formulations with an Optimal Control Problem (OCP) structure - 2. efficient integration methods, also called *integrators*, to solve initial value problems with dynamic systems given as an ODE or index-1 DAE. - These integrators can efficiently compute first and second-order sensitivities of the results. +`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make `acados` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). + +### Key Features: +Some key features of `acados` are summarized in the following. +The [software design](#design-paradigms) allows to implement many algorithms beyond this list. +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. +`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). ## Documentation - Documentation can be found on [docs.acados.org](https://docs.acados.org/) @@ -28,3 +42,26 @@ It is written in `C` and offers interfaces to the programming languages `Python` ## Installation - Instructions can be found at [docs.acados.org/installation](https://docs.acados.org/installation) + +### Design paradigms +The main design paradigms of `acados` are +- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. +Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. +Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. +- **modularity**: +`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. +More precisely, all problem functions and dimensions can vary between all stages. +Such problems are often called *multi-stage* or *multi-phase* problems. +Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. +Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. +The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. + +## Fields of applications +A non-exhaustive list of projects featuring `acados` is available at [docs.acados.org/list_of_projects](https://docs.acados.org/list_of_projects/index.html). +Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. +- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. +- Energy Systems: Optimization-based control for microgrids and wind turbines. +- Biomechanics: Optimal control in biomechanics through libraries like bioptim. +- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. diff --git a/docs/index.md b/docs/index.md index a2f6118401..f31ad6fddd 100644 --- a/docs/index.md +++ b/docs/index.md @@ -32,21 +32,54 @@ Contributions via pull requests are welcome! ## About `acados` -`acados` is a software package providing fast and embedded solvers for nonlinear optimal control. -Problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework and the high-level `acados` interfaces. - -`acados` provides a collection of computationally efficient building blocks tailored to optimal control structured problems, most prominently optimal control problems (OCP) and moving horizon estimation (MHE) problems. -Among others, `acados` implements: -- modules for the integration of ordinary differential equations (ODE) and differential-algebraic equations (DAE), -- interfaces to state-of-the-art QP solvers like [`HPIPM`](https://github.com/giaf/hpipm), `qpOASES`, [`DAQP`](https://github.com/darnstrom/daqp) and [`OSQP`](https://github.com/osqp/osqp) -- (partial) condensing routines, provided by `HPIPM` -- nonlinear programming solvers for optimal control structured problems -- real-time algorithms, such as the real-time iteration (RTI) and advanced-step real-time iteration (AS-RTI) algorithms +`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make `acados` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). + +`acados` is written in `C`, but control problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework via the high-level `acados` interfaces to the programming languages `Python`, `MATLAB` and `Octave`. + +Some key features of `acados` are summarized in the following. +The [software design](#design-paradigms) allows to implement many algorithms beyond this list. +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. +`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). The back-end of acados uses the high-performance linear algebra package [`BLASFEO`](https://github.com/giaf/blasfeo), in order to boost computational efficiency for small to medium scale matrices typical of embedded optimization applications. `MATLAB`, `Octave` and `Python` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. +### Design paradigms +The main design paradigms of `acados` are +- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. +Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. +Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. +- **modularity**: +`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. +More precisely, all problem functions and dimensions can vary between all stages. +Such problems are often called *multi-stage* or *multi-phase* problems. +Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. +Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. +The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. + +## Fields of applications +A non-exhaustive list of projects featuring `acados` is available [here](https://docs.acados.org/list_of_projects/index.html). +Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. +- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. +- Energy Systems: Optimization-based control for microgrids and wind turbines. +- Biomechanics: Optimal control in biomechanics through libraries like bioptim. +- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. + + # Documentation page overview ```eval_rst From 398d7a6c98af535c7c6a343cbc4d99511f6695d3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 18:02:00 +0200 Subject: [PATCH 035/164] Simulink: `levenberg_marquardt` as optional input (#1507) --- .../matlab_templates/acados_solver_sfun.in.c | 15 +++++++++++++++ .../matlab_templates/make_sfun.in.m | 6 ++++++ .../acados_template/simulink_default_opts.json | 3 ++- 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 1381bd4d48..c1c8f12cac 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -286,6 +286,10 @@ static void mdlInitializeSizes (SimStruct *S) {%- set n_inputs = n_inputs + 1 -%} {%- endif -%} + {%- if simulink_opts.inputs.levenberg_marquardt -%} {#- levenberg_marquardt #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} {%- for input_name, input_spec in simulink_opts.customizable_inputs -%} @@ -589,6 +593,11 @@ static void mdlInitializeSizes (SimStruct *S) ssSetInputPortVectorDimension(S, {{ i_input }}, 1); {%- endif -%} + {%- if simulink_opts.inputs.levenberg_marquardt -%} {#- levenberg_marquardt #} + {%- set i_input = i_input + 1 %} + // levenberg_marquardt + ssSetInputPortVectorDimension(S, {{ i_input }}, 1); + {%- endif -%} {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} @@ -1235,6 +1244,12 @@ static void mdlOutputs(SimStruct *S, int_T tid) ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); {%- endif %} + {%- if simulink_opts.inputs.levenberg_marquardt %} {#- levenberg_marquardt #} + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + double levenberg_marquardt = (double)(*in_sign[0]); + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + {%- endif %} {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m index 4bb9f3f3d8..d5b9b6336f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m @@ -430,6 +430,12 @@ i_in = i_in + 1; {%- endif %} +{%- if simulink_opts.inputs.levenberg_marquardt %} {#- levenberg_marquardt #} +input_note = strcat(input_note, num2str(i_in), ') levenberg_marquardt, size [1]\n '); +sfun_input_names = [sfun_input_names; 'levenberg_marquardt [1]']; +i_in = i_in + 1; +{%- endif %} + {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} {%- for input_name, input_spec in simulink_opts.customizable_inputs -%} diff --git a/interfaces/acados_template/acados_template/simulink_default_opts.json b/interfaces/acados_template/acados_template/simulink_default_opts.json index 0ed61ec488..852229681b 100644 --- a/interfaces/acados_template/acados_template/simulink_default_opts.json +++ b/interfaces/acados_template/acados_template/simulink_default_opts.json @@ -53,7 +53,8 @@ "u_init": 0, "pi_init": 0, "slacks_init": 0, - "rti_phase": 0 + "rti_phase": 0, + "levenberg_marquardt": 0 }, "samplingtime": "t0" } From cca1902a8162a1534724e2da1d1837056e461ceb Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 25 Apr 2025 16:54:37 +0200 Subject: [PATCH 036/164] Add real-world examples with videos to documentation page (#1508) --- docs/conf.py | 6 +- docs/index.md | 117 --------------------------- docs/index.rst | 124 +++++++++++++++++++++++++++++ docs/list_of_projects/index.md | 6 +- docs/real_world_examples/index.rst | 80 +++++++++++++++++++ docs/requirements.txt | 6 +- docs/troubleshooting/index.md | 4 + 7 files changed, 221 insertions(+), 122 deletions(-) delete mode 100644 docs/index.md create mode 100644 docs/index.rst create mode 100644 docs/real_world_examples/index.rst diff --git a/docs/conf.py b/docs/conf.py index a090199e46..310c0d9830 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -81,7 +81,11 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables'] +extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables', + 'sphinx.ext.intersphinx', + 'sphinxcontrib.youtube', + 'sphinxemoji.sphinxemoji', +] # Add any paths that contain templates here, relative to this directory. diff --git a/docs/index.md b/docs/index.md deleted file mode 100644 index f31ad6fddd..0000000000 --- a/docs/index.md +++ /dev/null @@ -1,117 +0,0 @@ -# acados - - -```eval_rst - -|github-workflow-full-build| -|github-workflow-c_test_blasfeo_reference| -|appveyor-build| - -.. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg - :target: https://github.com/acados/acados/actions/workflows/full_build.yml - :alt: Github workflow status - -.. |github-workflow-c_test_blasfeo_reference| image:: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml/badge.svg - :target: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml - :alt: Github workflow status - - -.. |appveyor-build| image:: https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true - :target: https://ci.appveyor.com/project/roversch/acados - :alt: Appveyor workflow status - -``` - -Fast and embedded solvers for nonlinear optimal control. - -- `acados` source code is hosted on [Github](https://github.com/acados/acados). -Contributions via pull requests are welcome! -- `acados` has a discourse based [forum](https://discourse.acados.org/). -- `acados` is mainly developed by the [syscop group around Prof. Moritz Diehl, the Systems Control and Optimization Laboratory, at the University of Freiburg](https://www.syscop.de/). - - -## About `acados` - -`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. -Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. -The computational efficiency and modularity make `acados` an ideal choice for real-time applications. -It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). - -`acados` is written in `C`, but control problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework via the high-level `acados` interfaces to the programming languages `Python`, `MATLAB` and `Octave`. - -Some key features of `acados` are summarized in the following. -The [software design](#design-paradigms) allows to implement many algorithms beyond this list. -- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. -- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. -- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. -- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. -- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. -- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. -- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. -- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. -- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. -`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). - -The back-end of acados uses the high-performance linear algebra package [`BLASFEO`](https://github.com/giaf/blasfeo), in order to boost computational efficiency for small to medium scale matrices typical of embedded optimization applications. -`MATLAB`, `Octave` and `Python` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. - - -### Design paradigms -The main design paradigms of `acados` are -- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. -Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. -Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. -- **modularity**: -`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. -More precisely, all problem functions and dimensions can vary between all stages. -Such problems are often called *multi-stage* or *multi-phase* problems. -Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. -Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. -- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. -The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. - -## Fields of applications -A non-exhaustive list of projects featuring `acados` is available [here](https://docs.acados.org/list_of_projects/index.html). -Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. -- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. -- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. -- Energy Systems: Optimization-based control for microgrids and wind turbines. -- Biomechanics: Optimal control in biomechanics through libraries like bioptim. -- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. - - -# Documentation page overview - -```eval_rst -Documentation latest build: |today| -``` - -```eval_rst -.. toctree:: - :maxdepth: 2 - - Home - citing/index - installation/index - list_of_projects/index - developer_guide/index - -.. toctree:: - :maxdepth: 2 - :caption: Interfaces - - interfaces/index - python_interface/index - matlab_octave_interface/index - embedded_workflow/index - -.. toctree:: - :maxdepth: 2 - :caption: User Guide - - problem_formulation/index - troubleshooting/index - features/index -``` - - \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000000..bdc33bbd3f --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,124 @@ +acados +====== + +.. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg + :target: https://github.com/acados/acados/actions/workflows/full_build.yml + :alt: Github workflow status + +.. |github-workflow-c_test_blasfeo_reference| image:: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml/badge.svg + :target: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml + :alt: Github workflow status + +.. |appveyor-build| image:: https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true + :target: https://ci.appveyor.com/project/roversch/acados + :alt: Appveyor workflow status + +|github-workflow-full-build| +|github-workflow-c_test_blasfeo_reference| +|appveyor-build| + +Fast and embedded solvers for real-world applications of nonlinear optimal control. + + +Important links +---------------- +|:cinema:| Get inspired by `real-world applications using acados `_ |:rocket:| + +|:star:| The ``acados`` **source code** is hosted on `Github `_. +Contributions via pull requests are welcome! + +|:handshake:| ``acados`` has a discourse-based `forum `_. + +|:homes:| ``acados`` is mainly developed by the `syscop group around Prof. Moritz Diehl, at the University of Freiburg `_. + +About ``acados`` +---------------- + +``acados`` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make ``acados`` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in `a wide range of applications `_. + +``acados`` is written in ``C``, but control problems can be conveniently formulated using the `CasADi `_ symbolic framework via the high-level ``acados`` interfaces to the programming languages ``Python``, ``MATLAB``, and ``Octave``. + +Some key features of ``acados`` are summarized in the following. +The `software design <#design-paradigms>`_ allows implementing many algorithms beyond this list: + +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or, in particular, policy gradients. + ``acados`` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture, as demonstrated in the `leap-c project `_. + +The back-end of ``acados`` uses the high-performance linear algebra package `BLASFEO `_, in order to boost computational efficiency for small to medium-scale matrices typical of embedded optimization applications. +``MATLAB``, ``Octave``, and ``Python`` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. + +Design paradigms +---------------- + +The main design paradigms of ``acados`` are: + +- **Efficiency**: Realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as ``HPIPM``, and (partial) condensing methods to transform QPs, enabling their efficient treatment. + Moreover, the common structure of slack variables, which, for example, occur when formulating soft constraints, can be exploited. + Additionally, a structure-exploiting Runge-Kutta method is implemented, allowing the utilization of linear dependencies within dynamical system models. +- **Modularity**: ``acados`` offers an extremely flexible problem formulation, allowing not only the formulation of problems that occur in MPC and MHE. + More precisely, all problem functions and dimensions can vary between all stages. + Such problems are often called *multi-stage* or *multi-phase* problems. + Different NLP solvers, QP solvers, integration methods, regularization methods, and globalization methods can be combined freely. + Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **Usability**: The interfaces to Python, MATLAB, Simulink, and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular `CasADi `_ symbolic software framework. + The interfaces allow users to conveniently specify commonly used problem formulations via the ``AcadosOcp`` class and additionally expose the full flexibility of the internal ``acados`` problem formulation, via multi-phase formulations and ``AcadosMultiphaseOcp``. + +Fields of applications +---------------------- + +A non-exhaustive list of projects featuring ``acados`` is available `here `_. +Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. + +- **Robotics**: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- **Autonomous Vehicles**: Used in projects like openpilot in driving assistance systems. +- **Energy Systems**: Optimization-based control for microgrids and wind turbines. +- **Biomechanics**: Optimal control in biomechanics through libraries like bioptim. +- **Aerospace**: Applications in trajectory optimization and control for drones and morphing-wing aircraft. + +Documentation page overview +--------------------------- + +Documentation latest build: |today| + +.. toctree:: + :maxdepth: 2 + + Home + real_world_examples/index + citing/index + installation/index + list_of_projects/index + +.. toctree:: + :maxdepth: 2 + :caption: Interfaces + + interfaces/index + python_interface/index + matlab_octave_interface/index + +.. toctree:: + :maxdepth: 2 + :caption: User Guide + + problem_formulation/index + troubleshooting/index + features/index + +.. toctree:: + :maxdepth: 2 + :caption: Advanced + + developer_guide/index + embedded_workflow/index diff --git a/docs/list_of_projects/index.md b/docs/list_of_projects/index.md index d8015e0f6f..de39697cd4 100644 --- a/docs/list_of_projects/index.md +++ b/docs/list_of_projects/index.md @@ -1,5 +1,8 @@ -# Other Projects that feature `acados` +# Related Projects + + ## Software interfaced with `acados` - [Rockit (Rapid Optimal Control kit)](https://gitlab.kuleuven.be/meco-software/rockit) @@ -39,7 +42,6 @@ It also provides a higher level interface to `acados`, which is based on the Mat - [NMPC for Racing Using a Singularity-Free Path-Parametric Model with Obstacle Avoidance](https://cdn.syscop.de/publications/Kloeser2020.pdf) - [Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://arxiv.org/abs/2105.05998) - - [Video to Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://www.dropbox.com/sh/mkr4pftcug6jlo7/AABNqu1AsGED2WSR8IqvaiUla?dl=0) - [Continuous Control Set Nonlinear Model Predictive Control of Reluctance Synchronous Machines - IEEE Transactions on Control System Technology](https://ieeexplore.ieee.org/document/9360312) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst new file mode 100644 index 0000000000..ce4249e321 --- /dev/null +++ b/docs/real_world_examples/index.rst @@ -0,0 +1,80 @@ +.. _real_world_examples: + +================== +Real-world examples +================== + +This page shows some real-world examples enabled by ``acados``. +The list is not complete and is meant to show a variety of domains and applications. +It is intended to inspire and impress. +If you have awesome videos with ``acados``-based controllers in action, reach out to get featured. +Enjoy! |:popcorn:| + +.. Check this documentation for embedding YouTube videos: +.. https://sphinxcontrib-youtube.readthedocs.io/en/latest/usage.html + + +.. rubric:: Drone racing: Quadrotor control +|:linked_paperclips:| `Romero et al. (2022) `_ + +.. youtube:: zBVpx3bgI6E + :width: 50% + :url_parameters: ?start=86 + + +.. rubric:: Driving assistance: acados is used within openpilot +See blog post `here `_ + +.. youtube:: 0aq4Wi2rsOk + :width: 50% + :url_parameters: ?start=152 + + +.. rubric:: Bird-like drones +|:linked_paperclips:| `Wüest et al. (2024) `_ + +.. youtube:: Mv3I3Bv8UyQ + :width: 50% + :url_parameters: ?start=116 + + +.. rubric:: Flying Hand: End-Effector-Centric Framework for Versatile Aerial Manipulation Teleoperation and Policy Learning +|:linked_paperclips:| `He et al. (2024) `_ + +.. raw:: html + + + +
+
+ +
+
+```` + +.. rubric:: Swinging up a Custom-Made Furuta Pendulum with NMPC using acados (Slow Motion) +.. youtube:: oJYyD5beMqM + :width: 30% + :aspect: 9:16 diff --git a/docs/requirements.txt b/docs/requirements.txt index 9a0e9524cb..11d6d12d2c 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,8 +1,10 @@ sphinx==5.3 recommonmark==0.7.1 breathe==4.35.0 -sphinx_rtd_theme==2.0.0 -docutils<=0.16 +sphinx_rtd_theme>=2.0.0 +docutils jinja2==3.1.5 sphinx-book-theme==1.1.3 sphinx-markdown-tables==0.0.17 +sphinxcontrib-youtube +sphinxemoji \ No newline at end of file diff --git a/docs/troubleshooting/index.md b/docs/troubleshooting/index.md index 7740aa2d21..a046195074 100644 --- a/docs/troubleshooting/index.md +++ b/docs/troubleshooting/index.md @@ -7,6 +7,10 @@ Next, check the NLP residuals by calling `solver.print_statistics()` in Python a In order to asses the QP solver status, you need to check the corresponding QP solver status definitions. For `HPIPM`, they are given [here](https://github.com/giaf/hpipm/blob/deb7808e49a3cc2b1bdb721cba23f13869c0a35c/include/hpipm_common.h#L57). +### Solver initialization +Use the `set` method of `AcadosOcpSolver` to provide different initializations. +Are all problem functions defined at the initial guess? +`NaN` errors can often be mitigated by solver initializations. ### QP diagnostics - [Python](examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py) From ce6a9fa6ef7d1673c81a6b286e83e8517662d653 Mon Sep 17 00:00:00 2001 From: "LI, Jinjie" <45286479+Li-Jinjie@users.noreply.github.com> Date: Mon, 28 Apr 2025 23:20:26 +0900 Subject: [PATCH 037/164] add our application on tiltable quadrotors to "real_world_examples" (#1510) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This PR provides an example of using NMPC for overactuated tiltable quadrotors. We have some more impressive demos, and we’ll update them after we submit the paper. --- docs/real_world_examples/index.rst | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index ce4249e321..ce0d358aca 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -78,3 +78,11 @@ See blog post `here `_ .. youtube:: oJYyD5beMqM :width: 30% :aspect: 9:16 + + +.. rubric:: Flight Control: Overactuated Tiltable-Quadrotors +|:linked_paperclips:| `Li et al. (2024) `_ + +.. youtube:: 8_pYdeuQnC0 + :width: 50% + :url_parameters: ?start=59 From c22b4a8728006f9af99a2baeb531970fc6d28e04 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 11:38:13 +0200 Subject: [PATCH 038/164] Docs: minor changes and update build requirements (#1509) Co-authored-by: sandmaennchen --- docs/conf.py | 26 +++++++++++++++++++++++++- docs/features/index.md | 8 ++++++-- docs/index.rst | 11 ++++++++--- docs/real_world_examples/index.rst | 6 ++++++ docs/requirements.txt | 9 ++++----- 5 files changed, 49 insertions(+), 11 deletions(-) diff --git a/docs/conf.py b/docs/conf.py index 310c0d9830..5ec81ac428 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -45,6 +45,24 @@ from recommonmark.transform import AutoStructify +import sphinx.ext.autodoc as autodoc + +_original_get_doc = autodoc.Documenter.get_doc + +def patched_get_doc(self, *args, **kwargs): + try: + if self.directive and hasattr(self.directive, "state"): + doc = getattr(self.directive.state, "document", None) + if doc and hasattr(doc, "settings") and not hasattr(doc.settings, "tab_width"): + doc.settings.tab_width = 4 + except Exception: + pass # Silent fail-safe + + return _original_get_doc(self, *args, **kwargs) + +autodoc.Documenter.get_doc = patched_get_doc + + source_suffix = { '.rst': 'restructuredtext', '.txt': 'markdown', @@ -81,7 +99,13 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables', +extensions = [ + 'sphinx.ext.mathjax', + 'breathe', + 'recommonmark', + 'sphinx.ext.autodoc', + 'sphinx.ext.graphviz', + 'sphinx_markdown_tables', 'sphinx.ext.intersphinx', 'sphinxcontrib.youtube', 'sphinxemoji.sphinxemoji', diff --git a/docs/features/index.md b/docs/features/index.md index be480b5226..da2d757ec6 100644 --- a/docs/features/index.md +++ b/docs/features/index.md @@ -5,9 +5,13 @@ If you are new to `acados`, we highly recommend you to start with the `getting_s - [Python](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started) +### Getting started with Model Predictive Control +In particular, the **closed-loop** examples are a great starting point to develop model predictive control (MPC) in a simulation. +Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs and an integrator (`AcadosSimSolver`) is used to simulate the real system. +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started/minimal_example_closed_loop.py) +- [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m) ## Simulation and Sensitivity propagation - - [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py) - [MATLAB/Octave and Simulink](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/minimal_example_sim.m) @@ -23,7 +27,7 @@ If you are new to `acados`, we highly recommend you to start with the `getting_s `acados` supports general nonlinear cost, but can also exploit particular cost structures such as (non)linear least-squares costs and convex-over-nonlinear costs. -- [Python](examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py) ### Soft constraints diff --git a/docs/index.rst b/docs/index.rst index bdc33bbd3f..b06259c5f0 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -1,6 +1,10 @@ acados ====== +.. meta:: + :description: acados is an open-source software package for fast and embedded nonlinear model predictive control (MPC) and moving horizon estimation (MHE). It is written in C and has interfaces to Python, MATLAB, Octave, and Simulink. The software is designed for real-time applications and is used in various fields such as robotics, autonomous vehicles, energy systems, biomechanics, and aerospace. + :keywords: optimal control, nonlinear programming, nonlinear model predictive control, embedded optimization, real-time optimization, moving horizon estimation, open-source software, software library, C library, Python interface, MATLAB interface, Octave interface, Simulink + :google-site-verification: otz_joxGlxsY8wbpkkqOty47dLxqqWPa9JCC5HeyCg8 .. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg :target: https://github.com/acados/acados/actions/workflows/full_build.yml :alt: Github workflow status @@ -77,15 +81,16 @@ The main design paradigms of ``acados`` are: Fields of applications ---------------------- -A non-exhaustive list of projects featuring ``acados`` is available `here `_. -Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. - +``acados`` is used in a wide range of applications. Some examples include: - **Robotics**: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. - **Autonomous Vehicles**: Used in projects like openpilot in driving assistance systems. - **Energy Systems**: Optimization-based control for microgrids and wind turbines. - **Biomechanics**: Optimal control in biomechanics through libraries like bioptim. - **Aerospace**: Applications in trajectory optimization and control for drones and morphing-wing aircraft. +Please also check this non-exhaustive `list of projects `_ featuring ``acados``. +Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. + Documentation page overview --------------------------- diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index ce0d358aca..2b03fa44c5 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -86,3 +86,9 @@ See blog post `here `_ .. youtube:: 8_pYdeuQnC0 :width: 50% :url_parameters: ?start=59 + +.. rubric:: MPC of an Autonomous Warehouse Vehicle with Tricycle Kinematic +|:linked_paperclips:| `Subash et al. (2024) `_ +.. youtube:: NDta6AD5WCA + :width: 50% + :url_parameters: ?start=0 diff --git a/docs/requirements.txt b/docs/requirements.txt index 11d6d12d2c..46c342767b 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,10 +1,9 @@ -sphinx==5.3 +Sphinx==6.0.0 recommonmark==0.7.1 breathe==4.35.0 -sphinx_rtd_theme>=2.0.0 -docutils +docutils==0.19 jinja2==3.1.5 sphinx-book-theme==1.1.3 sphinx-markdown-tables==0.0.17 -sphinxcontrib-youtube -sphinxemoji \ No newline at end of file +sphinxcontrib-youtube==1.3.0 +sphinxemoji==0.3.1 From 78aee36e92bd66f557ceac0af9e6cb9a043f65c1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 12:54:57 +0200 Subject: [PATCH 039/164] Improve Solution Sensitivity related tests and examples (#1513) --- .github/workflows/full_build.yml | 2 + .../solution_sensitivity_example.py | 41 +++-- .../example_solution_sens_closed_loop.py | 8 +- .../forw_vs_adj_param_sens.py | 151 ++++++++---------- .../test_solution_sens_and_exact_hess.py | 21 +-- .../mass_spring_example.c | 6 +- .../mass_spring_nmpc_example.c | 1 + 7 files changed, 120 insertions(+), 110 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index cae5cddaed..8b7175e517 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -209,6 +209,8 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp python initialization_test.py python ocp_example_cost_formulations.py + cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart + python example_solution_sens_closed_loop.py - name: Python Furuta pendulum timeout test diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index b1198a147f..77b1bbd4e6 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -47,12 +47,12 @@ import time -def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_stages: int = 2) -> ca.SX: +def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_steps: int = 2) -> ca.SX: """Define ERK4 integrator for continuous dynamics.""" - dt = h / n_stages + dt = h / n_steps ode = ca.Function("f", [x, u, p], [f_expl]) xnext = x - for _ in range(n_stages): + for _ in range(n_steps): k1 = ode(xnext, u, p) k2 = ode(xnext + dt / 2 * k1, u, p) k3 = ode(xnext + dt / 2 * k2, u, p) @@ -61,6 +61,17 @@ def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct return xnext +def export_discrete_euler_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_steps: int = 2) -> ca.SX: + """Define Euler integrator for continuous dynamics.""" + dt = h / n_steps + ode = ca.Function("f", [x, u, p], [f_expl]) + xnext = x + for _ in range(n_steps): + k1 = ode(xnext, u, p) + xnext = xnext + dt * k1 + + return xnext + def define_param_ssymStruct(n_mass: int, disturbance: bool = True) -> ssymStruct: """Define parameter struct.""" @@ -97,7 +108,7 @@ def find_idx_for_labels(sub_vars: SX, sub_label: str) -> list[int]: return [i for i, label in enumerate(sub_vars.str().strip("[]").split(", ")) if sub_label in label] -def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = False) -> Tuple[AcadosModel, DMStruct]: +def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = False, discrete_dyn_type: str = "RK4") -> Tuple[AcadosModel, DMStruct]: """Export chain mass model for acados.""" x0 = np.array([0, 0, 0]) # fix mass (at wall) @@ -170,7 +181,12 @@ def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = Fa f_expl = vertcat(xvel, u, f) f_impl = xdot - f_expl - f_disc = export_discrete_erk4_integrator_step(f_expl, x, u, p, Ts) + if discrete_dyn_type == "RK4": + f_disc = export_discrete_erk4_integrator_step(f_expl, x, u, p, Ts) + elif discrete_dyn_type == "EULER": + f_disc = export_discrete_euler_integrator_step(f_expl, x, u, p, Ts) + else: + raise ValueError("discrete_dyn_type must be either 'RK4' or 'EULER'") model = AcadosModel() @@ -239,6 +255,7 @@ def export_parametric_ocp( hessian_approx: str = "GAUSS_NEWTON", integrator_type: str = "IRK", nlp_solver_type: str = "SQP", + discrete_dyn_type: str = "RK4", nlp_iter: int = 50, nlp_tol: float = 1e-5, random_scale: dict = {"m": 0.0, "D": 0.0, "L": 0.0, "C": 0.0}, @@ -248,7 +265,7 @@ def export_parametric_ocp( ocp.solver_options.N_horizon = chain_params_["N"] # export model - ocp.model, p = export_chain_mass_model(n_mass=chain_params_["n_mass"], Ts=chain_params_["Ts"], disturbance=True) + ocp.model, p = export_chain_mass_model(n_mass=chain_params_["n_mass"], Ts=chain_params_["Ts"], disturbance=True, discrete_dyn_type=discrete_dyn_type) # parameters np.random.seed(chain_params_["seed"]) @@ -349,11 +366,17 @@ def export_parametric_ocp( return ocp, p -def main_parametric(qp_solver_ric_alg: int = 0, chain_params_: dict = get_chain_params(), generate_code: bool = True) -> None: +def main_parametric(qp_solver_ric_alg: int = 0, + discrete_dyn_type: str = "RK4", + chain_params_: dict = get_chain_params(), + with_more_adjoints = True, + generate_code: bool = True) -> None: + if discrete_dyn_type == "EULER": + print("Warning: OCP solver does not converge with EULER integrator.") ocp, parameter_values = export_parametric_ocp( chain_params_=chain_params_, qp_solver_ric_alg=qp_solver_ric_alg, integrator_type="DISCRETE", + discrete_dyn_type=discrete_dyn_type ) - with_more_adjoints = True ocp_json_file = "acados_ocp_" + ocp.model.name + ".json" # Check if json_file exists @@ -592,4 +615,4 @@ def print_timings(timing_results: dict, metric: str = "median"): if __name__ == "__main__": chain_params = get_chain_params() chain_params["n_mass"] = 3 - main_parametric(qp_solver_ric_alg=0, chain_params_=chain_params, generate_code=True) + main_parametric(qp_solver_ric_alg=0, discrete_dyn_type="RK4", chain_params_=chain_params, generate_code=True, with_more_adjoints=True) diff --git a/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py b/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py index 8b8fa42187..b0cb8bd3b3 100644 --- a/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py +++ b/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py @@ -129,7 +129,11 @@ def main(): acados_ocp_solver = create_ocp_solver() acados_integrator = create_integrator() - print("Please check the documentation fo eval_solution_sensitivities() for the requirements on exact solution sensitivities with acados.") + print("Please check the documentation of eval_solution_sensitivities() for the requirements on exact solution sensitivities with acados.") + # NOTE: the correct computation of solution sensitivities requires an exact Hessian. + # The formulation here uses the Gauss-Newton Hessian approximation, + # which is exact as a linear dynamics and constraints are used + # and the cost function is of linear least squares type. nx = acados_ocp_solver.acados_ocp.dims.nx nu = acados_ocp_solver.acados_ocp.dims.nu @@ -163,7 +167,7 @@ def main(): u_lin = simU[i,:] x_lin = xcurrent - out_dict = acados_ocp_solver.eval_solution_sensitivity(0, with_respect_to="initial_state", return_sens_u=True, return_sens_x=False) + out_dict = acados_ocp_solver.eval_solution_sensitivity(0, with_respect_to="initial_state", return_sens_u=True, return_sens_x=False, sanity_checks=False) sens_u = out_dict['sens_u'] else: diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py index e911a5d84a..af9877d652 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py @@ -34,121 +34,113 @@ TOL = 1e-6 -def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_trajectory=False): +def main(qp_solver_ric_alg: int, generate_solvers=True, plot_trajectory=False): """ Evaluate policy and calculate its gradient for the pendulum on a cart with a parametric model. """ p_nominal = 1.0 x0 = np.array([0.0, np.pi / 2, 0.0, 0.0]) - p_test = p_nominal + 0.2 nx = len(x0) - nu = 1 - N_horizon = 10 - T_horizon = 1.0 - - # TODO: look more into why the test fails with these settings: - # adjoint sensitivities of first parameter dont match with forward sensitivities - # only "small" difference: in 5th digit, might be due to numerical inaccuracies - # N_horizon = 50 - # T_horizon = 2.0 - # p_test = p_nominal + 0.3 + N_horizon = 20 + T_horizon = 2.0 + p_test = p_nominal + 0.3 Fmax = 80.0 - cost_scale_as_param = True # test with 2 parameters + cost_scale_as_param = True with_parametric_constraint = True with_nonlinear_constraint = True ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, qp_solver_ric_alg=1, cost_scale_as_param=cost_scale_as_param, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) - if use_cython: - raise NotImplementedError() - AcadosOcpSolver.generate(ocp, json_file="parameter_augmented_acados_ocp.json") - AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) - ocp_solver = AcadosOcpSolver.create_cython_solver("parameter_augmented_acados_ocp.json") - else: - ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", generate=generate_solvers, build=generate_solvers) + ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", generate=generate_solvers, build=generate_solvers) # create sensitivity solver ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, hessian_approx='EXACT', qp_solver_ric_alg=qp_solver_ric_alg, cost_scale_as_param=cost_scale_as_param, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) ocp.model.name = 'sensitivity_solver' ocp.code_export_directory = f'c_generated_code_{ocp.model.name}' + sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - hp_sens_solver = True - if hp_sens_solver: - original_ocp = ocp_solver.acados_ocp - # undo some settings that are not needed for HP sens solver - ocp.solver_options.globalization_fixed_step_length = 1.0 - ocp.solver_options.nlp_solver_max_iter = original_ocp.solver_options.nlp_solver_max_iter - # to "force" a QP solve - ocp.solver_options.tol = original_ocp.solver_options.tol - ocp.solver_options.qp_tol = original_ocp.solver_options.tol - ocp.solver_options.nlp_solver_max_iter = original_ocp.solver_options.nlp_solver_max_iter - # QP warm start - # ocp.solver_options.qp_solver_warm_start = 3 - # ocp.solver_options.nlp_solver_warm_start_first_qp = True - # ocp.solver_options.nlp_solver_warm_start_first_qp_from_nlp = True - # # HPIPM settings - # ocp.solver_options.qp_solver_iter_max = 0 - # ocp.remove_x0_elimination() - - sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - else: - if use_cython: - AcadosOcpSolver.generate(ocp, json_file=f"{ocp.model.name}.json") - AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) - sensitivity_solver = AcadosOcpSolver.create_cython_solver(f"{ocp.model.name}.json") - else: - ocp.solver_options.nlp_solver_warm_start_first_qp = True - ocp.solver_options.qp_solver_warm_start = 2 - sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - - # set parameter value if cost_scale_as_param: - p_val = np.array([p_test, 1.0]) + p_vals = [ + np.array([p_nominal, 1.0]), + np.array([p_nominal + 0.2, 1.0]), + np.array([p_nominal + 0.3, 1.0]), + np.array([p_nominal + 0.4, 1.0]), + ] else: - p_val = np.array([p_test]) + p_vals = np.array([p_test]) + + for p_val in p_vals: + print(f"Testing with p_val = {p_val}") + + # solve and compare forward and adjoint + solve_and_compare_fwd_and_adj(ocp_solver, sensitivity_solver, x0, p_val, with_parametric_constraint) + + if plot_trajectory: + nx = ocp.dims.nx + nu = ocp.dims.nu + simX = np.zeros((N_horizon+1, nx)) + simU = np.zeros((N_horizon, nu)) + + # get solution + for i in range(N_horizon): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") + + plot_pendulum(ocp.solver_options.shooting_nodes, Fmax, simU, simX, latexify=True, time_label=ocp.model.t_label, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) + - ocp_solver.set_p_global_and_precompute_dependencies(p_val) + +def solve_and_compare_fwd_and_adj(ocp_solver: AcadosOcpSolver, + sensitivity_solver: AcadosOcpSolver, + x0: np.ndarray, + p_val: np.ndarray, + with_parametric_constraint: bool): + + N_horizon = ocp_solver.N + nx = ocp_solver.acados_ocp.dims.nx + nu = ocp_solver.acados_ocp.dims.nu + + # set parameter value + ocp_solver.set_p_global_and_precompute_dependencies(p_val) sensitivity_solver.set_p_global_and_precompute_dependencies(p_val) + # nominal solve u_opt = ocp_solver.solve_for_x0(x0)[0] - tau_iter = ocp_solver.get_stats("qp_tau_iter") print(f"qp tau iter: {tau_iter}\n") - iterate = ocp_solver.store_iterate_to_obj() if with_parametric_constraint: lambdas = np.zeros((N_horizon-1, 2)) for i in range(1, N_horizon): lam_ = ocp_solver.get(i, "lam") lambdas[i-1] = np.array([lam_[1], lam_[3]]) - print(f"lambdas of parametric constraints: {lambdas}\n") + # print(f"lambdas of parametric constraints: {lambdas}\n") max_lam = np.max(np.abs(lambdas)) print(f"max lambda of parametric constraints: {max_lam:.2f}\n") + # transfer iterate to sensitivity solver + iterate = ocp_solver.store_iterate_to_obj() sensitivity_solver.load_iterate_from_obj(iterate) - if hp_sens_solver: - # sensitivity_solver.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) - sensitivity_solver.setup_qp_matrices_and_factorize() - - else: - sensitivity_solver.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) + # setup QP matrices and factorize + sensitivity_solver.setup_qp_matrices_and_factorize() - sensitivity_solver.print_statistics() qp_iter = sensitivity_solver.get_stats("qp_iter") print(f"qp iter: {qp_iter}\n") - for i in range(1, N_horizon-1): - P_mat = sensitivity_solver.get_from_qp_in(i, "P") - K_mat = sensitivity_solver.get_from_qp_in(i, "K") - Lr_mat = sensitivity_solver.get_from_qp_in(i, "Lr") - print(f"stage {i} got factorization") - # print(f"P_mat = {P_mat}") - # print(f"K_mat = {K_mat}") - print(f"Lr_mat = {Lr_mat}") + # check factorization + # for i in range(1, N_horizon-1): + # P_mat = sensitivity_solver.get_from_qp_in(i, "P") + # K_mat = sensitivity_solver.get_from_qp_in(i, "K") + # Lr_mat = sensitivity_solver.get_from_qp_in(i, "Lr") + # print(f"stage {i} got factorization") + # # print(f"P_mat = {P_mat}") + # # print(f"K_mat = {K_mat}") + # print(f"Lr_mat = {Lr_mat}") if sensitivity_solver.get_status() not in [0, 2]: breakpoint() @@ -181,7 +173,6 @@ def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_t print(f"{adj_p=} {adj_p_ref=}") if not np.allclose(adj_p, adj_p_ref, atol=TOL): test_failure_message("adj_p and adj_p_ref should match.") - # print("ERROR: adj_p and adj_p_ref should match.") else: print("Success: adj_p and adj_p_ref match!") @@ -223,24 +214,10 @@ def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_t else: print(f"Success: adj_p_vec and adj_p_mat[{i}, :] match!") - if plot_trajectory: - nx = ocp.dims.nx - nu = ocp.dims.nu - simX = np.zeros((N_horizon+1, nx)) - simU = np.zeros((N_horizon, nu)) - - # get solution - for i in range(N_horizon): - simX[i,:] = ocp_solver.get(i, "x") - simU[i,:] = ocp_solver.get(i, "u") - simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") - - plot_pendulum(ocp.solver_options.shooting_nodes, Fmax, simU, simX, latexify=True, time_label=ocp.model.t_label, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) - def test_failure_message(msg): # print(f"ERROR: {msg}") raise Exception(msg) if __name__ == "__main__": - main(qp_solver_ric_alg=0, use_cython=False, generate_solvers=True, plot_trajectory=False) + main(qp_solver_ric_alg=0, generate_solvers=True, plot_trajectory=False) diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py index a71273749d..41d021061b 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py @@ -114,7 +114,7 @@ def create_ocp_description(hessian_approx, linearized_dynamics=False, discrete=F ocp.solver_options.qp_solver_iter_max = 500 ocp.solver_options.nlp_solver_max_iter = 1000 if hessian_approx == 'EXACT': - ocp.solver_options.nlp_solver_step_length = 0.5 + ocp.solver_options.globalization_fixed_step_length = 0.5 ocp.solver_options.nlp_solver_max_iter = 1 ocp.solver_options.tol = 1e-14 # set prediction horizon @@ -242,14 +242,14 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) for i, p0 in enumerate(p_vals): x0[idxp] = p0 u0 = acados_ocp_solver_gn.solve_for_x0(x0) - u0_values[i] = u0 + u0_values[i] = u0[0] du0_dp_finite_diff = np.gradient(u0_values, p_vals[1]-p_vals[0]) for i, p0 in enumerate(p_vals): x0[idxp] = p0 u0 = acados_ocp_solver_gn.solve_for_x0(x0) - acados_ocp_solver_gn.store_iterate(filename='iterate.json', overwrite=True, verbose=False) - acados_ocp_solver_exact.load_iterate(filename='iterate.json', verbose=False) + iterate = acados_ocp_solver_gn.store_iterate_to_flat_obj() + acados_ocp_solver_exact.load_iterate_from_flat_obj(iterate) acados_ocp_solver_exact.set(0, 'u', u0+1e-7) acados_ocp_solver_exact.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) @@ -260,8 +260,7 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) exact_hessian_status[i] = acados_ocp_solver_exact.get_stats('qp_stat')[-1] residuals = acados_ocp_solver_exact.get_stats("residuals") - print(f"residuals sensitivity_solver {residuals}") - + # print(f"residuals sensitivity_solver {residuals}") # solve with casadi and compare hessians nlp_sol = casadi_solver(p=x0, lbg=lbg, ubg=ubg) @@ -281,9 +280,13 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) if max_hess_error > 1e-4: raise Exception(f"Hessian error {max_hess_error} > 1e-4 when comparing to casadi.") - solution_sens_mean_diff = np.mean(np.abs(du0_dp_values -du0_dp_finite_diff)) + solution_sens_mean_diff = np.mean(np.abs(du0_dp_values - du0_dp_finite_diff)) + solution_sens_median_diff = np.median(np.abs(du0_dp_values - du0_dp_finite_diff)) + print(f"Difference of solution sensitivity difference wrt finite differences: mean: {solution_sens_mean_diff:.2e} median: {solution_sens_median_diff:.2e}") if solution_sens_mean_diff > 1.0: raise Exception(f"Mean of solution sensitivity difference wrt finite differences {solution_sens_mean_diff} > 1.0.") + if solution_sens_median_diff > 0.1: + raise Exception(f"Median of solution sensitivity difference wrt finite differences {solution_sens_median_diff} > 0.1.") # plot_tangents(p_vals, u0_values, du0_dp_values) # Finite difference comparison @@ -392,8 +395,8 @@ def run_hessian_comparison(linearized_dynamics=False, discrete=False): casadi_hess_l = lag_hess_fun(x=nlp_sol['x'], p=x0, lam_f=1.0, lam_g=nlp_sol['lam_g'])['triu_hess_gamma_x_x'] casadi_hess = ca.triu2symm(ca.triu(casadi_hess_l)).full() acados_ocp_solver_gn.solve_for_x0(x0) - acados_ocp_solver_gn.store_iterate(filename='iterate.json', overwrite=True) - acados_ocp_solver_exact.load_iterate(filename='iterate.json') + iterate = acados_ocp_solver_gn.store_iterate_to_flat_obj() + acados_ocp_solver_exact.load_iterate_from_flat_obj(iterate) acados_ocp_solver_exact.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) _ = compare_hessian(casadi_hess, acados_ocp_solver_exact) diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index a4d2a3da70..e14857e37b 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -188,9 +188,9 @@ int main() { // bool ok = false; // if (ok == true) config++; // dummy command to shut up Werror in Release -#ifdef ACADOS_WITH_QPDUNES - int clipping; -#endif +// #ifdef ACADOS_WITH_QPDUNES +// int clipping; +// #endif #ifdef SOFT_CONSTRAINTS double mu0 = 1e2; diff --git a/examples/c/no_interface_examples/mass_spring_nmpc_example.c b/examples/c/no_interface_examples/mass_spring_nmpc_example.c index 0d2985cb30..fff57f5205 100644 --- a/examples/c/no_interface_examples/mass_spring_nmpc_example.c +++ b/examples/c/no_interface_examples/mass_spring_nmpc_example.c @@ -61,6 +61,7 @@ #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" #include "acados/ocp_nlp/ocp_nlp_reg_common.h" #include "acados/ocp_nlp/ocp_nlp_reg_noreg.h" +#include "acados/ocp_nlp/ocp_nlp_globalization_fixed_step.h" #include "acados/ocp_nlp/ocp_nlp_sqp.h" // temp From eeab1a3b78d682afe73e8f9cfd33ae203e8a8453 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 29 Apr 2025 14:12:46 +0200 Subject: [PATCH 040/164] Fix docs (#1514) --- docs/real_world_examples/index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index 2b03fa44c5..493717bc0c 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -89,6 +89,7 @@ See blog post `here `_ .. rubric:: MPC of an Autonomous Warehouse Vehicle with Tricycle Kinematic |:linked_paperclips:| `Subash et al. (2024) `_ + .. youtube:: NDta6AD5WCA :width: 50% :url_parameters: ?start=0 From 49f85de76dad2aa0d71e8378fe3ffcdb630157be Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 29 Apr 2025 15:52:10 +0200 Subject: [PATCH 041/164] Python/MATLAB: Check CasADi expressions of terminal stage (#1506) Check whether CasADi expressions of terminal stage depend on `u` or `z` + minor cleanup in `make_consistent` and code generation --- interfaces/acados_matlab_octave/AcadosOcp.m | 16 ++- .../acados_template/acados_model.py | 16 +-- .../acados_template/acados_ocp.py | 116 +++++++----------- .../casadi_function_generation.py | 63 ++++------ 4 files changed, 80 insertions(+), 131 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index e958c4b740..36affd3fcc 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -859,11 +859,11 @@ function make_consistent(self, is_mocp_phase) end if opts.N_horizon == 0 - cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]; else cost_types_to_check = [strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]; end if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any(cost_types_to_check) error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); @@ -984,6 +984,18 @@ function make_consistent(self, is_mocp_phase) end self.zoro_description.process(); end + + % check terminal stage + fields = {'cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', ... + 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', ... + 'con_h_expr_e', 'con_phi_expr_e', 'con_r_expr_e'}; + for i = 1:length(fields) + field = fields{i}; + val = model.(field); + if ~isempty(val) && (depends_on(val, model.u) || depends_on(val, model.z)) + error([field ' can not depend on u or z.']) + end + end end function [] = detect_cost_and_constraints(self) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 0089c73354..e0a75c3f56 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -850,31 +850,23 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: # nu if is_empty(self.u): - dims.nu = 0 self.u = casadi_symbol('u', 0, 1) - else: - dims.nu = casadi_length(self.u) + dims.nu = casadi_length(self.u) # nz if is_empty(self.z): - dims.nz = 0 self.z = casadi_symbol('z', 0, 1) - else: - dims.nz = casadi_length(self.z) + dims.nz = casadi_length(self.z) # np if is_empty(self.p): - dims.np = 0 self.p = casadi_symbol('p', 0, 1) - else: - dims.np = casadi_length(self.p) + dims.np = casadi_length(self.p) # np_global if is_empty(self.p_global): - dims.np_global = 0 self.p_global = casadi_symbol('p_global', 0, 1) - else: - dims.np_global = casadi_length(self.p_global) + dims.np_global = casadi_length(self.p_global) # sanity checks for symbol, name in [(self.x, 'x'), (self.xdot, 'xdot'), (self.u, 'u'), (self.z, 'z'), (self.p, 'p'), (self.p_global, 'p_global')]: diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 229f0dc1c9..6d732dc845 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -330,26 +330,24 @@ def _make_consistent_cost_terminal(self): if isinstance(cost.W_e, (ca.SX, ca.MX, ca.DM)): raise Exception("W_e should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") - if cost.cost_type_e == 'LINEAR_LS': ny_e = cost.W_e.shape[0] check_if_square(cost.W_e, 'W_e') - if cost.Vx_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ - f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') - if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: - raise ValueError('inconsistent dimension: Vx_e should have nx columns.') - if cost.yref_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension: regarding W_e, yref_e.') dims.ny_e = ny_e - elif cost.cost_type_e == 'NONLINEAR_LS': - ny_e = cost.W_e.shape[0] - check_if_square(cost.W_e, 'W_e') - if (is_empty(model.cost_y_expr_e) and ny_e != 0) or casadi_length(model.cost_y_expr_e) != ny_e or cost.yref_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + - f'\nGot W_e[{cost.W_e.shape}], yref_e[{cost.yref_e.shape}], ', - f'cost_y_expr_e [{casadi_length(model.cost_y_expr_e)}]\n') - dims.ny_e = ny_e + if cost.cost_type_e == 'LINEAR_LS': + if cost.Vx_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') + if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: + raise ValueError('inconsistent dimension: Vx_e should have nx columns.') + if cost.yref_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension: regarding W_e, yref_e.') + + elif cost.cost_type_e == 'NONLINEAR_LS': + if (is_empty(model.cost_y_expr_e) and ny_e != 0) or casadi_length(model.cost_y_expr_e) != ny_e or cost.yref_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + + f'\nGot W_e[{cost.W_e.shape}], yref_e[{cost.yref_e.shape}], ', + f'cost_y_expr_e [{casadi_length(model.cost_y_expr_e)}]\n') elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': if is_empty(model.cost_y_expr_e): @@ -388,9 +386,9 @@ def _make_consistent_constraints_initial(self): return nbx_0 = constraints.idxbx_0.shape[0] + dims.nbx_0 = nbx_0 if constraints.ubx_0.shape[0] != nbx_0 or constraints.lbx_0.shape[0] != nbx_0: raise ValueError('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') - dims.nbx_0 = nbx_0 if any(constraints.idxbx_0 >= dims.nx): raise ValueError(f'idxbx_0 = {constraints.idxbx_0} contains value >= nx = {dims.nx}.') @@ -404,10 +402,7 @@ def _make_consistent_constraints_initial(self): if any(constraints.idxbxe_0 >= dims.nbx_0): raise ValueError(f'idxbxe_0 = {constraints.idxbxe_0} contains value >= nbx_0 = {dims.nbx_0}.') - if not is_empty(model.con_h_expr_0): - nh_0 = casadi_length(model.con_h_expr_0) - else: - nh_0 = 0 + nh_0 = 0 if is_empty(model.con_h_expr_0) else casadi_length(model.con_h_expr_0) if constraints.uh_0.shape[0] != nh_0 or constraints.lh_0.shape[0] != nh_0: raise ValueError('inconsistent dimension nh_0, regarding lh_0, uh_0, con_h_expr_0.') @@ -505,10 +500,7 @@ def _make_consistent_constraints_terminal(self): else: dims.ng_e = ng_e - if not is_empty(model.con_h_expr_e): - nh_e = casadi_length(model.con_h_expr_e) - else: - nh_e = 0 + nh_e = 0 if is_empty(model.con_h_expr_e) else casadi_length(model.con_h_expr_e) if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e: raise ValueError('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') @@ -588,22 +580,10 @@ def _make_consistent_slacks_initial(self): else: raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") - wrong_fields = [] - if cost.Zl_0.shape[0] != ns_0: - wrong_fields += ["Zl_0"] - dim = cost.Zl_0.shape[0] - elif cost.Zu_0.shape[0] != ns_0: - wrong_fields += ["Zu_0"] - dim = cost.Zu_0.shape[0] - elif cost.zl_0.shape[0] != ns_0: - wrong_fields += ["zl_0"] - dim = cost.zl_0.shape[0] - elif cost.zu_0.shape[0] != ns_0: - wrong_fields += ["zu_0"] - dim = cost.zu_0.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + for field in ("Zl_0", "Zu_0", "zl_0", "zu_0"): + dim = getattr(cost, field).shape[0] + if dim != ns_0: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') dims.ns_0 = ns_0 @@ -697,24 +677,12 @@ def _make_consistent_slacks_path(self): dims.nsg = nsg ns = nsbx + nsbu + nsh + nsg + nsphi - wrong_fields = [] - if cost.Zl.shape[0] != ns: - wrong_fields += ["Zl"] - dim = cost.Zl.shape[0] - elif cost.Zu.shape[0] != ns: - wrong_fields += ["Zu"] - dim = cost.Zu.shape[0] - elif cost.zl.shape[0] != ns: - wrong_fields += ["zl"] - dim = cost.zl.shape[0] - elif cost.zu.shape[0] != ns: - wrong_fields += ["zu"] - dim = cost.zu.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' - + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ - + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') + for field in ("Zl", "Zu", "zl", "zu"): + dim = getattr(cost, field).shape[0] + if dim != ns: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns @@ -788,22 +756,10 @@ def _make_consistent_slacks_terminal(self): # terminal ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e - wrong_field = "" - if cost.Zl_e.shape[0] != ns_e: - wrong_field = "Zl_e" - dim = cost.Zl_e.shape[0] - elif cost.Zu_e.shape[0] != ns_e: - wrong_field = "Zu_e" - dim = cost.Zu_e.shape[0] - elif cost.zl_e.shape[0] != ns_e: - wrong_field = "zl_e" - dim = cost.zl_e.shape[0] - elif cost.zu_e.shape[0] != ns_e: - wrong_field = "zu_e" - dim = cost.zu_e.shape[0] - - if wrong_field != "": - raise ValueError(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t' + for field in ("Zl_e", "Zu_e", "zl_e", "zu_e"): + dim = getattr(cost, field).shape[0] + if dim != ns_e: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}.') @@ -1077,6 +1033,7 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None raise ValueError('DDP only supports initial state constraints, got terminal constraints.') ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') + # Set default parameters for globalization if opts.globalization_alpha_min is None: if ddp_with_merit_or_funnel: @@ -1135,6 +1092,15 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None # nlp_solver_warm_start_first_qp_from_nlp if opts.nlp_solver_warm_start_first_qp_from_nlp and (opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon): raise NotImplementedError('nlp_solver_warm_start_first_qp_from_nlp only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.') + + # check terminal stage + for field in ('cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', + 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', + 'con_h_expr_e', 'con_phi_expr_e', 'con_r_expr_e',): + val = getattr(model, field) + if not is_empty(val) and (ca.depends_on(val, model.u) or ca.depends_on(val, model.z)): + raise ValueError(f'{field} can not depend on u or z.') + return diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 4f379376e1..940fd7b169 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -344,7 +344,6 @@ def generate_c_code_implicit_ode(context: GenerateContext, model: AcadosModel, m jac_z = ca.jacobian(f_impl, z) # Set up functions - p = model.p fun_name = model_name + '_impl_dae_fun' context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl], model_dir, 'dyn') @@ -399,7 +398,7 @@ def generate_c_code_gnsf(context: GenerateContext, model: AcadosModel, model_dir x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) z1 = symbol("gnsf_z1", gnsf_nz1, 1) dummy = symbol("gnsf_dummy", 1, 1) - empty_var = symbol("gnsf_empty_var", 0, 0) + empty_var = symbol("gnsf_empty_var", 0, 1) ## generate C code fun_name = model_name + '_gnsf_phi_fun' @@ -448,6 +447,7 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, u = model.u z = model.z p_global = model.p_global + symbol = model.get_casadi_symbol() if stage_type == 'terminal': @@ -458,14 +458,10 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, suffix_name_value_sens = "_cost_ext_cost_e_grad_p" ext_cost = model.cost_expr_ext_cost_e custom_hess = model.cost_expr_ext_cost_custom_hess_e - # Last stage cannot depend on u and z - if any(ca.which_depends(ext_cost, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(ext_cost, model.z)): - raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) elif stage_type == 'path': suffix_name = "_cost_ext_cost_fun" @@ -538,13 +534,10 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage if stage_type == 'terminal': middle_name = '_cost_y_e' y_expr = model.cost_y_expr_e - if any(ca.which_depends(y_expr, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(y_expr, model.z)): - raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) elif stage_type == 'initial': middle_name = '_cost_y_0' @@ -591,13 +584,14 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag opts = context.opts x = model.x + u = model.u z = model.z p = model.p p_global = model.p_global t = model.t + u = model.u symbol = model.get_casadi_symbol() - p_global = symbol('p_global', 0, 0) if stage_type == 'terminal': @@ -605,44 +599,37 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag inner_expr = model.cost_y_expr_e - yref outer_expr = model.cost_psi_expr_e res_expr = model.cost_r_in_psi_expr_e + custom_hess = model.cost_conl_custom_outer_hess_e suffix_name_fun = '_conl_cost_e_fun' suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess_e - if any(ca.which_depends(inner_expr, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(inner_expr, model.z)): - raise ValueError("terminal cost cannot depend on z.") # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) + elif stage_type == 'initial': - u = model.u yref = model.cost_r_in_psi_expr_0 inner_expr = model.cost_y_expr_0 - yref outer_expr = model.cost_psi_expr_0 res_expr = model.cost_r_in_psi_expr_0 + custom_hess = model.cost_conl_custom_outer_hess_0 suffix_name_fun = '_conl_cost_0_fun' suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess_0 - elif stage_type == 'path': - u = model.u yref = model.cost_r_in_psi_expr inner_expr = model.cost_y_expr - yref outer_expr = model.cost_psi_expr res_expr = model.cost_r_in_psi_expr + custom_hess = model.cost_conl_custom_outer_hess suffix_name_fun = '_conl_cost_fun' suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess - # set up function names fun_name_cost_fun = model.name + suffix_name_fun fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess @@ -661,13 +648,13 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag outer_hess_fun = ca.Function('outer_hess', [res_expr, t, p, p_global], [hess]) outer_hess_expr = outer_hess_fun(inner_expr, t, p, p_global) outer_hess_is_diag = outer_hess_expr.sparsity().is_diag() + if casadi_length(res_expr) <= 4: outer_hess_is_diag = 0 Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T Jt_z_expr = ca.jacobian(inner_expr, z).T - # change directory cost_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_cost')) context.add_function_definition( @@ -704,13 +691,11 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con constr_type = constraints.constr_type_e con_h_expr = model.con_h_expr_e con_phi_expr = model.con_phi_expr_e - if any(ca.which_depends(con_h_expr, model.u)) or any(ca.which_depends(con_phi_expr, model.u)): - raise ValueError("terminal constraints cannot depend on u.") - if any(ca.which_depends(con_h_expr, model.z)) or any(ca.which_depends(con_phi_expr, model.z)): - raise ValueError("terminal constraints cannot depend on z.") + # create dummy u, z - u = symbol('u', 0, 0) - z = symbol('z', 0, 0) + u = symbol('u', 0, 1) + z = symbol('z', 0, 1) + elif stage_type == 'initial': constr_type = constraints.constr_type_0 con_h_expr = model.con_h_expr_0 @@ -727,12 +712,6 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con # both empty -> nothing to generate return - if is_empty(p): - p = symbol('p', 0, 0) - - if is_empty(z): - z = symbol('z', 0, 0) - # multipliers for hessian nh = casadi_length(con_h_expr) lam_h = symbol('lam_h', nh, 1) From b4bf5cd640cd8e55863e41e06509435025f9c20b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 15:56:45 +0200 Subject: [PATCH 042/164] Test forward vs. adjoint solution sensitivities on CI with quick settings (#1515) --- .github/workflows/full_build.yml | 3 + .../solution_sensitivity_example.py | 190 +++++++++++------- 2 files changed, 118 insertions(+), 75 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 8b7175e517..99ed1ac620 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -211,6 +211,9 @@ jobs: python ocp_example_cost_formulations.py cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart python example_solution_sens_closed_loop.py + cd ${{runner.workspace}}/acados/examples/acados_python/chain_mass/ + python solution_sensitivity_example.py + - name: Python Furuta pendulum timeout test diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index 77b1bbd4e6..5f68ad44e1 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -42,7 +42,7 @@ import matplotlib.pyplot as plt from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver from utils import get_chain_params -from typing import Tuple +from typing import Tuple, Optional from plot_utils import plot_timings import time @@ -259,6 +259,7 @@ def export_parametric_ocp( nlp_iter: int = 50, nlp_tol: float = 1e-5, random_scale: dict = {"m": 0.0, "D": 0.0, "L": 0.0, "C": 0.0}, + ext_fun_compile_flags: Optional[str] = None, ) -> Tuple[AcadosOcp, DMStruct]: # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -362,6 +363,8 @@ def export_parametric_ocp( ocp.solver_options.tol = nlp_tol ocp.solver_options.tf = ocp.solver_options.N_horizon * chain_params_["Ts"] + if ext_fun_compile_flags is not None: + ocp.solver_options.ext_fun_compile_flags = ext_fun_compile_flags return ocp, p @@ -370,12 +373,17 @@ def main_parametric(qp_solver_ric_alg: int = 0, discrete_dyn_type: str = "RK4", chain_params_: dict = get_chain_params(), with_more_adjoints = True, - generate_code: bool = True) -> None: + generate_code: bool = True, + ext_fun_compile_flags: Optional[str] = None, + np_test: int = 20, + generate_plots: bool = True, + ) -> None: if discrete_dyn_type == "EULER": print("Warning: OCP solver does not converge with EULER integrator.") ocp, parameter_values = export_parametric_ocp( chain_params_=chain_params_, qp_solver_ric_alg=qp_solver_ric_alg, integrator_type="DISCRETE", - discrete_dyn_type=discrete_dyn_type + discrete_dyn_type=discrete_dyn_type, + ext_fun_compile_flags=ext_fun_compile_flags, ) ocp_json_file = "acados_ocp_" + ocp.model.name + ".json" @@ -390,6 +398,8 @@ def main_parametric(qp_solver_ric_alg: int = 0, qp_solver_ric_alg=qp_solver_ric_alg, hessian_approx="EXACT", integrator_type="DISCRETE", + discrete_dyn_type=discrete_dyn_type, + ext_fun_compile_flags=ext_fun_compile_flags, ) sensitivity_ocp.model.name = f"{ocp.model.name}_sensitivity" @@ -410,8 +420,6 @@ def main_parametric(qp_solver_ric_alg: int = 0, nx = ocp.dims.nx nu = ocp.dims.nu - np_test = 20 - # p_label = "L_2_0" # p_label = "D_2_0" p_label = f"C_{M}_0" @@ -505,9 +513,15 @@ def main_parametric(qp_solver_ric_alg: int = 0, timings_solve_params_adj[i] = sensitivity_solver.get_stats("time_solution_sens_solve") sens_adj_ref = seed_u.T @ sens_u_ + seed_x.T @ sens_x_ + # print(f"{sens_adj_ref=}") + # print(f"{sens_adj=}") + # print(f"{sens_u_=}") + # print(f"{sens_x_=}") - assert np.allclose(sens_adj_ref.ravel(), sens_adj) - # print(np.abs(sens_adj_ref.ravel() - sens_adj)) + test_tol = 1e-5 + diff_sens_adj_vs_ref = np.max(np.abs(sens_adj_ref.ravel() - sens_adj)) + if diff_sens_adj_vs_ref > test_tol: + raise_test_failure_message(f"diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} should be < {test_tol}") sens_u.append(sens_u_[:, p_idx]) @@ -523,79 +537,80 @@ def main_parametric(qp_solver_ric_alg: int = 0, print(f"i {i} {timings_solve_params_adj[i]*1e3:.5f} \t {timings_solve_params[i]*1e3:.5f} \t {timings_solve_params_adj_uforw[i]*1e3:.5f} \t {timings_solve_params_adj_all_primals[i]*1e3:.5f}") # check wrt forward - assert np.allclose(sens_adj, out_dict['sens_u']) - - timings_common = { - "NLP solve (S1)": timings_solve_ocp_solver * 1e3, - "store \& load iterates": timings_store_load * 1e3, - "parameter update": timings_parameter_update * 1e3, - "setup exact Lagrange Hessian (S2)": timings_lin_exact_hessian_qp * 1e3, - "factorize exact Lagrange Hessian (S3)": timings_lin_and_factorize * 1e3, - r"evaluate $J_\star$ (S4)": timings_lin_params * 1e3, - } - timing_results_forward = timings_common.copy() - timing_results_adjoint = timings_common.copy() - timing_results_adj_uforw = timings_common.copy() - timing_results_adj_all_primals = timings_common.copy() - - backsolve_label = "sensitivity solve given factorization (S5)" - timing_results_forward[backsolve_label] = timings_solve_params * 1e3 - timing_results_adjoint[backsolve_label] = timings_solve_params_adj * 1e3 - - timings_list = [timing_results_forward, timing_results_adjoint] - labels = [r'$\frac{\partial w^\star}{\partial \theta}$ via forward', r'$\nu^\top \frac{\partial w^\star}{\partial \theta}$ via adjoint'] + print(np.abs(sens_adj- out_dict['sens_u'])) + # assert np.allclose(sens_adj, out_dict['sens_u']) + + if generate_plots: + timings_common = { + "NLP solve (S1)": timings_solve_ocp_solver * 1e3, + "store \& load iterates": timings_store_load * 1e3, + "parameter update": timings_parameter_update * 1e3, + "setup exact Lagrange Hessian (S2)": timings_lin_exact_hessian_qp * 1e3, + "factorize exact Lagrange Hessian (S3)": timings_lin_and_factorize * 1e3, + r"evaluate $J_\star$ (S4)": timings_lin_params * 1e3, + } + timing_results_forward = timings_common.copy() + timing_results_adjoint = timings_common.copy() + timing_results_adj_uforw = timings_common.copy() + timing_results_adj_all_primals = timings_common.copy() + + backsolve_label = "sensitivity solve given factorization (S5)" + timing_results_forward[backsolve_label] = timings_solve_params * 1e3 + timing_results_adjoint[backsolve_label] = timings_solve_params_adj * 1e3 + + timings_list = [timing_results_forward, timing_results_adjoint] + labels = [r'$\frac{\partial w^\star}{\partial \theta}$ via forward', r'$\nu^\top \frac{\partial w^\star}{\partial \theta}$ via adjoint'] - if with_more_adjoints: - timing_results_adj_uforw[backsolve_label] = timings_solve_params_adj_uforw * 1e3 - timing_results_adj_all_primals[backsolve_label] = timings_solve_params_adj_all_primals * 1e3 - timings_list += [timing_results_adj_uforw, timing_results_adj_all_primals] - labels += [r'$\frac{\partial u_0^\star}{\partial \theta}$ via adjoints', r'$\frac{\partial z^\star}{\partial \theta} $ via adjoints'] - - - print_timings(timing_results_forward, metric="median") - print_timings(timing_results_forward, metric="min") - - u_opt = np.vstack(u_opt) - sens_u = np.vstack(sens_u) - - # Compare to numerical gradients - sens_u_fd = np.gradient(u_opt, p_var, axis=0) - u_opt_reconstructed_fd = np.cumsum(sens_u_fd, axis=0) * delta_p + u_opt[0, :] - u_opt_reconstructed_fd += u_opt[0, :] - u_opt_reconstructed_fd[0, :] - - u_opt_reconstructed_acados = np.cumsum(sens_u, axis=0) * delta_p + u_opt[0, :] - u_opt_reconstructed_acados += u_opt[0, :] - u_opt_reconstructed_acados[0, :] - - # TODO move to plot utils - plt.figure(figsize=(7, 7)) - for col in range(3): - plt.subplot(4, 1, col + 1) - plt.plot(p_var, u_opt[:, col], label=f"$u^*_{col}$") - plt.plot(p_var, u_opt_reconstructed_fd[:, col], label=f"$u^*_{col}$, reconstructed with fd gradients", linestyle="--") - plt.plot( - p_var, u_opt_reconstructed_acados[:, col], label=f"$u^*_{col}$, reconstructed with acados gradients", linestyle=":" - ) - plt.ylabel(f"$u^*_{col}$") + if with_more_adjoints: + timing_results_adj_uforw[backsolve_label] = timings_solve_params_adj_uforw * 1e3 + timing_results_adj_all_primals[backsolve_label] = timings_solve_params_adj_all_primals * 1e3 + timings_list += [timing_results_adj_uforw, timing_results_adj_all_primals] + labels += [r'$\frac{\partial u_0^\star}{\partial \theta}$ via adjoints', r'$\frac{\partial z^\star}{\partial \theta} $ via adjoints'] + + + print_timings(timing_results_forward, metric="median") + print_timings(timing_results_forward, metric="min") + + u_opt = np.vstack(u_opt) + sens_u = np.vstack(sens_u) + + # Compare to numerical gradients + sens_u_fd = np.gradient(u_opt, p_var, axis=0) + u_opt_reconstructed_fd = np.cumsum(sens_u_fd, axis=0) * delta_p + u_opt[0, :] + u_opt_reconstructed_fd += u_opt[0, :] - u_opt_reconstructed_fd[0, :] + + u_opt_reconstructed_acados = np.cumsum(sens_u, axis=0) * delta_p + u_opt[0, :] + u_opt_reconstructed_acados += u_opt[0, :] - u_opt_reconstructed_acados[0, :] + + plt.figure(figsize=(7, 7)) + for col in range(3): + plt.subplot(4, 1, col + 1) + plt.plot(p_var, u_opt[:, col], label=f"$u^*_{col}$") + plt.plot(p_var, u_opt_reconstructed_fd[:, col], label=f"$u^*_{col}$, reconstructed with fd gradients", linestyle="--") + plt.plot( + p_var, u_opt_reconstructed_acados[:, col], label=f"$u^*_{col}$, reconstructed with acados gradients", linestyle=":" + ) + plt.ylabel(f"$u^*_{col}$") + plt.grid(True) + plt.legend() + plt.xlim(p_var[0], p_var[-1]) + + for col in range(3): + plt.subplot(4, 1, 4) + plt.plot(p_var, np.abs(sens_u[:, col] - sens_u_fd[:, col]), label=f"$u^*_{col}$", linestyle="--") + + plt.ylabel("abs difference") plt.grid(True) plt.legend() + plt.yscale("log") + plt.xlabel(p_label) plt.xlim(p_var[0], p_var[-1]) + plt.tight_layout() + plt.savefig("chain_adj_fwd_sens.pdf") - for col in range(3): - plt.subplot(4, 1, 4) - plt.plot(p_var, np.abs(sens_u[:, col] - sens_u_fd[:, col]), label=f"$u^*_{col}$", linestyle="--") + plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.png", t_max=10, horizontal=True, figsize=(12, 3), with_patterns=True) - plt.ylabel("abs difference") - plt.grid(True) - plt.legend() - plt.yscale("log") - plt.xlabel(p_label) - plt.xlim(p_var[0], p_var[-1]) - plt.tight_layout() - plt.savefig("chain_adj_fwd_sens.pdf") - - plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.png", t_max=10, horizontal=True, figsize=(12, 3), with_patterns=True) - - plt.show() + plt.show() def print_timings(timing_results: dict, metric: str = "median"): @@ -612,7 +627,32 @@ def print_timings(timing_results: dict, metric: str = "median"): for key, value in timing_results.items(): print(f"{key}: {timing_func(value):.3f} ms") -if __name__ == "__main__": +def raise_test_failure_message(msg: str): + # print(f"ERROR: {msg}") + raise Exception(msg) + +def main_test(): + chain_params = get_chain_params() + chain_params['N'] = 4 + chain_params["n_mass"] = 3 + chain_params["Ts"] = 0.01 + main_parametric(qp_solver_ric_alg=0, + discrete_dyn_type="EULER", + chain_params_=chain_params, + generate_code=True, + with_more_adjoints=False, + ext_fun_compile_flags="", + np_test=2, + generate_plots=False, + ) + +def main_experiment(): chain_params = get_chain_params() chain_params["n_mass"] = 3 main_parametric(qp_solver_ric_alg=0, discrete_dyn_type="RK4", chain_params_=chain_params, generate_code=True, with_more_adjoints=True) + +if __name__ == "__main__": + # use settings for fast testing -> test with this on CI + main_test() + # use settings for full experiment + # main_experiment() From 1bbdd66df2e93eca7b8012d18dfdf6a07852486d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 2 May 2025 10:47:06 +0200 Subject: [PATCH 043/164] Ignore sim stuff in build templates if N==0 (#1516) --- .../c_templates_tera/CMakeLists.in.txt | 22 +++++++++------ .../c_templates_tera/Makefile.in | 27 +++++++++++++------ 2 files changed, 33 insertions(+), 16 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index d9c4d56609..7617c5350e 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -34,6 +34,12 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} +{%- if solver_options.N_horizon %} + {%- set N_horizon = solver_options.N_horizon %} +{%- else %} + {%- set N_horizon = 1 %} +{%- endif %} + {%- if solver_options.hessian_approx %} {%- set hessian_approx = solver_options.hessian_approx %} {%- elif solver_options.sens_hess %} @@ -83,7 +89,7 @@ option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }} be build?" OFF) option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF) {%- endif %} @@ -112,7 +118,7 @@ endif() set(MODEL_OBJ model_{{ model.name }}) {%- endif %} set(OCP_OBJ ocp_{{ model.name }}) -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} set(SIM_OBJ sim_{{ model.name }}) {%- endif %} @@ -144,10 +150,10 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP endif() {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} # for sim solver if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_EXAMPLE} - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} OR ${BUILD_SIM_EXAMPLE} OR ${BUILD_ACADOS_SIM_SOLVER_LIB} {%- endif -%} ) @@ -231,7 +237,7 @@ if(${BUILD_ACADOS_SOLVER_LIB}) $ {%- endif %} $ - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $ {%- endif -%} ) @@ -259,7 +265,7 @@ if(${BUILD_EXAMPLE}) $ {%- endif %} $ - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $ {%- endif -%} ) @@ -267,7 +273,7 @@ if(${BUILD_EXAMPLE}) endif(${BUILD_EXAMPLE}) {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} # example_sim if(${BUILD_SIM_EXAMPLE}) set(EX_SIM_SRC main_sim_{{ model.name }}.c) @@ -290,7 +296,7 @@ unset(BUILD_ACADOS_SOLVER_LIB CACHE) unset(BUILD_ACADOS_OCP_SOLVER_LIB CACHE) unset(BUILD_EXAMPLE CACHE) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} unset(BUILD_SIM_EXAMPLE CACHE) unset(BUILD_ACADOS_SIM_SOLVER_LIB CACHE) {%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index fbfda16be8..7f074d2010 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -35,6 +35,12 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} +{%- if solver_options.N_horizon %} + {%- set N_horizon = solver_options.N_horizon %} +{%- else %} + {%- set N_horizon = 1 %} +{%- endif %} + {%- if solver_options.hessian_approx %} {%- set hessian_approx = solver_options.hessian_approx %} {%- elif solver_options.sens_hess %} @@ -104,7 +110,7 @@ OCP_OBJ := $(OCP_SRC:.c=.o) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} # for sim solver SIM_SRC= acados_sim_solver_{{ model.name }}.c SIM_OBJ := $(SIM_SRC:.c=.o) @@ -126,7 +132,7 @@ EX_EXE := $(EX_SRC:.c=) # combine model, (potentially) sim and ocp object files OBJ= OBJ+= $(MODEL_OBJ) -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} OBJ+= $(SIM_OBJ) {%- endif %} @@ -194,7 +200,7 @@ LDLIBS+= {{ link_libs }} # libraries LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }} LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }}) {%- endif %} @@ -206,8 +212,13 @@ LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }}) all: clean example shared_lib: ocp_shared_lib {%- else %} + {% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} all: clean example_sim example shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib + {%-else %} +all: clean example +shared_lib: bundled_shared_lib ocp_shared_lib + {%- endif %} {%- endif %} {%-else %} all: clean example_sim @@ -221,7 +232,7 @@ example: $(EX_OBJ) $(OBJ) $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) @@ -268,7 +279,7 @@ ocp_cython: ocp_cython_o {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} # Sim Cython targets sim_cython_c: sim_shared_lib cython \ @@ -318,7 +329,7 @@ clean_ocp_cython: del \Q acados_ocp_solver_pyx.o 2>nul {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} clean_sim_cython: del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul del \Q acados_sim_solver_{{ model.name }}.o 2>nul @@ -329,7 +340,7 @@ clean_sim_cython: {%- else %} clean: -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) $(RM) $(EX_EXE) $(EX_SIM_EXE) @@ -351,7 +362,7 @@ clean_ocp_cython: $(RM) acados_ocp_solver_pyx.o {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} clean_sim_cython: $(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} $(RM) acados_sim_solver_{{ model.name }}.o From fd231104913cc9c305e0dff736281b91dc1dbc5b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 2 May 2025 12:00:03 +0200 Subject: [PATCH 044/164] Updates with respect to new HPIPM sensitivity API (#1512) --- .github/workflows/full_build.yml | 1 + acados/dense_qp/dense_qp_common.c | 30 +++++++ acados/dense_qp/dense_qp_common.h | 11 ++- acados/dense_qp/dense_qp_daqp.c | 8 +- acados/dense_qp/dense_qp_daqp.h | 2 - acados/dense_qp/dense_qp_hpipm.c | 10 +-- acados/dense_qp/dense_qp_hpipm.h | 2 - acados/dense_qp/dense_qp_ooqp.c | 7 +- acados/dense_qp/dense_qp_ooqp.h | 2 - acados/dense_qp/dense_qp_qore.c | 6 +- acados/dense_qp/dense_qp_qore.h | 2 +- acados/dense_qp/dense_qp_qpoases.c | 8 +- acados/dense_qp/dense_qp_qpoases.h | 2 - acados/ocp_nlp/ocp_nlp_common.c | 86 ++++++++++--------- acados/ocp_nlp/ocp_nlp_common.h | 3 + acados/ocp_qp/ocp_qp_common.c | 27 ++++++ acados/ocp_qp/ocp_qp_common.h | 13 ++- acados/ocp_qp/ocp_qp_full_condensing.c | 72 ++++++++++++++-- acados/ocp_qp/ocp_qp_full_condensing.h | 3 + acados/ocp_qp/ocp_qp_hpipm.c | 10 +-- acados/ocp_qp/ocp_qp_hpipm.h | 2 - acados/ocp_qp/ocp_qp_hpmpc.c | 6 +- acados/ocp_qp/ocp_qp_hpmpc.h | 2 +- acados/ocp_qp/ocp_qp_ooqp.c | 6 +- acados/ocp_qp/ocp_qp_ooqp.h | 2 - acados/ocp_qp/ocp_qp_osqp.c | 8 +- acados/ocp_qp/ocp_qp_osqp.h | 2 - acados/ocp_qp/ocp_qp_partial_condensing.c | 75 +++++++++++++++- acados/ocp_qp/ocp_qp_partial_condensing.h | 3 + acados/ocp_qp/ocp_qp_qpdunes.c | 8 +- acados/ocp_qp/ocp_qp_qpdunes.h | 2 - acados/ocp_qp/ocp_qp_xcond_solver.c | 19 ++-- acados/ocp_qp/ocp_qp_xcond_solver.h | 5 +- .../solution_sensitivity_example.py | 14 ++- external/hpipm | 2 +- 35 files changed, 337 insertions(+), 124 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 99ed1ac620..5962663278 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -202,6 +202,7 @@ jobs: python policy_gradient_example.py python test_solution_sens_and_exact_hess.py python forw_vs_adj_param_sens.py + python smooth_policy_gradients.py cd ${{runner.workspace}}/acados/examples/acados_python/solution_sensitivities_convex_example python value_gradient_example_linear.py python batch_adjoint_solution_sensitivity_example.py diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 8ee3067889..705f621e89 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -41,6 +41,7 @@ #include "hpipm/include/hpipm_d_dense_qp_ipm.h" #include "hpipm/include/hpipm_d_dense_qp_kkt.h" #include "hpipm/include/hpipm_d_dense_qp_res.h" +#include "hpipm/include/hpipm_d_dense_qp_seed.h" #include "hpipm/include/hpipm_d_dense_qp_sol.h" #include "hpipm/include/hpipm_d_dense_qp_dim.h" @@ -228,6 +229,35 @@ void dense_qp_out_get(dense_qp_out *out, const char *field, void *value) return; } +/************************************************ + * seed + ************************************************/ + +acados_size_t dense_qp_seed_calculate_size(dense_qp_dims *dims) +{ + acados_size_t size = sizeof(dense_qp_seed); + size += d_dense_qp_seed_memsize(dims); + + make_int_multiple_of(8, &size); + return size; +} + + + +dense_qp_seed *dense_qp_seed_assign(dense_qp_dims *dims, void *raw_memory) +{ + char *c_ptr = (char *) raw_memory; + + dense_qp_seed *qp_seed = (dense_qp_seed *) c_ptr; + c_ptr += sizeof(dense_qp_seed); + + align_char_to(8, &c_ptr); + d_dense_qp_seed_create(dims, qp_seed, c_ptr); + c_ptr += d_dense_qp_seed_memsize(dims); + assert((char *) raw_memory + dense_qp_seed_calculate_size(dims) >= c_ptr); + + return qp_seed; +} /************************************************ diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index 54c742a71b..42ea02d69f 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -48,6 +48,7 @@ typedef struct d_dense_qp dense_qp_in; typedef struct d_dense_qp_sol dense_qp_out; typedef struct d_dense_qp_res dense_qp_res; typedef struct d_dense_qp_res_ws dense_qp_res_ws; +typedef struct d_dense_qp_seed dense_qp_seed; @@ -69,8 +70,8 @@ typedef struct int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); void (*solver_get)(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); } qp_solver_config; #endif @@ -137,6 +138,12 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res // void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]); +/* seed */ +// +acados_size_t dense_qp_seed_calculate_size(dense_qp_dims *dims); +// +dense_qp_seed *dense_qp_seed_assign(dense_qp_dims *dims, void *raw_memory); + /* misc */ // void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c index ed22c6f7f2..22f8adee29 100644 --- a/acados/dense_qp/dense_qp_daqp.c +++ b/acados/dense_qp/dense_qp_daqp.c @@ -781,14 +781,14 @@ int dense_qp_daqp(void* config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void } -void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_daqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_daqp_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_daqp_eval_forw_sens: not implemented yet\n"); exit(1); } -void dense_qp_daqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_daqp_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: dense_qp_daqp_eval_adj_sens: not implemented yet\n"); exit(1); @@ -834,7 +834,7 @@ void dense_qp_daqp_config_initialize_default(void *config_) config->memory_get = &dense_qp_daqp_memory_get; config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_workspace_calculate_size; - config->eval_sens = &dense_qp_daqp_eval_sens; + config->eval_forw_sens = &dense_qp_daqp_eval_forw_sens; config->eval_adj_sens = &dense_qp_daqp_eval_adj_sens; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_daqp; config->memory_reset = &dense_qp_daqp_memory_reset; diff --git a/acados/dense_qp/dense_qp_daqp.h b/acados/dense_qp/dense_qp_daqp.h index 4b0503b77f..425c734a4a 100644 --- a/acados/dense_qp/dense_qp_daqp.h +++ b/acados/dense_qp/dense_qp_daqp.h @@ -97,8 +97,6 @@ void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_ // functions int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_daqp_config_initialize_default(void *config_); diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index 97171a296d..2e5b7d1027 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -320,7 +320,7 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void -void dense_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void dense_qp_hpipm_eval_forw_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { dense_qp_in *param_qp_in = param_qp_in_; dense_qp_out *sens_qp_out = sens_qp_out_; @@ -328,13 +328,13 @@ void dense_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_o dense_qp_hpipm_opts *opts = opts_; dense_qp_hpipm_memory *memory = mem_; - d_dense_qp_ipm_sens(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); + d_dense_qp_ipm_sens_frw(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); return; } -void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { dense_qp_in *param_qp_in = param_qp_in_; dense_qp_out *sens_qp_out = sens_qp_out_; @@ -342,7 +342,7 @@ void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_ dense_qp_hpipm_opts *opts = opts_; dense_qp_hpipm_memory *memory = mem_; - d_dense_qp_ipm_sens_adj(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); + d_dense_qp_ipm_sens_adj(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); return; } @@ -384,7 +384,7 @@ void dense_qp_hpipm_config_initialize_default(void *config_) config->memory_get = &dense_qp_hpipm_memory_get; config->workspace_calculate_size = &dense_qp_hpipm_workspace_calculate_size; config->evaluate = &dense_qp_hpipm; - config->eval_sens = &dense_qp_hpipm_eval_sens; + config->eval_forw_sens = &dense_qp_hpipm_eval_forw_sens; config->eval_adj_sens = &dense_qp_hpipm_eval_adj_sens; config->memory_reset = &dense_qp_hpipm_memory_reset; config->solver_get = &dense_qp_hpipm_solver_get; diff --git a/acados/dense_qp/dense_qp_hpipm.h b/acados/dense_qp/dense_qp_hpipm.h index 7d05fe5138..7d94d7f2e0 100644 --- a/acados/dense_qp/dense_qp_hpipm.h +++ b/acados/dense_qp/dense_qp_hpipm.h @@ -81,8 +81,6 @@ acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_); // int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // -void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_hpipm_config_initialize_default(void *config_); // void dense_qp_hpipm_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); diff --git a/acados/dense_qp/dense_qp_ooqp.c b/acados/dense_qp/dense_qp_ooqp.c index b25497a946..42e0c8652f 100644 --- a/acados/dense_qp/dense_qp_ooqp.c +++ b/acados/dense_qp/dense_qp_ooqp.c @@ -626,10 +626,9 @@ void dense_qp_ooqp_destroy(void *mem_, void *work) } - -void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_ooqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_ooqp_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_ooqp_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -678,7 +677,7 @@ void dense_qp_ooqp_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_ooqp_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_ooqp; - config->eval_sens = &dense_qp_ooqp_eval_sens; + config->eval_forw_sens = &dense_qp_ooqp_eval_forw_sens; config->eval_adj_sens = &dense_qp_ooqp_eval_adj_sens; config->memory_reset = &dense_qp_ooqp_memory_reset; config->solver_get = &dense_qp_ooqp_solver_get; diff --git a/acados/dense_qp/dense_qp_ooqp.h b/acados/dense_qp/dense_qp_ooqp.h index dff92f3cf5..37a8628658 100644 --- a/acados/dense_qp/dense_qp_ooqp.h +++ b/acados/dense_qp/dense_qp_ooqp.h @@ -114,8 +114,6 @@ int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void // void dense_qp_ooqp_destroy(void *mem_, void *work); // -void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_ooqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); // void dense_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 3da2c73909..f4eca21c0d 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -563,9 +563,9 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void -void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qore_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_qore_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_qore_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -612,7 +612,7 @@ void dense_qp_qore_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_qore_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qore; - config->eval_sens = &dense_qp_qore_eval_sens; + config->eval_forw_sens = &dense_qp_qore_eval_forw_sens; config->eval_adj_sens = &dense_qp_qore_eval_adj_sens; config->memory_reset = &dense_qp_qore_memory_reset; config->solver_get = &dense_qp_qore_solver_get; diff --git a/acados/dense_qp/dense_qp_qore.h b/acados/dense_qp/dense_qp_qore.h index cc19476509..5569aa0a0e 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -113,7 +113,7 @@ acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims // int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +void dense_qp_qore_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_qore_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); // diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index f7ac8355c4..0a15bbe601 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -768,14 +768,14 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo -void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qpoases_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_qpoases_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_qpoases_eval_forw_sens: not implemented yet\n"); exit(1); } -void dense_qp_qpoases_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qpoases_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: dense_qp_qpoases_eval_adj_sens: not implemented yet\n"); exit(1); @@ -819,7 +819,7 @@ void dense_qp_qpoases_config_initialize_default(void *config_) config->memory_get = &dense_qp_qpoases_memory_get; config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_qpoases_workspace_calculate_size; - config->eval_sens = &dense_qp_qpoases_eval_sens; + config->eval_forw_sens = &dense_qp_qpoases_eval_forw_sens; config->eval_adj_sens = &dense_qp_qpoases_eval_adj_sens; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qpoases; config->memory_reset = &dense_qp_qpoases_memory_reset; diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 1758a4ec27..2c17540f2e 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -113,8 +113,6 @@ acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_d // int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_qpoases_config_initialize_default(void *config_); diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 893a19a3bc..e878149025 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -41,6 +41,8 @@ #include "blasfeo_d_blas.h" // hpipm #include "hpipm/include/hpipm_d_ocp_qp_dim.h" +#include "hpipm/include/hpipm_d_ocp_qp_res.h" +#include "hpipm/include/hpipm_d_ocp_qp_seed.h" // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" @@ -1962,6 +1964,17 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d // tmp_qp_out size += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); + // qp_seed + size += ocp_qp_seed_calculate_size(dims->qp_solver->orig_dims); + + if (opts->ext_qp_res) + { + // qp_res + size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + // qp_res_ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + // blasfeo_dvec int nv_max = 0; int nx_max = 0; @@ -2136,15 +2149,6 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d } } - if (opts->ext_qp_res) - { - // qp_res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp_res_ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - size += 64; // ext_fun_workspace_size align size += ext_fun_workspace_size; @@ -2220,6 +2224,20 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims work->weight_merit_fun = ocp_nlp_out_assign(config, dims, c_ptr); c_ptr += ocp_nlp_out_calculate_size(config, dims); + // qp seed + work->qp_seed = ocp_qp_seed_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->qp_solver->orig_dims); + + if (opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + assign_and_advance_double(nv_max, &work->tmp_nv_double, &c_ptr); assign_and_advance_int(ni_max+ns_max, &work->tmp_nins, &c_ptr); @@ -2389,17 +2407,6 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } } - if (opts->ext_qp_res) - { - // qp res - work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + mem->workspace_size >= c_ptr); return work; @@ -3657,31 +3664,31 @@ void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims, struct blasfeo_dmat *jac_ineq_p_global = mem->jac_ineq_p_global; struct blasfeo_dmat *jac_dyn_p_global = mem->jac_dyn_p_global; - ocp_qp_in *tmp_qp_in = work->tmp_qp_in; ocp_qp_out *tmp_qp_out = work->tmp_qp_out; - d_ocp_qp_copy_all(mem->qp_in, tmp_qp_in); - d_ocp_qp_set_rhs_zero(tmp_qp_in); + ocp_qp_seed *qp_seed = work->qp_seed; + d_ocp_qp_seed_set_zero(qp_seed); if ((!strcmp("ex", field)) && (stage==0)) { - double one = 1.0; - d_ocp_qp_set_el("lbx", stage, index, &one, tmp_qp_in); - d_ocp_qp_set_el("ubx", stage, index, &one, tmp_qp_in); + int tmp_nbu; + config->constraints[0]->dims_get(config->constraints[0], dims->constraints[0], "nbu", &tmp_nbu); + BLASFEO_DVECEL(qp_seed->seed_d+0, tmp_nbu+index) = 1.0; + BLASFEO_DVECEL(qp_seed->seed_d+0, tmp_nbu+index+nb[0]+ng[0]+ni_nl[0]) = 1.0; } else if (!strcmp("p_global", field)) { for (i = 0; i <= N; i++) { // stationarity - blasfeo_dcolex(nv[i], &jac_lag_stat_p_global[i], 0, index, &tmp_qp_in->rqz[i], 0); + blasfeo_dcolex(nv[i], &jac_lag_stat_p_global[i], 0, index, &qp_seed->seed_g[i], 0); // dynamics if (i < N) - blasfeo_dcolex(nx[i+1], &jac_dyn_p_global[i], 0, index, &tmp_qp_in->b[i], 0); + blasfeo_dcolex(nx[i+1], &jac_dyn_p_global[i], 0, index, &qp_seed->seed_b[i], 0); // inequalities - blasfeo_dcolex(ni_nl[i], &jac_ineq_p_global[i], 0, index, &tmp_qp_in->d[i], nb[i]+ng[i]); - blasfeo_dvecsc(ni_nl[i], -1.0, &tmp_qp_in->d[i], nb[i]+ng[i]); - blasfeo_daxpy(ni_nl[i], -1.0, &tmp_qp_in->d[i], nb[i]+ng[i], &tmp_qp_in->d[i], 2*(nb[i]+ng[i])+ni_nl[i], - &tmp_qp_in->d[i], 2*(nb[i]+ng[i])+ni_nl[i]); + blasfeo_dcolex(ni_nl[i], &jac_ineq_p_global[i], 0, index, &qp_seed->seed_d[i], nb[i]+ng[i]); + blasfeo_dvecsc(ni_nl[i], -1.0, &qp_seed->seed_d[i], nb[i]+ng[i]); + blasfeo_daxpy(ni_nl[i], -1.0, &qp_seed->seed_d[i], nb[i]+ng[i], &qp_seed->seed_d[i], 2*(nb[i]+ng[i])+ni_nl[i], + &qp_seed->seed_d[i], 2*(nb[i]+ng[i])+ni_nl[i]); } } else @@ -3691,7 +3698,8 @@ void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims, } // d_ocp_qp_print(tmp_qp_in->dim, tmp_qp_in); - config->qp_solver->eval_sens(config->qp_solver, dims->qp_solver, tmp_qp_in, tmp_qp_out, + // d_ocp_qp_seed_print(qp_seed->dim, qp_seed); + config->qp_solver->eval_forw_sens(config->qp_solver, dims->qp_solver, mem->qp_in, qp_seed, tmp_qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); // d_ocp_qp_sol_print(tmp_qp_out->dim, tmp_qp_out); @@ -3734,16 +3742,14 @@ void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dim struct blasfeo_dmat *jac_ineq_p_global = mem->jac_ineq_p_global; struct blasfeo_dmat *jac_dyn_p_global = mem->jac_dyn_p_global; - ocp_qp_in *tmp_qp_in = work->tmp_qp_in; + ocp_qp_seed *qp_seed = work->qp_seed; ocp_qp_out *tmp_qp_out = work->tmp_qp_out; + d_ocp_qp_seed_set_zero(qp_seed); - d_ocp_qp_copy_all(mem->qp_in, tmp_qp_in); - d_ocp_qp_set_rhs_zero(tmp_qp_in); - - /* copy sens_nlp_out to tmp_qp_in */ + /* copy sens_nlp_out to qp_seed */ for (i = 0; i <= N; i++) { - blasfeo_dveccp(nv[i], sens_nlp_out->ux + i, 0, tmp_qp_in->rqz + i, 0); + blasfeo_dveccp(nv[i], sens_nlp_out->ux + i, 0, qp_seed->seed_g + i, 0); // NOTE: noone needs sensitivities in adj dir pi, lam, t wrt. p // if (i < N) // blasfeo_dveccp(nx[i + 1], sens_nlp_out->pi + i, 0, tmp_qp_in->b + i, 0); @@ -3751,7 +3757,7 @@ void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dim // blasfeo_dveccp(2 * ni[i], sens_nlp_out->t + i, ?); } - config->qp_solver->eval_adj_sens(config->qp_solver, dims->qp_solver, tmp_qp_in, tmp_qp_out, + config->qp_solver->eval_adj_sens(config->qp_solver, dims->qp_solver, mem->qp_in, qp_seed, tmp_qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); if (!strcmp("p_global", field)) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index c8b9e6aa43..cc52c24881 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -483,6 +483,9 @@ typedef struct ocp_nlp_workspace ocp_qp_res *qp_res; ocp_qp_res_ws *qp_res_ws; + // qp seed (for solution sensitivities) + ocp_qp_seed *qp_seed; + // for globalization: -> move to module?! ocp_nlp_out *tmp_nlp_out; ocp_nlp_out *weight_merit_fun; diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 6038b51999..6fd0f91ad4 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -278,6 +278,33 @@ double ocp_qp_out_compute_primal_nrm_inf(ocp_qp_out* qp_out) return res; } +/************************************************ + * seed + ************************************************/ + +acados_size_t ocp_qp_seed_calculate_size(ocp_qp_dims *dims) +{ + acados_size_t size = sizeof(ocp_qp_seed); + size += d_ocp_qp_seed_memsize(dims); + return size; +} + + + +ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory) +{ + char *c_ptr = (char *) raw_memory; + + ocp_qp_seed *qp_seed = (ocp_qp_seed *) c_ptr; + c_ptr += sizeof(ocp_qp_seed); + + d_ocp_qp_seed_create(dims, qp_seed, c_ptr); + c_ptr += d_ocp_qp_seed_memsize(dims); + + assert((char *) raw_memory + ocp_qp_seed_calculate_size(dims) == c_ptr); + + return qp_seed; +} /************************************************ diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index 5ac123ff70..6c660e2284 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -51,6 +51,7 @@ typedef struct d_ocp_qp ocp_qp_in; typedef struct d_ocp_qp_sol ocp_qp_out; typedef struct d_ocp_qp_res ocp_qp_res; typedef struct d_ocp_qp_res_ws ocp_qp_res_ws; +typedef struct d_ocp_qp_seed ocp_qp_seed; @@ -72,8 +73,8 @@ typedef struct int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); void (*solver_get)(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); } qp_solver_config; #endif @@ -98,9 +99,11 @@ typedef struct acados_size_t (*workspace_calculate_size)(void *dims, void *opts); 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_rhs_seed)(void *qp_in, void *seed, void *xcond_seed, 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); + int (*expand_sol_seed)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); } ocp_qp_xcond_config; @@ -172,7 +175,11 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res // void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]); - +/* seed */ +// +acados_size_t ocp_qp_seed_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory); /* misc */ // diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index 72f229e9c8..b2c76290fe 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -316,12 +316,12 @@ acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims_, void *op size += sizeof(ocp_qp_full_condensing_memory); size += dense_qp_in_calculate_size(dims->fcond_dims); - size += dense_qp_out_calculate_size(dims->fcond_dims); + size += dense_qp_seed_calculate_size(dims->fcond_dims); size += ocp_qp_in_calculate_size(dims->red_dims); - size += ocp_qp_out_calculate_size(dims->red_dims); + size += ocp_qp_seed_calculate_size(dims->red_dims); size += sizeof(struct d_cond_qp_ws); size += d_cond_qp_ws_memsize(dims->red_dims, opts->hpipm_cond_opts); @@ -374,12 +374,18 @@ void *ocp_qp_full_condensing_memory_assign(void *dims_, void *opts_, void *raw_m mem->fcond_qp_out = dense_qp_out_assign(dims->fcond_dims, c_ptr); c_ptr += dense_qp_out_calculate_size(dims->fcond_dims); + mem->fcond_qp_seed = dense_qp_seed_assign(dims->fcond_dims, c_ptr); + c_ptr += dense_qp_seed_calculate_size(dims->fcond_dims); + mem->red_qp = ocp_qp_in_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_in_calculate_size(dims->red_dims); mem->red_sol = ocp_qp_out_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->red_dims); + mem->red_seed = ocp_qp_seed_assign(dims->red_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->red_dims); + mem->qp_out_info = (qp_info *) mem->fcond_qp_out->misc; assert((char *) raw_memory + ocp_qp_full_condensing_memory_calculate_size(dims, opts) >= c_ptr); @@ -403,6 +409,11 @@ void ocp_qp_full_condensing_memory_get(void *config_, void *mem_, const char *fi dense_qp_out **ptr = value; *ptr = mem->fcond_qp_out; } + else if(!strcmp(field, "xcond_seed")) + { + dense_qp_seed **ptr = value; + *ptr = mem->fcond_qp_seed; + } else if(!strcmp(field, "qp_out_info")) { qp_info **ptr = value; @@ -557,6 +568,34 @@ int ocp_qp_full_condensing_condense_rhs(void *qp_in_, void *fcond_qp_in_, void * } +int ocp_qp_full_condensing_condense_rhs_seed(void *qp_in_, void *qp_seed, void *dense_seed_, void *opts_, void *mem_, void *work) +{ + ocp_qp_in *qp_in = qp_in_; + dense_qp_seed *dense_seed = dense_seed_; + + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // save pointers to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + mem->ptr_qp_seed = qp_seed; + + // reduce eq constr DOF: residual + d_ocp_qp_reduce_eq_dof_seed(qp_in, qp_seed, mem->red_seed, opts->hpipm_red_opts, mem->hpipm_red_work); + + // convert to fully condensed qp structure + d_cond_qp_cond_seed(mem->red_qp, mem->red_seed, dense_seed, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) @@ -577,9 +616,6 @@ int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void // restore solution d_ocp_qp_restore_eq_dof(mem->ptr_qp_in, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); -//d_ocp_qp_sol_print(mem->red_sol->dim, mem->red_sol); -//d_ocp_qp_sol_print(qp_out->dim, qp_out); -//exit(1); // stop timer mem->time_qp_xcond += acados_toc(&timer); @@ -587,6 +623,30 @@ int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void } +int ocp_qp_full_condensing_expand_sol_seed(void *fcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) +{ + dense_qp_out *fcond_qp_out = fcond_qp_out_; + ocp_qp_out *qp_out = qp_out_; + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // expand solution + d_cond_qp_expand_sol_seed(mem->red_qp, mem->red_seed, fcond_qp_out, mem->red_sol, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + // restore solution + d_ocp_qp_restore_eq_dof_seed(mem->ptr_qp_in, mem->ptr_qp_seed, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + void ocp_qp_full_condensing_config_initialize_default(void *config_) { @@ -607,9 +667,11 @@ void ocp_qp_full_condensing_config_initialize_default(void *config_) config->workspace_calculate_size = &ocp_qp_full_condensing_workspace_calculate_size; config->condensing = &ocp_qp_full_condensing; config->condense_rhs = &ocp_qp_full_condensing_condense_rhs; + config->condense_rhs_seed = &ocp_qp_full_condensing_condense_rhs_seed; config->condense_lhs = &ocp_qp_full_condensing_condense_lhs; config->condense_qp_out = &ocp_qp_full_condensing_condense_qp_out; config->expansion = &ocp_qp_full_expansion; + config->expand_sol_seed = &ocp_qp_full_condensing_expand_sol_seed; return; } diff --git a/acados/ocp_qp/ocp_qp_full_condensing.h b/acados/ocp_qp/ocp_qp_full_condensing.h index 39ca81c09a..d9483f6b88 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/acados/ocp_qp/ocp_qp_full_condensing.h @@ -74,10 +74,13 @@ typedef struct ocp_qp_full_condensing_memory_ // in memory dense_qp_in *fcond_qp_in; dense_qp_out *fcond_qp_out; + dense_qp_seed *fcond_qp_seed; ocp_qp_in *red_qp; // reduced qp ocp_qp_out *red_sol; // reduced qp sol + ocp_qp_seed *red_seed; // only pointer ocp_qp_in *ptr_qp_in; + ocp_qp_seed *ptr_qp_seed; qp_info *qp_out_info; // info in fcond_qp_in double time_qp_xcond; } ocp_qp_full_condensing_memory; diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index b490901403..72667e929f 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -450,7 +450,7 @@ void ocp_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *o } -void ocp_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void ocp_qp_hpipm_eval_forw_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { ocp_qp_in *param_qp_in = param_qp_in_; ocp_qp_out *sens_qp_out = sens_qp_out_; @@ -458,13 +458,13 @@ void ocp_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out ocp_qp_hpipm_opts *opts = opts_; ocp_qp_hpipm_memory *mem = mem_; - d_ocp_qp_ipm_sens(param_qp_in, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); + d_ocp_qp_ipm_sens_frw(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); return; } -void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { ocp_qp_in *param_qp_in = param_qp_in_; ocp_qp_out *sens_qp_out = sens_qp_out_; @@ -472,7 +472,7 @@ void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp ocp_qp_hpipm_opts *opts = opts_; ocp_qp_hpipm_memory *mem = mem_; - d_ocp_qp_ipm_sens_adj(param_qp_in, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); + d_ocp_qp_ipm_sens_adj(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); return; } @@ -504,7 +504,7 @@ void ocp_qp_hpipm_config_initialize_default(void *config_) config->evaluate = &ocp_qp_hpipm; config->solver_get = &ocp_qp_hpipm_solver_get; config->memory_reset = &ocp_qp_hpipm_memory_reset; - config->eval_sens = &ocp_qp_hpipm_eval_sens; + config->eval_forw_sens = &ocp_qp_hpipm_eval_forw_sens; config->eval_adj_sens = &ocp_qp_hpipm_eval_adj_sens; config->terminate = &ocp_qp_hpipm_terminate; diff --git a/acados/ocp_qp/ocp_qp_hpipm.h b/acados/ocp_qp/ocp_qp_hpipm.h index 9c78922ddb..26af6e839e 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.h +++ b/acados/ocp_qp/ocp_qp_hpipm.h @@ -88,8 +88,6 @@ void ocp_qp_hpipm_memory_reset(void *config_, void *qp_in_, void *qp_out_, void // void ocp_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); // -void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_hpipm_config_initialize_default(void *config); diff --git a/acados/ocp_qp/ocp_qp_hpmpc.c b/acados/ocp_qp/ocp_qp_hpmpc.c index d7c88b4d9c..06486381ef 100644 --- a/acados/ocp_qp/ocp_qp_hpmpc.c +++ b/acados/ocp_qp/ocp_qp_hpmpc.c @@ -577,9 +577,9 @@ void ocp_qp_hpmpc_solver_get(void *config_, void *qp_in_, void *qp_out_, void *o exit(1); } -void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_hpmpc_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_hpmpc_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_hpmpc_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -616,7 +616,7 @@ void ocp_qp_hpmpc_config_initialize_default(void *config_) config->workspace_calculate_size = (size_t (*)(void *, void *, void *)) & ocp_qp_hpmpc_workspace_calculate_size; config->evaluate = &ocp_qp_hpmpc; - config->eval_sens = &ocp_qp_hpmpc_eval_sens; + config->eval_forw_sens = &ocp_qp_hpmpc_eval_forw_sens; config->eval_adj_sens = &ocp_qp_hpmpc_eval_adj_sens; config->memory_reset = &ocp_qp_hpmpc_memory_reset; config->solver_get = &ocp_qp_hpmpc_solver_get; diff --git a/acados/ocp_qp/ocp_qp_hpmpc.h b/acados/ocp_qp/ocp_qp_hpmpc.h index a830629b68..ab620f926e 100644 --- a/acados/ocp_qp/ocp_qp_hpmpc.h +++ b/acados/ocp_qp/ocp_qp_hpmpc.h @@ -118,7 +118,7 @@ int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *me // void ocp_qp_hpmpc_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // -void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +void ocp_qp_hpmpc_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void ocp_qp_hpmpc_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2) // diff --git a/acados/ocp_qp/ocp_qp_ooqp.c b/acados/ocp_qp/ocp_qp_ooqp.c index 1640ada5be..bb603ec932 100644 --- a/acados/ocp_qp/ocp_qp_ooqp.c +++ b/acados/ocp_qp/ocp_qp_ooqp.c @@ -1176,9 +1176,9 @@ void ocp_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *op } -void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_ooqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_ooqp_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_ooqp_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -1215,7 +1215,7 @@ void ocp_qp_ooqp_config_initialize_default(void *config_) config->workspace_calculate_size = (size_t (*)(void *, void *, void *)) & ocp_qp_ooqp_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & ocp_qp_ooqp; - config->eval_sens = &ocp_qp_ooqp_eval_sens; + config->eval_forw_sens = &ocp_qp_ooqp_eval_forw_sens; config->eval_adj_sens = &ocp_qp_ooqp_eval_adj_sens; config->memory_reset = &ocp_qp_ooqp_memory_reset; config->solver_get = &ocp_qp_ooqp_solver_get; diff --git a/acados/ocp_qp/ocp_qp_ooqp.h b/acados/ocp_qp/ocp_qp_ooqp.h index 1b33bd4152..6672ac58b1 100644 --- a/acados/ocp_qp/ocp_qp_ooqp.h +++ b/acados/ocp_qp/ocp_qp_ooqp.h @@ -133,8 +133,6 @@ int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_ // void ocp_qp_ooqp_destroy(void *mem_, void *work); // -void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_ooqp_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // void ocp_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index d3c341fdad..7df6f585af 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -1788,13 +1788,13 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m -void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_osqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_osqp_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_osqp_eval_forw_sens: not implemented yet\n"); exit(1); } -void ocp_qp_osqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_osqp_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: ocp_qp_osqp_eval_adj_sens: not implemented yet\n"); exit(1); @@ -1824,7 +1824,7 @@ void ocp_qp_osqp_config_initialize_default(void *config_) config->workspace_calculate_size = &ocp_qp_osqp_workspace_calculate_size; config->evaluate = &ocp_qp_osqp; config->terminate = &ocp_qp_osqp_terminate; - config->eval_sens = &ocp_qp_osqp_eval_sens; + config->eval_forw_sens = &ocp_qp_osqp_eval_forw_sens; config->eval_adj_sens = &ocp_qp_osqp_eval_adj_sens; config->memory_reset = &ocp_qp_osqp_memory_reset; config->solver_get = &ocp_qp_osqp_solver_get; diff --git a/acados/ocp_qp/ocp_qp_osqp.h b/acados/ocp_qp/ocp_qp_osqp.h index e4158c9e59..6e8ff677f5 100644 --- a/acados/ocp_qp/ocp_qp_osqp.h +++ b/acados/ocp_qp/ocp_qp_osqp.h @@ -92,8 +92,6 @@ acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, voi // int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // -void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_osqp_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // void ocp_qp_osqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index 0ff4ab68e2..a8b8cc6cbd 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -376,12 +376,12 @@ acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims_, void size += sizeof(ocp_qp_partial_condensing_memory); size += ocp_qp_in_calculate_size(dims->pcond_dims); - size += ocp_qp_out_calculate_size(dims->pcond_dims); + size += ocp_qp_seed_calculate_size(dims->pcond_dims); size += ocp_qp_in_calculate_size(dims->red_dims); - size += ocp_qp_out_calculate_size(dims->red_dims); + size += ocp_qp_seed_calculate_size(dims->red_dims); // hpipm_pcond_work size += sizeof(struct d_part_cond_qp_ws); @@ -435,12 +435,18 @@ void *ocp_qp_partial_condensing_memory_assign(void *dims_, void *opts_, void *ra mem->pcond_qp_out = ocp_qp_out_assign(dims->pcond_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->pcond_dims); + mem->pcond_qp_seed = ocp_qp_seed_assign(dims->pcond_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->pcond_dims); + mem->red_qp = ocp_qp_in_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_in_calculate_size(dims->red_dims); mem->red_sol = ocp_qp_out_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->red_dims); + mem->red_seed = ocp_qp_seed_assign(dims->red_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->red_dims); + mem->qp_out_info = (qp_info *) mem->pcond_qp_out->misc; mem->dims = dims; @@ -466,6 +472,11 @@ void ocp_qp_partial_condensing_memory_get(void *config_, void *mem_, const char ocp_qp_out **ptr = value; *ptr = mem->pcond_qp_out; } + else if(!strcmp(field, "xcond_seed")) + { + ocp_qp_seed **ptr = value; + *ptr = mem->pcond_qp_seed; + } else if(!strcmp(field, "qp_out_info")) { qp_info **ptr = value; @@ -614,6 +625,36 @@ int ocp_qp_partial_condensing_condense_rhs(void *qp_in_, void *pcond_qp_in_, voi +int ocp_qp_partial_condensing_condense_rhs_seed(void *qp_in_, void *qp_seed, void *pcond_seed, void *opts_, void *mem_, void *work) +{ + ocp_qp_in *qp_in = qp_in_; + ocp_qp_partial_condensing_opts *opts = opts_; + ocp_qp_partial_condensing_memory *mem = mem_; + + assert(opts->N2 == opts->N2_bkp); + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // save pointers to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + mem->ptr_qp_seed = qp_seed; + + // reduce eq constr DOF: residual + d_ocp_qp_reduce_eq_dof_seed(qp_in, qp_seed, mem->red_seed, opts->hpipm_red_opts, mem->hpipm_red_work); + + // convert to partially condensed qp structure + d_part_cond_qp_cond_seed(mem->red_qp, mem->red_seed, pcond_seed, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + + int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) { ocp_qp_out *pcond_qp_out = pcond_qp_out_; @@ -630,7 +671,7 @@ int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, vo // expand solution // TODO only if N2red_qp, mem->ptr_pcond_qp_in, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + d_part_cond_qp_expand_sol(mem->red_qp, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); // restore solution d_ocp_qp_restore_eq_dof(mem->ptr_qp_in, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); @@ -641,6 +682,32 @@ int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, vo return ACADOS_SUCCESS; } +int ocp_qp_partial_condensing_expand_sol_seed(void *pcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) +{ + ocp_qp_out *pcond_qp_out = pcond_qp_out_; + ocp_qp_out *qp_out = qp_out_; + ocp_qp_partial_condensing_opts *opts = opts_; + ocp_qp_partial_condensing_memory *mem = mem_; + + assert(opts->N2 == opts->N2_bkp); + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // expand solution + d_part_cond_qp_expand_sol_seed(mem->red_qp, mem->red_seed, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + + // restore solution + d_ocp_qp_restore_eq_dof_seed(mem->ptr_qp_in, mem->ptr_qp_seed, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + void ocp_qp_partial_condensing_config_initialize_default(void *config_) @@ -663,8 +730,10 @@ void ocp_qp_partial_condensing_config_initialize_default(void *config_) config->condensing = &ocp_qp_partial_condensing; config->condense_lhs = &ocp_qp_partial_condensing_condense_lhs; config->condense_rhs = &ocp_qp_partial_condensing_condense_rhs; + config->condense_rhs_seed = &ocp_qp_partial_condensing_condense_rhs_seed; config->condense_qp_out = &ocp_qp_partial_condensing_condense_qp_out; config->expansion = &ocp_qp_partial_expansion; + config->expand_sol_seed = &ocp_qp_partial_condensing_expand_sol_seed; return; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.h b/acados/ocp_qp/ocp_qp_partial_condensing.h index 581fb3c77a..b5687ad50d 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -76,11 +76,14 @@ typedef struct ocp_qp_partial_condensing_memory_ // in memory ocp_qp_in *pcond_qp_in; ocp_qp_out *pcond_qp_out; + ocp_qp_seed *pcond_qp_seed; ocp_qp_in *red_qp; // reduced qp ocp_qp_out *red_sol; // reduced qp sol + ocp_qp_seed *red_seed; // only pointer ocp_qp_in *ptr_qp_in; ocp_qp_in *ptr_pcond_qp_in; + ocp_qp_seed *ptr_qp_seed; qp_info *qp_out_info; // info in pcond_qp_in ocp_qp_partial_condensing_dims *dims; double time_qp_xcond; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.c b/acados/ocp_qp/ocp_qp_qpdunes.c index 87c82ec7d3..a36c729963 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.c +++ b/acados/ocp_qp/ocp_qp_qpdunes.c @@ -925,13 +925,13 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *in, ocp_qp_out *out, void *opts_, v -void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_qpdunes_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_qpdunes_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_qpdunes_eval_forw_sens: not implemented yet\n"); exit(1); } -void ocp_qp_qpdunes_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_qpdunes_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: ocp_qp_qpdunes_eval_adj_sens: not implemented yet\n"); exit(1); @@ -971,7 +971,7 @@ void ocp_qp_qpdunes_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & ocp_qp_qpdunes_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & ocp_qp_qpdunes; - config->eval_sens = &ocp_qp_qpdunes_eval_sens; + config->eval_forw_sens = &ocp_qp_qpdunes_eval_forw_sens; config->eval_adj_sens = &ocp_qp_qpdunes_eval_adj_sens; config->solver_get = &ocp_qp_qpdunes_solver_get; config->terminate = &ocp_qp_qpdunes_terminate; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.h b/acados/ocp_qp/ocp_qp_qpdunes.h index ecbc6da63f..60e3288ba6 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.h +++ b/acados/ocp_qp/ocp_qp_qpdunes.h @@ -108,8 +108,6 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *op // void ocp_qp_qpdunes_free_memory(void *mem_); // -void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_qpdunes_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); // void ocp_qp_qpdunes_config_initialize_default(void *config_); diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.c b/acados/ocp_qp/ocp_qp_xcond_solver.c index bcc7926277..9150a0eb0c 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -371,6 +371,7 @@ void *ocp_qp_xcond_solver_memory_assign(void *config_, ocp_qp_xcond_solver_dims xcond->memory_get(xcond, mem->xcond_memory, "xcond_qp_in", &mem->xcond_qp_in); xcond->memory_get(xcond, mem->xcond_memory, "xcond_qp_out", &mem->xcond_qp_out); + xcond->memory_get(xcond, mem->xcond_memory, "xcond_seed", &mem->xcond_seed); assert((char *) raw_memory + ocp_qp_xcond_solver_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -656,7 +657,7 @@ int ocp_qp_xcond_condense_rhs_and_solve(void *config_, ocp_qp_xcond_solver_dims -void ocp_qp_xcond_solver_eval_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, +void ocp_qp_xcond_solver_eval_forw_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts_, void *mem_, void *work_) { ocp_qp_xcond_solver_config *config = config_; @@ -672,18 +673,18 @@ void ocp_qp_xcond_solver_eval_sens(void *config_, ocp_qp_xcond_solver_dims *dims cast_workspace(config_, dims, opts, memory, work); // condensing - xcond->condense_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->condense_rhs_seed(param_qp_in, seed, memory->xcond_seed, opts->xcond_opts, memory->xcond_memory, work->xcond_work); // qp evaluate sensitivity - qp_solver->eval_sens(qp_solver, memory->xcond_qp_in, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); + qp_solver->eval_forw_sens(qp_solver, memory->xcond_qp_in, memory->xcond_seed, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); // expansion - xcond->expansion(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->expand_sol_seed(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); return; } -void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, +void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts_, void *mem_, void *work_) { ocp_qp_xcond_solver_config *config = config_; @@ -699,13 +700,13 @@ void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims * cast_workspace(config_, dims, opts, memory, work); // condensing - xcond->condense_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->condense_rhs_seed(param_qp_in, seed, memory->xcond_seed, opts->xcond_opts, memory->xcond_memory, work->xcond_work); // qp evaluate sensitivity - qp_solver->eval_adj_sens(qp_solver, memory->xcond_qp_in, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); + qp_solver->eval_adj_sens(qp_solver, memory->xcond_qp_in, memory->xcond_seed, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); // expansion - xcond->expansion(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->expand_sol_seed(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); return; } @@ -748,7 +749,7 @@ void ocp_qp_xcond_solver_config_initialize_default(void *config_) config->evaluate = &ocp_qp_xcond_solve; config->condense_lhs = &ocp_qp_xcond_condense_lhs; config->condense_rhs_and_solve = &ocp_qp_xcond_condense_rhs_and_solve; - config->eval_sens = &ocp_qp_xcond_solver_eval_sens; + config->eval_forw_sens = &ocp_qp_xcond_solver_eval_forw_sens; config->eval_adj_sens = &ocp_qp_xcond_solver_eval_adj_sens; config->terminate = &ocp_qp_xcond_solver_terminate; diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.h b/acados/ocp_qp/ocp_qp_xcond_solver.h index a0e20c1fb6..771548c5b1 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.h +++ b/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -65,6 +65,7 @@ typedef struct ocp_qp_xcond_solver_memory_ void *solver_memory; void *xcond_qp_in; void *xcond_qp_out; + void *xcond_seed; } ocp_qp_xcond_solver_memory; @@ -98,8 +99,8 @@ typedef struct int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); int (*condense_lhs)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); int (*condense_rhs_and_solve)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver ocp_qp_xcond_config *xcond; diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index 5f68ad44e1..0475524575 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -518,10 +518,15 @@ def main_parametric(qp_solver_ric_alg: int = 0, # print(f"{sens_u_=}") # print(f"{sens_x_=}") - test_tol = 1e-5 + test_tol = 1e-9 diff_sens_adj_vs_ref = np.max(np.abs(sens_adj_ref.ravel() - sens_adj)) if diff_sens_adj_vs_ref > test_tol: raise_test_failure_message(f"diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} should be < {test_tol}") + else: + print(f"Success: diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} < {test_tol}") + + # assert not all zero + assert np.max(np.abs(sens_adj)) > 1.0 sens_u.append(sens_u_[:, p_idx]) @@ -537,7 +542,12 @@ def main_parametric(qp_solver_ric_alg: int = 0, print(f"i {i} {timings_solve_params_adj[i]*1e3:.5f} \t {timings_solve_params[i]*1e3:.5f} \t {timings_solve_params_adj_uforw[i]*1e3:.5f} \t {timings_solve_params_adj_all_primals[i]*1e3:.5f}") # check wrt forward - print(np.abs(sens_adj- out_dict['sens_u'])) + diff_sens_u_vs_via_adj = np.max(np.abs(sens_adj- out_dict['sens_u'])) + if diff_sens_u_vs_via_adj > test_tol: + raise_test_failure_message(f"diff_sens_u_vs_via_adj = {diff_sens_u_vs_via_adj} should be < {test_tol}") + else: + print(f"Success: diff_sens_u_vs_via_adj = {diff_sens_u_vs_via_adj} < {test_tol}") + # assert np.allclose(sens_adj, out_dict['sens_u']) if generate_plots: diff --git a/external/hpipm b/external/hpipm index 185517cf53..945645e8c0 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 185517cf539598cf173b45fd4d4ea2a74eed3f38 +Subproject commit 945645e8c0ac473f3983fb3c7f22dbae50d864cc From 7c2faf954e8d923c17cedac773e4540c470fa509 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Mon, 5 May 2025 15:24:36 +0200 Subject: [PATCH 045/164] Python: fix `u_labels` init (#1520) When not set, `u_labels` were wrongly initialized with the dimension of `x`. --- interfaces/acados_template/acados_template/acados_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index e0a75c3f56..e3aa7415f7 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -797,7 +797,7 @@ def x_labels(self, x_labels): def u_labels(self): """Contains list of labels for the controls. Default: :code:`None`""" if self.__u_labels is None: - return [f"x{i}" for i in range(self.x.size()[0])] + return [f"u{i}" for i in range(self.u.size()[0])] else: return self.__u_labels From c0ff95493b78d1530441285a59b95456db4165ba Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Mon, 5 May 2025 20:28:56 +0200 Subject: [PATCH 046/164] Docs update (#1519) This PR fixes a few things in the documentation, mainly broken links and Python docstring formatting. --------- Co-authored-by: Jonathan Frey --- .github/workflows/full_build.yml | 6 +- .gitignore | 2 +- CMakeLists.txt | 2 +- ROADMAP.md | 22 +- docs/c_interface/index.md | 4 +- docs/embedded_workflow/index.md | 14 +- docs/features/index.md | 20 +- docs/installation/index.md | 14 +- docs/interfaces/index.md | 2 +- docs/list_of_projects/index.md | 4 +- docs/matlab_octave_interface/index.md | 22 +- .../problem_formulation_ocp_mex.tex | 2 +- docs/troubleshooting/index.md | 8 +- .../p_global_example/simulink_test_p_global.m | 4 +- .../pendulum_on_cart_model/zoro_example.m | 2 +- .../acados_matlab_octave/race_cars/README.md | 2 +- .../test/simulink_sparse_param_test.m | 4 +- .../ocp/minimal_example_ocp_reuse_code.py | 4 +- .../ocp/nonuniform_discretization_example.py | 2 +- interfaces/acados_matlab_octave/AcadosOcp.m | 2 +- .../acados_matlab_octave/AcadosOcpSolver.m | 4 +- .../acados_matlab_octave/AcadosSimSolver.m | 2 +- .../acados_install_windows.m | 2 +- interfaces/acados_matlab_octave/acados_ocp.m | 2 +- .../acados_matlab_octave/acados_ocp_opts.m | 2 +- .../acados_matlab_octave/acados_sim_opts.m | 2 +- .../compile_ocp_shared_lib.m | 2 +- .../check_acados_requirements.m | 2 +- .../detect_gnsf_structure.m | 2 +- .../ocp_compile_interface.m | 2 +- .../acados_matlab_octave/set_up_t_renderer.m | 2 +- .../acados_template/acados_ocp.py | 2 +- .../acados_template/acados_ocp_options.py | 6 +- .../acados_template/acados_ocp_solver.py | 196 +++++++++--------- .../acados_template/acados_sim.py | 6 +- .../acados_template/acados_sim_solver.py | 22 +- .../c_templates_tera/CMakeLists.in.txt | 2 +- .../c_templates_tera/multi_CMakeLists.in.txt | 2 +- .../casadi_function_generation.py | 2 +- .../gnsf/detect_gnsf_structure.py | 2 +- 40 files changed, 203 insertions(+), 203 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 5962663278..9c5564fb69 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -279,7 +279,7 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test source env.sh - - name: Run Matlab tests + - name: Run MATLAB tests uses: matlab-actions/run-command@v2 if: always() with: @@ -338,7 +338,7 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test source env.sh - - name: Run Matlab tests + - name: Run MATLAB tests uses: matlab-actions/run-command@v2 if: always() with: @@ -399,7 +399,7 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test source env.sh - - name: Run Matlab examples + - name: Run MATLAB examples uses: matlab-actions/run-command@v2 if: always() with: diff --git a/.gitignore b/.gitignore index eebbf8b88a..62f1a779fd 100644 --- a/.gitignore +++ b/.gitignore @@ -83,7 +83,7 @@ Thumbs.db *download_software *download_matlab -# Matlab # +# MATLAB # *.m~ # Python # diff --git a/CMakeLists.txt b/CMakeLists.txt index f7e6bf8aaa..d752c3642a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -120,7 +120,7 @@ endif() if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows") # MinGW: remove prefix and change suffix to match MSVC - # (such that Matlab mex recognizes the libraries) + # (such that MATLAB mex recognizes the libraries) set(CMAKE_SHARED_LIBRARY_PREFIX "") set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib") set(CMAKE_IMPORT_LIBRARY_PREFIX "") diff --git a/ROADMAP.md b/ROADMAP.md index 97edbd5091..dc2d6a894b 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -1,23 +1,18 @@ ## Roadmap - [ ] Test 32 bit - [ ] Test OOQP -- [ ] More flexible solution sensitivities? - [ ] get_optimal_value_hessian() - at least for HPIPM and exact Hessians -- [ ] remove old layer in Matlab interface +- [ ] remove old layer in MATLAB interface #### core - [x] propagate cost in integrator for NLS+IRK - or: add support for quadrature state, separate dimension in integrator and OCP solver - [ ] faster workspace memory casting -#### `ocp_nlp` -- [ ] partial tightening -- [x] RTI implementation similar to ACADO -- [x] support cost on z for external, NLS #### `sim` - [ ] GNSF Hessians -- [ ] propagate cost in integrator for CONL+IRK +- [x] propagate cost in integrator for CONL+IRK - [ ] time in integrator + time dependent model functions @@ -25,15 +20,12 @@ - [x] closed loop example MPC + MHE - [x] Templates: avoid global memory - [x] add support for manual model functions -- partly done: external cost and discrete dynamics +- [x] More flexible solution sensitivities -#### `sim` -- [x] collocation integrators Radau - - NOTE: currently always Gauss(-Legendre) Butcher tables - - A-stable, but not L-stable - - order is 2 * num_stages - - implement also Radau IIA collocation methods - - L-stable - - order is 2 * num_stages - 1 +#### `ocp_nlp` +- [x] partial tightening -> implemented via multi-phase +- [x] RTI implementation similar to ACADO +- [x] support cost on z for external, NLS #### C - [x] split ocp solve into prepare and feedback diff --git a/docs/c_interface/index.md b/docs/c_interface/index.md index a323fd60a0..67e0fef012 100644 --- a/docs/c_interface/index.md +++ b/docs/c_interface/index.md @@ -11,10 +11,10 @@ It is recommended to instead look at the header files in [`interfaces/acados_c/` ## Examples -Note that the `Matlab` and `Python` interfaces can be used to code generate `C` examples which are cleaner than some of the plain C examples in [`examples/c`](https://github.com/acados/acados/tree/main/examples/c). +Note that the `MATLAB` and `Python` interfaces can be used to code generate `C` examples which are cleaner than some of the plain C examples in [`examples/c`](https://github.com/acados/acados/tree/main/examples/c). A recommended workflow is thus to prototype an NMPC controller from one of the high-level interfaces and deploy the generated code with minor modifications in a `C`, `C++` or `ROS` framework. -A very important resource are the templated `C` files used by the template based interfaces (`Python` and code generation for `Matlab` and `Octave`), which show how to use the `C` interface properly. +A very important resource are the templated `C` files used by the template based interfaces (`Python` and code generation for `MATLAB` and `Octave`), which show how to use the `C` interface properly. These templates are actively maintained and tested using CI. The templates can be found in [`interfaces/acados_template/acados_template/c_templates_tera`](https://github.com/acados/acados/tree/main/interfaces/acados_template/acados_template/c_templates_tera). diff --git a/docs/embedded_workflow/index.md b/docs/embedded_workflow/index.md index a7c9f35221..96ae77ccee 100644 --- a/docs/embedded_workflow/index.md +++ b/docs/embedded_workflow/index.md @@ -15,7 +15,7 @@ In the next section another workflow for DS1401 and DS1403 is also explained. ### Prerequisites - you were able to install `acados` and dSPACE on your system - you were able to generate S-Functions with acados, which also work in your Simulink simulation `'Simulation_Model_Name'.slx`. -Thus, you have a folder `c_generated_code` with your S-Functions, a `make_sfun.m` Matlab script (and a `make_sfun_sim.m` script, if needed) and the corresponding C files. +Thus, you have a folder `c_generated_code` with your S-Functions, a `make_sfun.m` MATLAB script (and a `make_sfun_sim.m` script, if needed) and the corresponding C files. - you have prepared a Simulink model with the name `'dSPACE_Model_Name'.slx`, which does not contain the S-Functions yet and you were able to compile it for your dSPACE Platform. During the compilation process, the dSPACE Makefile `'dSPACE_Model_Name'_usr.mk` was created, which can be found in the same directory as the dSPACE Simulink model. @@ -52,14 +52,14 @@ These are the folders you need to deploy `acados` on your dSPACE Platform. ### Step 3: Create a dSPACE build folder, prepare Simulink model 1. Create a new folder `'dSPACE_Build_Folder_Name'` (anywhere) and copy your Simulink model `'dSPACE_Model_Name'.slx`, the dSPACE Makefile `'dSPACE_Model_Name'_usr.mk` and the `acados` S-Function folder `c_generated_code` to this folder. 2. Copy the two folders `lib` and `include`, which you created in the cross-compiling process (Step 2), to this folder too. -3. Add the folders `c_generated_code` and `lib` to the Matlab search path. +3. Add the folders `c_generated_code` and `lib` to the MATLAB search path. 4. Open the Simulink model `'dSPACE_Model_Name'.slx`, and copy the `acados` S-Function(s) from the Simulink simulation file `'Simulation_Model_Name'.slx` into the dSPACE Simulink model. Make sure the S-Function(s) get the correct inputs (Ctrl+D to check). ### Step 4: Adapt the dSPACE Makefile Adapt the dSPACE Makefile in order to include the `acados` headers, libraries and additional C code source files. 1. Your `acados` S-Function(s) are based on C code source files. -These files are listed as `SOURCES` in the Matlab script `make_sfun.m` (and `make_sfun_sim.m` if the simulation S-Function is used too). +These files are listed as `SOURCES` in the MATLAB script `make_sfun.m` (and `make_sfun_sim.m` if the simulation S-Function is used too). Open the dSPACE Makefile `'dSPACE_Model_Name'_usr.mk` and list all source files needed for the S-Functions, except for the ones which have the same name as the S-Functions. **Example:** @@ -119,18 +119,18 @@ For the example in the previous step the entry in the dSPACE Makefile would look 5. Save the dSPACE Makefile. ### Step 5: Compile your dSPACE Simulink model for dSPACE -In order to compile your dSPACE Simulink model `'dSPACE_Model_Name'.slx` use the `rtwbuild` command in Matlab or press Ctrl+B in Simulink. +In order to compile your dSPACE Simulink model `'dSPACE_Model_Name'.slx` use the `rtwbuild` command in MATLAB or press Ctrl+B in Simulink. The Makefile should now integrate all the necessary files for the compilation of the `acados` S-Functions. ## dSPACE DS1401 and DS1403 Here, an alternative workflow for the deployment of `acados` on a dSPACE Platform is described. -This has been successfully tested with Matlab / Simulink R2018b on the DS1401 MicroAutobox-II (MABX2) and the DS1403 MicroAutobox-III (MABX3). +This has been successfully tested with MATLAB / Simulink R2018b on the DS1401 MicroAutobox-II (MABX2) and the DS1403 MicroAutobox-III (MABX3). With some adaptation this method could also work with different dSPACE Platforms. ### Prerequisites - you were able to install `acados` and dSPACE on your system - you were able to generate S-Functions with acados, which also work in your Simulink simulation `'Simulation_Model_Name'.slx`. -Thus, you have a folder `c_generated_code` with your S-Functions, a `make_sfun.m` Matlab script (and a `make_sfun_sim.m` script, if needed) and the corresponding C files. +Thus, you have a folder `c_generated_code` with your S-Functions, a `make_sfun.m` MATLAB script (and a `make_sfun_sim.m` script, if needed) and the corresponding C files. - you have prepared a Simulink model with the name `'dSPACE_Model_Name'.slx`, which does not contain the S-Functions yet and you were able to compile it for your dSPACE Platform. - your dSPACE installation, and your project folder, do not contain spaces in their paths (all the paths you will use in the next steps should not contain any space). It is usually sufficient to copy-paste the compiler folder to a new one without spaces in it, without re-installing the whole dSPACE software suite) @@ -162,7 +162,7 @@ If all these steps worked, you will find the two folders `lib` and `include` in These are the folders you need to deploy `acados` on your dSPACE Platform. ### Step 3: Prepare and build Simulink model -1. Add `c_generated_code` folder to Matlab path. +1. Add `c_generated_code` folder to MATLAB path. 2. Open Simulink model configuration parameters, and under Code Generation / Custom Code / Additional build info, add the following paths: - Include directories: all the include directories in the `buildDS1401/install/include` (or `buildDS1403/install/include`) folder as in this example: diff --git a/docs/features/index.md b/docs/features/index.md index da2d757ec6..f22b7deb0c 100644 --- a/docs/features/index.md +++ b/docs/features/index.md @@ -31,7 +31,7 @@ Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs an ### Soft constraints -- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/linear_mass_model/solve_marathos_ocp.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/tests/soft_constraint_test.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/test/create_slacked_ocp_qp_solver_formulation.m) @@ -56,8 +56,8 @@ Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs an - [MATLAB/Octave and Simulink](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m) ### Time-varying reference tracking -- [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/control_rates/main.m) - [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/mhe/closed_loop_mhe_ocp.py) +- [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/control_rates/main.m) ### One-sided constraints @@ -74,7 +74,7 @@ Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs an ### Advanced-step real-time iterations (AS-RTI) Relevant publications: [Frey2024a](https://publications.syscop.de/Frey2024a.pdf), [Nurkanovic2019a](https://publications.syscop.de/Nurkanovic2019a.pdf) -- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/as_rti) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py) ### Solver timeout @@ -83,19 +83,19 @@ Relevant publications: [Frey2024a](https://publications.syscop.de/Frey2024a.pdf) ### Cost integration -- [Python](examples/acados_python/tests/test_ggn_cost_integration.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/tests/test_ggn_cost_integration.py) ### Globalization -- [Python](examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py) ### Differential dynamic programming (DDP) -- [Python](examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion/hour_glass_time_optimal_p2p_motion.py) ### Partial condensing Use a `qp_solver` starting with `PARTIAL_CONDENSING`, use `qp_solver_cond_N` to set the horizon of the partially condensed QP. Additionally, one can use `qp_solver_cond_block_size` to specify how many blocks are condensed into one block in partial condensing. -- [Python](examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/p_global_example/set_solver_options.m) @@ -107,10 +107,10 @@ Relevant publications: [Frey2024](https://publications.syscop.de/Frey2024.pdf), ### Solution sensitivities -- [Python](examples/acados_python/chain_mass/solution_sensitivity_example.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/chain_mass/solution_sensitivity_example.py) - currently not supported for MATLAB/Octave ### Adjoint solution sensitivities -- [Python](examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py) -- currently no supported for MATLAB/Octave +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py) +- currently not supported for MATLAB/Octave diff --git a/docs/installation/index.md b/docs/installation/index.md index ca54cba5e6..35b051bf03 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -113,13 +113,13 @@ cd /mnt/c/Users/Documents/ For the installation of Python/MATLAB/Octave interfaces, please refer to the [Interfaces](../interfaces/index.md) page. -## Windows (for use with Matlab) +## Windows (for use with MATLAB) Disclaimer: The high-level interfaces on Windows are not tested on Github Actions. ### Prerequisites You should have the following software installed on your machine. -- Recent Matlab version +- Recent MATLAB version - CMake for Windows - [Windows Git Client](https://git-scm.com/download/win) @@ -139,7 +139,7 @@ git submodule update --recursive --init - You can check whether the modification to `PATH` variable is in effect by executing in cmd `echo %PATH%` or in PowerShell `echo $env:path`. ### Automatic build of acados (minGW) -Run the following in Matlab in the folder `/interfaces/acados_matlab_octave`: +Run the following in MATLAB in the folder `/interfaces/acados_matlab_octave`: ``` acados_install_windows() ``` @@ -175,8 +175,8 @@ mingw32-make.exe -j4 mingw32-make.exe install ``` -### Try a Matlab example -- Open Matlab +### Try a MATLAB example +- Open MATLAB - select MinGW compiler using `mex` - go to `/examples/acados_matlab_octave` - run `acados_env_variables_windows` @@ -195,5 +195,5 @@ cmake -G "Visual Studio 15 2017 Win64" -DBLASFEO_TARGET=GENERIC -DACADOS_INSTALL # cmake -G "Visual Studio 16 2019" -DBLASFEO_TARGET=GENERIC -DACADOS_INSTALL_DIR=.. -DBUILD_SHARED_LIBS=OFF .. cmake --build . -j10 --target INSTALL --config Release ``` -- In Matlab, run `mex -setup C` and select the same `MSVC` version. -- Try a Matlab example (see above). +- In MATLAB, run `mex -setup C` and select the same `MSVC` version. +- Try a MATLAB example (see above). diff --git a/docs/interfaces/index.md b/docs/interfaces/index.md index 5c7ece348b..71b9fc9a91 100644 --- a/docs/interfaces/index.md +++ b/docs/interfaces/index.md @@ -2,6 +2,6 @@ `acados` comes with the following interfaces 1. [`C` Interface](../c_interface/index.md) 2. [`Python` Interface](../python_interface/index.md) -3. [`Matlab` + `Simulink` and `Octave` interface](../matlab_octave_interface/index.md) +3. [`MATLAB` + `Simulink` and `Octave` interface](../matlab_octave_interface/index.md) These interfaces are described on individual subpages for readibility. diff --git a/docs/list_of_projects/index.md b/docs/list_of_projects/index.md index de39697cd4..23276a8a98 100644 --- a/docs/list_of_projects/index.md +++ b/docs/list_of_projects/index.md @@ -28,9 +28,9 @@ It uses the `Cython` wrapper to the `acados` OCP solver in its software stack. - [acados-STM32 - acados Nonlinear MPC example (inverted pendulum control) using HPIPM on STM32H7 device (Cortex-M7 @ 400 MHz)](https://github.com/mindThomas/acados-STM32) - [OpenOCL](https://github.com/OpenOCL/OpenOCL) -is an open-source Matlab toolbox for modeling and solving optimal control problems. +is an open-source MATLAB toolbox for modeling and solving optimal control problems. It can use `CasADi` with IPOPT as a solver. -It also provides a higher level interface to `acados`, which is based on the Matlab interface of `acados`. +It also provides a higher level interface to `acados`, which is based on the MATLAB interface of `acados`. ## Papers featuring `acados` ### with embedded deployment diff --git a/docs/matlab_octave_interface/index.md b/docs/matlab_octave_interface/index.md index 7bc68cbda6..f244dae5c2 100644 --- a/docs/matlab_octave_interface/index.md +++ b/docs/matlab_octave_interface/index.md @@ -1,10 +1,10 @@ -# Matlab + Simulink and Octave Interface +# MATLAB + Simulink and Octave Interface -In order to use `acados` from Octave or Matlab, you need to create the `acados` shared libraries using either the `CMake` or `Make` build system, as described [on the installation page](../installation/index.md). +In order to use `acados` from Octave or MATLAB, you need to create the `acados` shared libraries using either the `CMake` or `Make` build system, as described [on the installation page](../installation/index.md). ## Getting started -Check out the examples [`minimal_example_ocp.m`](https://github.com/acados/acados/tree/main/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m) and [`minimal_example_sim.m`](https://github.com/acados/acados/tree/main/examples/acados_matlab_octave/getting_started/minimal_example_sim.m) to get started with the Matlab interface of `acados`. -Note that `acados` currently supports both an old Matlab interface (< v0.4.0) as well as the new one (>= v0.4.0). +Check out the examples [`minimal_example_ocp.m`](https://github.com/acados/acados/tree/main/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m) and [`minimal_example_sim.m`](https://github.com/acados/acados/tree/main/examples/acados_matlab_octave/getting_started/minimal_example_sim.m) to get started with the MATLAB interface of `acados`. +Note that `acados` currently supports both an old MATLAB interface (< v0.4.0) as well as the new one (>= v0.4.0). Unfortunately, not all MATLAB examples have been ported to the new interface yet. If you are new to `acados` please start with [those examples](https://github.com/acados/acados/issues/1196#issuecomment-2311822122) that use the new interface already. @@ -20,7 +20,7 @@ The problem formulation is stated in [this PDF](https://github.com/acados/acados ## Export environment variables In order to run the examples, some environment variables need to be exported. Instead of running the scripts below, you can modify an `rc` file, like `.bashrc` when launching MATLAB from bash, -[`.matlab7rc.sh`](https://discourse.acados.org/t/matlab-mex-more-elegant-way-to-setup-env-sh/62/4) or `startup.m` to always have those environment variables defined when starting `Matlab`. +[`.matlab7rc.sh`](https://discourse.acados.org/t/matlab-mex-more-elegant-way-to-setup-env-sh/62/4) or `startup.m` to always have those environment variables defined when starting `MATLAB`. ### Linux / macOS Navigate into the folder of the example you want to run and execute the following command: @@ -31,12 +31,12 @@ source env.sh # Which can be found in the folder of one of the examples If you want to run an `acados` example from another folder, you need to export the environment variable `ACADOS_INSTALL_DIR` properly. In the `env.sh` file it is assumed that `ACADOS_INSTALL_DIR` is two folders above the directory, in which the example is located. -Afterwards, launch `Matlab` or `Octave` from the same shell. +Afterwards, launch `MATLAB` or `Octave` from the same shell. If you want to run the examples in a different folder, please close the current shell and open a new one to repeat the procedure: this ensures the correct setting of the environment variables. ### Windows -1. Open `Matlab` and navigate into [`/examples/acados_matlab_octave`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave). +1. Open `MATLAB` and navigate into [`/examples/acados_matlab_octave`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave). 2. Run [`acados_env_variables_windows`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/acados_env_variables_windows.m) to export the environment variable `ACADOS_INSTALL_DIR`. 3. Navigate into [`/examples/acados_matlab_octave/getting_started`](https://github.com/acados/acados/tree/main/examples/acados_matlab_octave/getting_started) and run one of the examples. @@ -50,14 +50,14 @@ In addition to a `MEX` wrapper it contains all the `C` code that is needed for e These templates can be found in [`/interfaces/acados_template/acados_template/c_templates_tera`](https://github.com/acados/acados/tree/main/interfaces/acados_template/acados_template/c_templates_tera). ## Options documentation -For the template based part of the `Matlab` interface, we refer to [the docstring based documentation of the Python interface](../python_interface/index.md). +For the template based part of the `MATLAB` interface, we refer to [the docstring based documentation of the Python interface](../python_interface/index.md). ## Simulink The templates mentioned [above](#templates) also contain templated S-functions and corresponding make functions for both the OCP solver and the acados integrator. A basic Simulink example can be found in [`/examples/acados_python/getting_started/simulink_example.m`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/simulink_example.m) -A more advanced Simulink example which showcases how to customize the inputs and outputs of the Simulink block corresponding to the solver can be found in [`/examples/acados_python/getting_started/simulink_example.m`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m) +A more advanced Simulink example which showcases how to customize the inputs and outputs of the Simulink block corresponding to the solver can be found in [`/examples/acados_python/getting_started/simulink_example_advanced.m`](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m) ### List of possible inputs This is a list of possible inputs to the Simulink block of an OCP solver which can be activated by setting the corresponding values in the acados Simulink options. @@ -156,12 +156,12 @@ To use the mask command just copy-paste it in the "icon drawing commands" field, ## Setup CasADi To create external function for your problem, we suggest to use `CasADi` from the folder `/external`. -Depending on the environment you want to use to generate `CasADi` functions from, proceed with the corresponding paragraph (Matlab, Octave). +Depending on the environment you want to use to generate `CasADi` functions from, proceed with the corresponding paragraph (MATLAB, Octave). Any CasADi version between 3.4.0 and 3.6.7 should work. If you don't have CasADi yet, you can install it as described below. -### **Matlab** +### **MATLAB** Download and extract the `CasADi` binaries into `/external/casadi-matlab`: ``` cd external diff --git a/docs/problem_formulation/problem_formulation_ocp_mex.tex b/docs/problem_formulation/problem_formulation_ocp_mex.tex index be32fc86db..05c8736a4f 100644 --- a/docs/problem_formulation/problem_formulation_ocp_mex.tex +++ b/docs/problem_formulation/problem_formulation_ocp_mex.tex @@ -43,7 +43,7 @@ \newcommand{\str}[1]{\texttt{'#1'}} \newcommand{\casadi}{\texttt{CasADi}} \newcommand{\acados}{\texttt{acados}} -\newcommand{\matlab}{\textsc{Matlab}} +\newcommand{\matlab}{\textsc{MATLAB}} \newcommand{\python}{\textsc{Python}} \newcommand{\tran}{^\top} \newcommand{\norm}[1]{\left\lVert#1\right\rVert} diff --git a/docs/troubleshooting/index.md b/docs/troubleshooting/index.md index a046195074..2490c4a81a 100644 --- a/docs/troubleshooting/index.md +++ b/docs/troubleshooting/index.md @@ -1,5 +1,5 @@ -# Trouble shooting +# Troubleshooting As a first step, check the [solver status](https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp_solver.AcadosOcpSolver.get_status) which is returned by `solve()` in Python and can be obtained with `solver.get_status()` and `solver.get('status')` in Python and MATLAB/Octave, respectively. @@ -13,16 +13,16 @@ Are all problem functions defined at the initial guess? `NaN` errors can often be mitigated by solver initializations. ### QP diagnostics -- [Python](examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m) ### Store iterates One can always get the last iterate using `solver.get()`, see -- [Python](examples/acados_python/linear_mass_model/linear_mass_test_problem.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/linear_mass_model/linear_mass_test_problem.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m) In addition, one can set the solver option [`store_iterates`](https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp_options.AcadosOcpOptions.store_iterates) to store all intermediate NLP solver iterates and get them after a solver call. -- [Python](examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m) diff --git a/examples/acados_matlab_octave/p_global_example/simulink_test_p_global.m b/examples/acados_matlab_octave/p_global_example/simulink_test_p_global.m index e27067190d..1ae6c07da1 100644 --- a/examples/acados_matlab_octave/p_global_example/simulink_test_p_global.m +++ b/examples/acados_matlab_octave/p_global_example/simulink_test_p_global.m @@ -67,7 +67,7 @@ % OCP solver ocp_solver = AcadosOcpSolver(ocp); -%% Matlab test solve +%% MATLAB test solve % test with ones such that update is necessary p_global_values_test = ones(size(p_global_values)); if use_p_global @@ -105,4 +105,4 @@ error('utraj values in SIMULINK and MATLAB should match.') end -disp('Simulink p_global test: got matching trajectories in Matlab and Simulink!') \ No newline at end of file +disp('Simulink p_global test: got matching trajectories in MATLAB and Simulink!') \ No newline at end of file diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m b/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m index 5981d571a0..91578f5d9f 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/zoro_example.m @@ -136,7 +136,7 @@ zoro_tol = 1e-6; % sample disturbances -% NOTE: this requires statistics addon in Matlab and octave -> just load +% NOTE: this requires statistics addon in MATLAB and octave -> just load % some data instead % dist_samples = mvnrnd(zeros(nx, 1), zoro_description.W_mat, Nsim); load('dist_samples.mat') diff --git a/examples/acados_matlab_octave/race_cars/README.md b/examples/acados_matlab_octave/race_cars/README.md index fe0719018c..68fb666654 100644 --- a/examples/acados_matlab_octave/race_cars/README.md +++ b/examples/acados_matlab_octave/race_cars/README.md @@ -1,4 +1,4 @@ -This folder contains the code ported from Python to Matlab by Thomas Jespersen. +This folder contains the code ported from Python to MATLAB by Thomas Jespersen. The original Python code has been used for the simulations and experiments associated with the publication: *NMPC for Racing Using a Singularity-Free Path-Parametric Model with Obstacle Avoidance - Daniel Kloeser, Tobias Schoels, Tommaso Sartor, Andrea Zanelli, Gianluca Frison, Moritz Diehl. Proceedings of the 21th IFAC World Congress, Berlin, Germany - July 2020*. A video of the experiments can be found on youtube: https://www.youtube.com/watch?v=1JDBQXVrZbo. diff --git a/examples/acados_matlab_octave/test/simulink_sparse_param_test.m b/examples/acados_matlab_octave/test/simulink_sparse_param_test.m index d992288969..3e9893b4ee 100644 --- a/examples/acados_matlab_octave/test/simulink_sparse_param_test.m +++ b/examples/acados_matlab_octave/test/simulink_sparse_param_test.m @@ -95,9 +95,9 @@ p_ref([13], 3+1) = input_update_port_p12_stage3(end); if any(any(p_ref ~= p_matlab)) - error('Setting sparse parameters in Matlab does NOT work as expected.'); + error('Setting sparse parameters in MATLAB does NOT work as expected.'); else - disp('Setting sparse parameters in Matlab works as expected.'); + disp('Setting sparse parameters in MATLAB works as expected.'); end %% simulink test diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_reuse_code.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_reuse_code.py index 1cd06d16b2..c882407cef 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_reuse_code.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_reuse_code.py @@ -148,7 +148,7 @@ # 1) now reuse the code but set a new time-steps vector, with a new number of elements dt1 = Tf_01 / N12 -new_time_steps1 = np.tile(dt1, (N12,)) # Matlab's equivalent to repmat +new_time_steps1 = np.tile(dt1, (N12,)) # MATLAB's equivalent to repmat time1 = np.hstack([0, np.cumsum(new_time_steps1)]) simX1 = np.zeros((N12 + 1, nx)) @@ -184,7 +184,7 @@ # 2) reuse the code again, set a new time-steps vector, only with a different final time dt2 = Tf_2 / N12 -new_time_steps2 = np.tile(dt2, (N12,)) # Matlab's equivalent to repmat +new_time_steps2 = np.tile(dt2, (N12,)) # MATLAB's equivalent to repmat time2 = np.hstack([0, np.cumsum(new_time_steps2)]) simX2 = np.zeros((N12 + 1, nx)) diff --git a/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py b/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py index a712c64470..12adf2c4aa 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py +++ b/examples/acados_python/pendulum_on_cart/ocp/nonuniform_discretization_example.py @@ -56,7 +56,7 @@ def main(discretization='shooting_nodes'): if integrator_type == 'GNSF': acados_dae_model_json_dump(model) - # structure detection in Matlab/Octave -> produces 'pendulum_ode_gnsf_functions.json' + # structure detection in MATLAB/Octave -> produces 'pendulum_ode_gnsf_functions.json' status = os.system('octave detect_gnsf_from_json.m') # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 36affd3fcc..a876326246 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -816,7 +816,7 @@ function make_consistent(self, is_mocp_phase) end end - % TODO: add checks for solution sensitivities when brining them to Matlab + % TODO: add checks for solution sensitivities when brining them to MATLAB % check if qp_solver_cond_N is set if isempty(opts.qp_solver_cond_N) diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 94cad1cacf..9550ce2815 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -32,7 +32,7 @@ classdef AcadosOcpSolver < handle properties (Access = public) - ocp % Matlab class AcadosOcp describing the OCP formulation + ocp % MATLAB class AcadosOcp describing the OCP formulation end % properties properties (Access = private) @@ -156,7 +156,7 @@ function solve(obj) % TODO: remove this! in v.0.5.0! function generate_c_code(obj, simulink_opts) - warning('acados_ocp will be deprecated in the future. Use AcadosOcpSolver instead. For more information on the major acados Matlab interface overhaul, see https://github.com/acados/acados/releases/tag/v0.4.0'); + warning('acados_ocp will be deprecated in the future. Use AcadosOcpSolver instead. For more information on the major acados MATLAB interface overhaul, see https://github.com/acados/acados/releases/tag/v0.4.0'); if nargin < 2 warning("Code is generated in the constructor of AcadosOcpSolver.") else diff --git a/interfaces/acados_matlab_octave/AcadosSimSolver.m b/interfaces/acados_matlab_octave/AcadosSimSolver.m index 88b42ce9b8..639f876653 100644 --- a/interfaces/acados_matlab_octave/AcadosSimSolver.m +++ b/interfaces/acados_matlab_octave/AcadosSimSolver.m @@ -33,7 +33,7 @@ properties t_sim % templated solver - sim % Matlab class AcadosSim describing the initial value problem + sim % MATLAB class AcadosSim describing the initial value problem end % properties diff --git a/interfaces/acados_matlab_octave/acados_install_windows.m b/interfaces/acados_matlab_octave/acados_install_windows.m index 0019e6fe8c..08d395a420 100644 --- a/interfaces/acados_matlab_octave/acados_install_windows.m +++ b/interfaces/acados_matlab_octave/acados_install_windows.m @@ -34,7 +34,7 @@ function acados_install_windows(varargin) % Read the mex C compiler configuration and extract the location cCompilerConfig = mex.getCompilerConfigurations('C'); pathToCompilerLocation = cCompilerConfig.Location; - % Modify the environment PATH variable for this Matlab session such that + % Modify the environment PATH variable for this MATLAB session such that % the mex C compiler takes priority ensuring calls to gcc uses the % configured mex compiler setenv('PATH', [fullfile(pathToCompilerLocation,'bin') ';' origEnvPath]); diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index 92fda8f6e7..ab432d0f9d 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -30,7 +30,7 @@ function solver = acados_ocp(model, opts, simulink_opts) - warning('acados_ocp will be deprecated in the future. Use AcadosOcpSolver instead. For more information on the major acados Matlab interface overhaul, see https://github.com/acados/acados/releases/tag/v0.4.0'); + warning('acados_ocp will be deprecated in the future. Use AcadosOcpSolver instead. For more information on the major acados MATLAB interface overhaul, see https://github.com/acados/acados/releases/tag/v0.4.0'); if nargin < 3 simulink_opts = get_acados_simulink_opts(); diff --git a/interfaces/acados_matlab_octave/acados_ocp_opts.m b/interfaces/acados_matlab_octave/acados_ocp_opts.m index bd289a0d57..93f3e242b6 100644 --- a/interfaces/acados_matlab_octave/acados_ocp_opts.m +++ b/interfaces/acados_matlab_octave/acados_ocp_opts.m @@ -124,7 +124,7 @@ function obj = set(obj, field, value) - % convert Matlab strings to char arrays + % convert MATLAB strings to char arrays if isstring(value) value = char(value); end diff --git a/interfaces/acados_matlab_octave/acados_sim_opts.m b/interfaces/acados_matlab_octave/acados_sim_opts.m index 5899f3ee67..9c8df3083d 100644 --- a/interfaces/acados_matlab_octave/acados_sim_opts.m +++ b/interfaces/acados_matlab_octave/acados_sim_opts.m @@ -72,7 +72,7 @@ function obj = set(obj, field, value) - % convert Matlab strings to char arrays + % convert MATLAB strings to char arrays if isstring(value) value = char(value); end diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m index c716dd6310..c23954bdfb 100644 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m +++ b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m @@ -43,7 +43,7 @@ function compile_ocp_shared_lib(export_dir) status, result); end else - % use CMake build system, has issues on Linux with Matlab, see https://github.com/acados/acados/issues/1209 + % use CMake build system, has issues on Linux with MATLAB, see https://github.com/acados/acados/issues/1209 [ status, result ] = system('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); if status cd(return_dir); diff --git a/interfaces/acados_matlab_octave/check_acados_requirements.m b/interfaces/acados_matlab_octave/check_acados_requirements.m index 9320aa536c..ddb877fa2f 100644 --- a/interfaces/acados_matlab_octave/check_acados_requirements.m +++ b/interfaces/acados_matlab_octave/check_acados_requirements.m @@ -23,7 +23,7 @@ function check_acados_requirements(varargin) if ~is_casadi_available % offer to install CasADi message = ['\nDear acados user, we could not import CasADi',... - ',\n which is needed to run the acados Matlab/Octave examples.',... + ',\n which is needed to run the acados MATLAB/Octave examples.',... '\n Press any key to proceed setting up the CasADi automatically.',... '\n Press "n" or "N" to exit, if you wish to set up CasADi yourself.\n']; if ~force diff --git a/interfaces/acados_matlab_octave/detect_gnsf_structure.m b/interfaces/acados_matlab_octave/detect_gnsf_structure.m index bdd2e6167b..76f2124138 100644 --- a/interfaces/acados_matlab_octave/detect_gnsf_structure.m +++ b/interfaces/acados_matlab_octave/detect_gnsf_structure.m @@ -44,7 +44,7 @@ function detect_gnsf_structure(model, dims, transcribe_opts) % functions, which were made part of the linear output system of the gnsf, % have changed signs. - % Options: transcribe_opts is a Matlab struct consisting of booleans: + % Options: transcribe_opts is a MATLAB struct consisting of booleans: % print_info: if extensive information on how the model is processed % is printed to the console. % check_E_invertibility: if the transcription method should check if the diff --git a/interfaces/acados_matlab_octave/ocp_compile_interface.m b/interfaces/acados_matlab_octave/ocp_compile_interface.m index 584dfcac10..137bb3f9fd 100644 --- a/interfaces/acados_matlab_octave/ocp_compile_interface.m +++ b/interfaces/acados_matlab_octave/ocp_compile_interface.m @@ -131,7 +131,7 @@ function ocp_compile_interface(output_dir) disp(['compiling ', mex_files{ii}]) if is_octave() linker_flags = ['-lacados', '-lhpipm', '-lblasfeo', acados_lib_extra]; - % NOTE: multiple linker flags in 1 argument do not work in Matlab + % NOTE: multiple linker flags in 1 argument do not work in MATLAB mex(acados_include, acados_interfaces_include, external_include, blasfeo_include, hpipm_include,... acados_lib_path, linker_flags{:}, mex_files{ii}) else diff --git a/interfaces/acados_matlab_octave/set_up_t_renderer.m b/interfaces/acados_matlab_octave/set_up_t_renderer.m index 8b30b1c12f..66d8531675 100644 --- a/interfaces/acados_matlab_octave/set_up_t_renderer.m +++ b/interfaces/acados_matlab_octave/set_up_t_renderer.m @@ -15,7 +15,7 @@ function set_up_t_renderer(t_renderer_location, varargin) message = ['\nDear acados user, we could not find t_renderer binaries,',... '\n which are needed to export templated C code from ',... - 'Matlab.\n Press any key to proceed setting up the t_renderer automatically.',... + 'MATLAB.\n Press any key to proceed setting up the t_renderer automatically.',... '\n Press "n" or "N" to exit, if you wish to set up t_renderer yourself.\n',... '\n https://github.com/acados/tera_renderer/releases']; diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 6d732dc845..1d5e54310a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1436,7 +1436,7 @@ def translate_cost_to_external_cost(self, :param p_global_values: numpy array with the same shape as p_global providing initial global parameter values. :param W_0, W, W_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, ny_0), (ny, ny), (ny_e, ny_e). :param yref_0, yref, yref_e: Optional CasADi expressions which should be used instead of the numerical values provided by the cost module, shapes should be (ny_0, 1), (ny, 1), (ny_e, 1). - cost_hessian: 'EXACT' or 'GAUSS_NEWTON', determines how the cost hessian is computed. + :param cost_hessian: 'EXACT' or 'GAUSS_NEWTON', determines how the cost hessian is computed. """ if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 8855b42355..e6a9981807 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -319,7 +319,11 @@ def collocation_type(self): """Collocation type: only relevant for implicit integrators -- string in {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}. - Default: GAUSS_LEGENDRE + Default: GAUSS_LEGENDRE. + + .. note:: GAUSS_LEGENDRE tableaus yield integration methods that are A-stable, but not L-stable and have order `2 * num_stages`, + .. note:: GAUSS_RADAU_IIA tableaus yield integration methods that are L-stable and have order `2 * num_stages - 1`. + .. note:: EXPLICIT_RUNGE_KUTTA tableaus can be used for comparisons of ERK and IRK to ensure correctness, but are only recommended with ERK for users. """ return self.__collocation_type diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 03cfc6e1aa..ce8f33df11 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -62,8 +62,8 @@ class AcadosOcpSolver: """ Class to interact with the acados ocp solver C object. - :param acados_ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` or :py:class:`~acados_template.acados_multiphase_ocp.AcadosMultiphaseOcp` - description of the OCP for acados - :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json + :param acados_ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` or :py:class:`~acados_template.acados_multiphase_ocp.AcadosMultiphaseOcp` - description of the OCP for acados + :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json """ if os.name == 'nt': dlclose = DllLoader('kernel32', use_last_error=True).FreeLibrary @@ -93,14 +93,15 @@ def shared_lib(self,): def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None, verbose=True): """ Generates the code for an acados OCP solver, given the description in acados_ocp. - :param acados_ocp: type Union[AcadosOcp, AcadosMultiphaseOcp] - description of the OCP for acados - :param json_file: name for the json file used to render the templated code - default: `acados_ocp_nlp.json` - :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible inputs and - outputs; default: `None` - :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use - the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with - `MS Visual Studio`); default: `None` - :param verbose: indicating if warnings are printed + + :param acados_ocp: type Union[AcadosOcp, AcadosMultiphaseOcp] - description of the OCP for acados + :param json_file: name for the json file used to render the templated code - default: `acados_ocp_nlp.json` + :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible inputs and + outputs; default: `None` + :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` + :param verbose: indicating if warnings are printed """ acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) @@ -142,13 +143,14 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: @classmethod def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): """ - Builds the code for an acados OCP solver, that has been generated in code_export_dir - :param code_export_dir: directory in which acados OCP solver has been generated, see generate() - :param with_cython: option indicating if the cython interface is build, default: False. - :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use + Builds the code for an acados OCP solver, that has been generated in code_export_dir. + + :param code_export_dir: directory in which acados OCP solver has been generated, see generate() + :param with_cython: option indicating if the cython interface is build, default: False. + :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with `MS Visual Studio`); default: `None` - :param verbose: indicating if build command is printed + :param verbose: indicating if build command is printed """ code_export_dir = os.path.abspath(code_export_dir) @@ -543,11 +545,11 @@ def set_new_time_steps(self, new_time_steps): Set new time steps. Recreates the solver if N changes. - :param new_time_steps: 1 dimensional np array of new time steps for the solver + :param new_time_steps: 1 dimensional np array of new time steps for the solver - .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of - the shooting nodes without changing the number, e.g., to reach a different final time. Both cases - do not require a new code export and compilation. + .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of + the shooting nodes without changing the number, e.g., to reach a different final time. Both cases + do not require a new code export and compilation. """ if self.__problem_class == "MOCP": raise ValueError('This function can only be used for single phase OCPs!') @@ -592,13 +594,13 @@ def update_qp_solver_cond_N(self, qp_solver_cond_N: int): This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or the influence of a different `qp_solver_cond_N` is studied without code export and compilation. - :param qp_solver_cond_N: new number of condensing stages for the solver + :param qp_solver_cond_N: new number of condensing stages for the solver - .. note:: This function can only be used in combination with a partial condensing QP solver. + .. note:: This function can only be used in combination with a partial condensing QP solver. - .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be - necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically - `qp_solver_cond_N < N`. + .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be + necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically + `qp_solver_cond_N < N`. """ if self.__problem_class == "MOCP": raise ValueError('This function can only be used for single phase OCPs!') @@ -626,9 +628,10 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st """ Returns the gradient of the optimal value function w.r.t. what is specified in `with_respect_to`. - Disclaimer: This function only returns reasonable values if the solver has converged for the current problem instance. + .. note:: This function only returns reasonable values if the solver has converged for the current problem instance. Notes: + - for field `initial_state`, the gradient is the Lagrange multiplier of the initial state constraint. The gradient computation consists of adding the Lagrange multipliers corresponding to the upper and lower bound of the initial state. @@ -718,32 +721,28 @@ def eval_solution_sensitivity(self, """ Evaluate the sensitivity of the current solution x_i, u_i with respect to the initial state or the parameters for all stages i in `stages`. - :param stages: stages for which the sensitivities are returned, int or list of int - :param with_respect_to: string in ["initial_state", "p_global"] - :param return_sens_x: Flag indicating whether sensitivities of x should be returned. Default: True. - :param return_sens_u: Flag indicating whether sensitivities of u should be returned. Default: True. - :param return_sens_pi: Flag indicating whether sensitivities of pi should be returned. Default: False. - :param return_sens_lam: Flag indicating whether sensitivities of lam should be returned. Default: False. - :param return_sens_su: Flag indicating whether sensitivities of su should be returned. Default: False. - :param return_sens_sl: Flag indicating whether sensitivities of sl should be returned. Default: False. - :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True - - :returns: A dictionary with the solution sensitivities with fields sens_x, sens_u, sens_pi, sens_lam, sens_su, sens_sl if corresponding flags were set. - If stages is a list, sens_x, sens_lam, sens_su, sens_sl is a list of the same length. - For sens_u, sens_pi, the list has length len(stages) or len(stages)-1 depending on whether N is included or not. - If stages is a scalar, the returned sensitivities are np.ndarrays of shape (nfield[stages], ngrad). - - .. note:: Correct computation of sensitivities requires \n - - (1) HPIPM as QP solver, \n - - (2) the usage of an exact Hessian, \n - - (3) positive definiteness of the full-space Hessian if the square-root version of the Riccati recursion is used - OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), \n - - (4) the last interaction before calling this function should involve the solution of the QP at the NLP solution. - This can happen as call to `solve()` with at least 1 QP being solved or `setup_qp_matrices_and_factorize()`, \n + :param stages: stages for which the sensitivities are returned, int or list of int + :param with_respect_to: string in ["initial_state", "p_global"] + :param return_sens_x: Flag indicating whether sensitivities of x should be returned. Default: True. + :param return_sens_u: Flag indicating whether sensitivities of u should be returned. Default: True. + :param return_sens_pi: Flag indicating whether sensitivities of pi should be returned. Default: False. + :param return_sens_lam: Flag indicating whether sensitivities of lam should be returned. Default: False. + :param return_sens_su: Flag indicating whether sensitivities of su should be returned. Default: False. + :param return_sens_sl: Flag indicating whether sensitivities of sl should be returned. Default: False. + :param sanity_checks: bool - whether to perform sanity checks, turn off for minimal overhead, default: True + + :return: A dictionary with the solution sensitivities with fields sens_x, sens_u, sens_pi, sens_lam, sens_su, sens_sl if corresponding flags were set. + If stages is a list, sens_x, sens_lam, sens_su, sens_sl is a list of the same length. + For sens_u, sens_pi, the list has length len(stages) or len(stages)-1 depending on whether N is included or not. + If stages is a scalar, the returned sensitivities are np.ndarrays of shape (nfield[stages], ngrad). + + .. note:: Correct computation of sensitivities requires: \n + (1) HPIPM as QP solver, \n + (2) the usage of an exact Hessian, \n + (3) positive definiteness of the full-space Hessian if the square-root version of the Riccati recursion is used + OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), \n + (4) the last interaction before calling this function should involve the solution of the QP at the NLP solution. + This can happen as call to `solve()` with at least 1 QP being solved or `setup_qp_matrices_and_factorize()`, \n .. note:: Timing of the sensitivities computation consists of time_solution_sens_lin, time_solution_sens_solve. .. note:: Solution sensitivities with respect to parameters are currently implemented assuming the parameter vector p is global within the OCP, i.e. p=p_i with i=0, ..., N. @@ -876,14 +875,15 @@ def eval_adjoint_solution_sensitivity(self, ) -> np.ndarray: """ Evaluate the adjoint sensitivity of the solution with respect to the parameters. - :param seed_x : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (nx, n_seeds) - :param seed_u : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (nu, n_seeds). - :param with_respect_to : string in ["p_global"] - :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True - The correct computation of solution of adjoint sensitivities has the same requirements as the computation of solution sensitivities, see the documentation of `eval_solution_sensitivity`. + :param seed_x: Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (nx, n_seeds) + :param seed_u: Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (nu, n_seeds). + :param with_respect_to: string in ["p_global"] + :param sanity_checks: bool - whether to perform sanity checks, turn off for minimal overhead, default: True + + The correct computation of solution of adjoint sensitivities has the same requirements as the computation of solution sensitivities, see the documentation of `eval_solution_sensitivity`. """ # get n_seeds @@ -976,7 +976,7 @@ def eval_param_sens(self, index: int, stage: int=0, field="ex"): OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), (4) the solution of at least one QP in advance to evaluation of the sensitivities as the factorization is reused. - :param index: integer corresponding to initial state index in range(nx) + :param index: integer corresponding to initial state index in range(nx) """ print("WARNING: eval_param_sens() is deprecated. Please use eval_solution_sensitivity() instead!") @@ -1010,17 +1010,17 @@ def eval_param_sens(self, index: int, stage: int=0, field="ex"): def get(self, stage_: int, field_: str): """ - Get the last solution of the solver: + Get the last solution of the solver. - :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'sens_u', 'sens_pi', 'sens_x', 'sens_lam', 'sens_sl', 'sens_su'] + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'sens_u', 'sens_pi', 'sens_x', 'sens_lam', 'sens_sl', 'sens_su'] - .. note:: regarding lam: \n - the inequalities are internally organized in the following order: \n - [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n - lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + .. note:: regarding lam: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] - .. note:: pi: multipliers for dynamics equality constraints \n + .. note:: pi: multipliers for dynamics equality constraints \n lam: multipliers for inequalities \n t: slack variables corresponding to evaluation of all inequalities (at the solution) \n sl: slack variables of soft lower inequality constraints \n @@ -1066,10 +1066,10 @@ def get_flat(self, field_: str) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global'] + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global'] - .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n - In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n + .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n + In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n """ if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global']: raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') @@ -1095,7 +1095,7 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: """ Set concatenation solver initialization . - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] """ field = field_.encode('utf-8') if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: @@ -1233,8 +1233,8 @@ def store_iterate(self, filename: str = '', overwrite: bool = False, verbose: bo Stores the current iterate of the OCP solver in a json file. Note: This does not contain the iterate of the integrators, and the parameters. - :param filename: if not set, use f'{self.name}_iterate.json' - :param overwrite: if false and filename exists add timestamp to filename + :param filename: if not set, use f'{self.name}_iterate.json' + :param overwrite: if false and filename exists add timestamp to filename """ if filename == '': filename = f'{self.name}_iterate.json' @@ -1363,8 +1363,8 @@ def dump_last_qp_to_json(self, filename: str = '', overwrite=False): """ Dumps the latest QP data into a json file - :param filename: if not set, use name + timestamp + '.json' - :param overwrite: if false and filename exists add timestamp to filename + :param filename: if not set, use name + timestamp + '.json' + :param overwrite: if false and filename exists add timestamp to filename """ if filename == '': filename = f'{self.name}_QP.json' @@ -1527,9 +1527,9 @@ 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', 'nlp_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: + Available fields: - time_tot: total CPU time previous call - time_lin: CPU time for linearization - time_sim: CPU time for integrator @@ -1771,15 +1771,15 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): """ Set numerical data inside the solver. - :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'pi', 'lam', 'p', 'xdot_guess', 'z_guess', 'sens_x', 'sens_u'] + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam', 'p', 'xdot_guess', 'z_guess', 'sens_x', 'sens_u'] - .. note:: regarding lam: \n + .. note:: regarding lam: \n the inequalities are internally organized in the following order: \n [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] - .. note:: pi: multipliers for dynamics equality constraints \n + .. note:: pi: multipliers for dynamics equality constraints \n lam: multipliers for inequalities \n t: slack variables corresponding to evaluation of all inequalities (at the solution) \n sl: slack variables of soft lower inequality constraints \n @@ -1854,8 +1854,8 @@ def cost_get(self, stage_: int, field_: str) -> np.ndarray: """ Get numerical data in the cost module of the solver. - :param stage: integer corresponding to shooting node - :param field: string in ['yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', 'scaling'] + :param stage: integer corresponding to shooting node + :param field: string in ['yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', 'scaling'] """ if not isinstance(stage_, int): @@ -1891,9 +1891,9 @@ def cost_set(self, stage_: int, field_: str, value_, api='warn'): """ Set numerical data in the cost module of the solver. - :param stage: integer corresponding to shooting node - :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', 'scaling' - :param value: of appropriate size + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu', 'scaling' + :param value: of appropriate size Note: by default the cost is scaled with the time step, and the terminal cost term scaled with 1. This can be overwritten by setting the 'scaling' field. @@ -1961,8 +1961,8 @@ def constraints_get(self, stage_: int, field_: str) -> np.ndarray: """ Get numerical data in the constraint module of the solver. - :param stage: integer corresponding to shooting node - :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] """ if not isinstance(stage_, int): @@ -1998,9 +1998,9 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war """ Set numerical data in the constraint module of the solver. - :param stage: integer corresponding to shooting node - :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] - :param value: of appropriate size + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D'] + :param value: of appropriate size """ # cast value_ to avoid conversion issues if isinstance(value_, (float, int)): @@ -2079,8 +2079,8 @@ def get_from_qp_in(self, stage_: int, field_: str): """ Get numerical data from the current QP. - :param stage: integer corresponding to shooting node - :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] + :param stage: integer corresponding to shooting node + :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] Note: - additional supported fields are ['P', 'K', 'Lr'], which can be extracted form QP solver PARTIAL_CONDENSING_HPIPM. @@ -2209,7 +2209,7 @@ def options_set(self, field_, value_): """ Set options of the solver. - :param field: string, possible values are: + :param field: string, possible values are: 'print_level', 'rti_phase', 'nlp_solver_max_iter, 'as_rti_level', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', @@ -2223,7 +2223,7 @@ def options_set(self, field_, value_): 'adaptive_levenberg_marquardt_lam', 'adaptive_levenberg_marquardt_mu_min', 'adaptive_levenberg_marquardt_mu0', 'tau_min' - :param value: of type int, float, string, bool + :param value: of type int, float, string, bool - qp_tol_stat: QP solver tolerance stationarity - qp_tol_eq: QP solver tolerance equalities @@ -2332,9 +2332,9 @@ def set_params_sparse(self, stage_: int, idx_values_: np.ndarray, param_values_) Pseudo: solver.param[idx_values] = param_values; Parameters: - :param stage: integer corresponding to shooting node - :param idx_values: 0 based np array (or iterable) of integers: indices of parameter to be set - :param param_values: new parameter values as numpy array + :param stage: integer corresponding to shooting node + :param idx_values: 0 based np array (or iterable) of integers: indices of parameter to be set + :param param_values: new parameter values as numpy array """ if not isinstance(stage_, int): diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 8ef0255073..3807cf04b4 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -140,7 +140,11 @@ def collocation_type(self): """Collocation type: relevant for implicit integrators -- string in {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}. - Default: GAUSS_LEGENDRE + Default: GAUSS_LEGENDRE. + + .. note:: GAUSS_LEGENDRE tableaus yield integration methods that are A-stable, but not L-stable and have order `2 * num_stages`, + .. note:: GAUSS_RADAU_IIA tableaus yield integration methods that are L-stable and have order `2 * num_stages - 1`. + .. note:: EXPLICIT_RUNGE_KUTTA tableaus can be used for comparisons of ERK and IRK to ensure correctness, but are only recommended with ERK for users. """ return self.__collocation_type diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index 8e7e021d59..75bf1da4d8 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -58,12 +58,12 @@ class AcadosSimSolver: """ Class to interact with the acados integrator C object. - :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim` - :param json_file: Default: 'acados_sim.json' - :param build: Default: True - :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use - the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with - `MS Visual Studio`); default: `None` + :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim` + :param json_file: Default: 'acados_sim.json' + :param build: Default: True + :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use + the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with + `MS Visual Studio`); default: `None` """ if sys.platform=="win32": dlclose = DllLoader('kernel32', use_last_error=True).FreeLibrary @@ -277,7 +277,7 @@ def get(self, field_): """ Get the last solution of the solver. - :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] + :param field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] """ field = field_.encode('utf-8') @@ -324,8 +324,8 @@ def set(self, field_: str, value_): """ Set numerical data inside the solver. - :param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T', 't0'] - :param value: the value with appropriate size. + :param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T', 't0'] + :param value: the value with appropriate size. """ settable = ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T', 't0'] # S_forw @@ -383,8 +383,8 @@ def options_set(self, field_: str, value_: bool): """ Set solver options - :param field: string in ['sens_forw', 'sens_adj', 'sens_hess'] - :param value: Boolean + :param field: string in ['sens_forw', 'sens_adj', 'sens_hess'] + :param value: Boolean """ fields = ['sens_forw', 'sens_adj', 'sens_hess'] if field_ not in fields: diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 7617c5350e..950eb9cae0 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -97,7 +97,7 @@ option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_ if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows") # MinGW: remove prefix and change suffix to match MSVC - # (such that Matlab mex recognizes the libraries) + # (such that MATLAB mex recognizes the libraries) set(CMAKE_SHARED_LIBRARY_PREFIX "") set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib") set(CMAKE_IMPORT_LIBRARY_PREFIX "") diff --git a/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt index 9250ebaffc..40472cde8d 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt @@ -82,7 +82,7 @@ option(BUILD_EXAMPLE "Should the example main_{{ name }} be build?" OFF) if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows") # MinGW: remove prefix and change suffix to match MSVC - # (such that Matlab mex recognizes the libraries) + # (such that MATLAB mex recognizes the libraries) set(CMAKE_SHARED_LIBRARY_PREFIX "") set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib") set(CMAKE_IMPORT_LIBRARY_PREFIX "") diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 940fd7b169..1f926a8fb8 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -386,7 +386,7 @@ def generate_c_code_gnsf(context: GenerateContext, model: AcadosModel, model_dir # set up expressions # if the model uses ca.MX because of cost/constraints - # the DAE can be exported as ca.SX -> detect GNSF in Matlab + # the DAE can be exported as ca.SX -> detect GNSF in MATLAB # -> evaluated ca.SX GNSF functions with ca.MX. u = model.u symbol = model.get_casadi_symbol() diff --git a/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py b/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py index c5e3cff724..3fa6d2f842 100644 --- a/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py +++ b/interfaces/acados_template/acados_template/gnsf/detect_gnsf_structure.py @@ -56,7 +56,7 @@ def detect_gnsf_structure(acados_ocp, transcribe_opts=None): # functions, which were made part of the linear output system of the gnsf, # have changed signs. - # Options: transcribe_opts is a Matlab struct consisting of booleans: + # Options: transcribe_opts is a MATLAB struct consisting of booleans: # print_info: if extensive information on how the model is processed # is printed to the console. # generate_gnsf_model: if the neccessary C functions to simulate the gnsf From f76c5fd80b04a706acd4c6f0025655299a915610 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Sun, 11 May 2025 12:47:50 +0200 Subject: [PATCH 047/164] Fix quadrotor example issue #1511 (#1524) --- examples/acados_python/quadrotor_nav/visualize_mpl.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/acados_python/quadrotor_nav/visualize_mpl.py b/examples/acados_python/quadrotor_nav/visualize_mpl.py index 45c2b6268e..67c2a857df 100644 --- a/examples/acados_python/quadrotor_nav/visualize_mpl.py +++ b/examples/acados_python/quadrotor_nav/visualize_mpl.py @@ -117,7 +117,9 @@ def animate(iter): # update simulation time time_text.set_text(f'time = {misc_steps[0, iter]:.2f} s' ) - drone[0]._offsets3d = (float(zetaC_hat[0, 0, iter]), float(zetaC_hat[1, 0, iter]), zetaC_hat[2, 0, iter:iter+1]) + drone[0]._offsets3d = ([float(zetaC_hat[0, 0, iter])], + [float(zetaC_hat[1, 0, iter])], + [float(zetaC_hat[2, 0, iter])]) horizon.set_data(zetaC_hat[0: 2, 1:, iter]) horizon.set_3d_properties(zetaC_hat[2, 1:, iter]) From ca2ff504788cbfbe4bca2580be21e2f0a08f22df Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 14 May 2025 23:29:40 +0200 Subject: [PATCH 048/164] Python: Add `AcadosCasadiOcpSolver` & minor fixes (#1523) The class `AcadosCasadiOcpSolver` was added, for now is not full featured, e.g. slacks are missing. The class aims to provide a very similar API as `AcadosOcpSolver` and should allow for sanity checks, debugging, benchmarking and more. Additional changes: - added `create_default_initial_iterate` to create an `AcadosOcpIterate` without creating a solver - fixed some issues in the evaluator class - fixed issue in C interface which made the algorithm fail when loading an iterate with empty z and ERK Follow-up tasks tracked in https://github.com/acados/acados/issues/1525 --- .github/workflows/full_build.yml | 2 + .../ocp/test_casadi_formulation.py | 63 ++++ interfaces/acados_c/ocp_nlp_interface.c | 3 +- .../acados_template/__init__.py | 1 + .../acados_casadi_ocp_solver.py | 307 ++++++++++++++++++ .../acados_template/acados_ocp.py | 143 +++++++- .../acados_template/mpc_utils.py | 65 +--- .../acados_template/acados_template/utils.py | 1 + 8 files changed, 511 insertions(+), 74 deletions(-) create mode 100644 examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py create mode 100644 interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 9c5564fb69..d6b106d297 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -191,6 +191,8 @@ jobs: source ${{runner.workspace}}/acados/acadosenv/bin/activate cd ${{runner.workspace}}/acados/examples/acados_python/tests python test_rti_sqp_residuals.py + cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + python test_casadi_formulation.py - name: Python sensitivity examples working-directory: ${{runner.workspace}}/acados/build diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py new file mode 100644 index 0000000000..344b539ab1 --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -0,0 +1,63 @@ + +import sys +sys.path.insert(0, '../common') + +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from ocp_example_cost_formulations import formulate_ocp, T_HORIZON + +from utils import plot_pendulum + +def get_x_u_traj(ocp_solver: Union[AcadosOcpSolver, AcadosCasadiOcpSolver], N_horizon: int): + ocp = ocp_solver.acados_ocp + simX = np.zeros((N_horizon+1, ocp.dims.nx)) + simU = np.zeros((N_horizon, ocp.dims.nu)) + for i in range(N_horizon): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") + + return simX, simU + + +def main(): + ocp = formulate_ocp("CONL") + ocp.solver_options.tf = T_HORIZON + N_horizon = ocp.solver_options.N_horizon + + ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) + casadi_ocp_solver.solve() + x_casadi_sol, u_casadi_sol = get_x_u_traj(casadi_ocp_solver, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + # initialize solver + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + # get solution + simX, simU = get_x_u_traj(ocp_solver, N_horizon) + + + # evaluate difference + diff_x = np.linalg.norm(x_casadi_sol - simX) + print(f"Difference between casadi and acados solution: {diff_x}") + diff_u = np.linalg.norm(u_casadi_sol - simU) + print(f"Difference between casadi and acados solution: {diff_u}") + + test_tol = 1e-5 + if diff_x > test_tol or diff_u > test_tol: + raise ValueError(f"Test failed: difference between casadi and acados solution should be smaller than {test_tol}, but got {diff_x} and {diff_u}.") + + plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu, u_casadi_sol, x_casadi_sol, latexify=False) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 7df455ebb0..8024ba5f17 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1880,7 +1880,8 @@ void ocp_nlp_set(ocp_nlp_solver *solver, int stage, const char *field, void *val int nx = dims->nx[stage]; double *double_values = value; blasfeo_pack_dvec(nz, double_values, 1, mem->sim_guess + stage, nx); - mem->set_sim_guess[stage] = true; + if (nz > 0) + mem->set_sim_guess[stage] = true; // printf("set z_guess\n"); // blasfeo_print_exp_dvec(nz, mem->sim_guess+stage, nx); } diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index da828bfe63..6d2dcdd2ae 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -43,6 +43,7 @@ from .acados_multiphase_ocp import AcadosMultiphaseOcp from .acados_ocp_solver import AcadosOcpSolver +from .acados_casadi_ocp_solver import AcadosCasadiOcpSolver from .acados_sim_solver import AcadosSimSolver from .acados_sim_batch_solver import AcadosSimBatchSolver from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \ diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py new file mode 100644 index 0000000000..6fa8d38f1e --- /dev/null +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -0,0 +1,307 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +import casadi as ca + +from typing import Union, Tuple + +import numpy as np +from .acados_ocp import AcadosOcp +from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates, AcadosOcpFlattenedIterate + + +class AcadosCasadiOcpSolver: + + @classmethod + def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: + """ + Creates an equivalent CasADi NLP formulation of the OCP. + Experimental, not fully implemented yet. + + :return: nlp_dict, bounds_dict + """ + ocp.make_consistent() + + # unpack + model = ocp.model + dims = ocp.dims + constraints = ocp.constraints + solver_options = ocp.solver_options + + if any([dims.ns_0, dims.ns, dims.ns_e]): + raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") + + # create variables + ca_symbol = model.get_casadi_symbol() + xtraj = ca_symbol('x', dims.nx, solver_options.N_horizon+1) + utraj = ca_symbol('u', dims.nu, solver_options.N_horizon) + if dims.nz > 0: + raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") + # parameters + ptraj = ca_symbol('p', dims.np, solver_options.N_horizon+1) + + ### Constraints: bounds + # setup state bounds + lb_xtraj = -np.inf * ca.DM.ones((dims.nx, solver_options.N_horizon+1)) + ub_xtraj = np.inf * ca.DM.ones((dims.nx, solver_options.N_horizon+1)) + lb_xtraj[constraints.idxbx_0, 0] = constraints.lbx_0 + ub_xtraj[constraints.idxbx_0, 0] = constraints.ubx_0 + for i in range(1, solver_options.N_horizon): + lb_xtraj[constraints.idxbx, i] = constraints.lbx + ub_xtraj[constraints.idxbx, i] = constraints.ubx + lb_xtraj[constraints.idxbx_e, -1] = constraints.lbx_e + ub_xtraj[constraints.idxbx_e, -1] = constraints.ubx_e + # setup control bounds + lb_utraj = -np.inf * ca.DM.ones((dims.nu, solver_options.N_horizon)) + ub_utraj = np.inf * ca.DM.ones((dims.nu, solver_options.N_horizon)) + for i in range(solver_options.N_horizon): + lb_utraj[constraints.idxbu, i] = constraints.lbu + ub_utraj[constraints.idxbu, i] = constraints.ubu + + ### Nonlinear constraints + g = [] + lbg = [] + ubg = [] + # dynamics + if solver_options.integrator_type == "DISCRETE": + f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) + elif solver_options.integrator_type == "ERK": + ca_expl_ode = ca.Function('ca_expl_ode', [model.x, model.u], [model.f_expl_expr]) + # , model.p, model.p_global] + f_discr_fun = ca.simpleRK(ca_expl_ode, solver_options.sim_method_num_steps[0], solver_options.sim_method_num_stages[0]) + else: + raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") + + for i in range(solver_options.N_horizon): + # add dynamics constraints + if solver_options.integrator_type == "DISCRETE": + g.append(xtraj[:, i+1] - f_discr_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + elif solver_options.integrator_type == "ERK": + g.append(xtraj[:, i+1] - f_discr_fun(xtraj[:, i], utraj[:, i], solver_options.time_steps[i])) + lbg.append(np.zeros((dims.nx, 1))) + ubg.append(np.zeros((dims.nx, 1))) + + # nonlinear constraints -- initial stage + h0_fun = ca.Function('h0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) + g.append(h0_fun(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global)) + lbg.append(constraints.lh_0) + ubg.append(constraints.uh_0) + + if dims.nphi_0 > 0: + conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) + conl_constr_0_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr_0]) + g.append(conl_constr_0_fun(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global)) + lbg.append(constraints.lphi_0) + ubg.append(constraints.uphi_0) + + # nonlinear constraints -- intermediate stages + h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) + + if dims.nphi > 0: + conl_constr_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) + conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr]) + + for i in range(1, solver_options.N_horizon): + g.append(h_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + lbg.append(constraints.lh) + ubg.append(constraints.uh) + + if dims.nphi > 0: + g.append(conl_constr_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + lbg.append(constraints.lphi) + ubg.append(constraints.uphi) + + # nonlinear constraints -- terminal stage + h_e_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) + + g.append(h_e_fun(xtraj[:, -1], ptraj[:, -1], model.p_global)) + lbg.append(constraints.lh_e) + ubg.append(constraints.uh_e) + + if dims.nphi_e > 0: + conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) + conl_constr_e_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_constr_expr_e]) + g.append(conl_constr_e_fun(xtraj[:, -1], ptraj[:, -1], model.p_global)) + lbg.append(constraints.lphi_e) + ubg.append(constraints.uphi_e) + + ### Cost + # initial cost term + nlp_cost = 0 + cost_expr_0 = ocp.get_initial_cost_expression() + cost_fun_0 = ca.Function('cost_fun_0', [model.x, model.u, model.p, model.p_global], [cost_expr_0]) + nlp_cost += solver_options.cost_scaling[0] * cost_fun_0(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global) + + # intermediate cost term + cost_expr = ocp.get_path_cost_expression() + cost_fun = ca.Function('cost_fun', [model.x, model.u, model.p, model.p_global], [cost_expr]) + for i in range(1, solver_options.N_horizon): + nlp_cost += solver_options.cost_scaling[i] * cost_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global) + + # terminal cost term + cost_expr_e = ocp.get_terminal_cost_expression() + cost_fun_e = ca.Function('cost_fun_e', [model.x, model.p, model.p_global], [cost_expr_e]) + nlp_cost += solver_options.cost_scaling[-1] * cost_fun_e(xtraj[:, -1], ptraj[:, -1], model.p_global) + + # call w all primal variables + w = ca.vertcat(ca.vec(xtraj), ca.vec(utraj)) + lbw = ca.vertcat(ca.vec(lb_xtraj), ca.vec(lb_utraj)) + ubw = ca.vertcat(ca.vec(ub_xtraj), ca.vec(ub_utraj)) + p_nlp = ca.vertcat(ca.vec(ptraj), model.p_global) + + # create NLP + nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} + bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} + + return nlp, bounds + + + def __init__(self, acados_ocp: AcadosOcp, verbose=True): + + if not isinstance(acados_ocp, AcadosOcp): + raise TypeError('acados_ocp should be of type AcadosOcp.') + + self.acados_ocp = acados_ocp + self.casadi_nlp, self.bounds = self.create_casadi_nlp_formulation(acados_ocp) + self.casadi_solver = ca.nlpsol("nlp_solver", 'ipopt', self.casadi_nlp) + self.nlp_sol = None + + + def solve_for_x0(self, x0_bar, fail_on_nonzero_status=True, print_stats_on_failure=True): + raise NotImplementedError() + + + def solve(self) -> int: + """ + Solve the ocp with current input. + + :return: status of the solver + """ + self.nlp_sol = self.casadi_solver(lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], lbg=self.bounds['lbg'], ubg=self.bounds['ubg']) + + # TODO: return correct status + return 0 + + def get_dim_flat(self, field: str): + """ + Get dimension of flattened iterate. + """ + if field not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: + raise ValueError(f'AcadosOcpSolver.get_dim_flat(field={field}): \'{field}\' is an invalid argument.') + + raise NotImplementedError() + + + + def get(self, stage: int, field: str): + """ + Get the last solution of the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'sens_u', 'sens_pi', 'sens_x', 'sens_lam', 'sens_sl', 'sens_su'] + + .. note:: regarding lam: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + if not isinstance(stage, int): + raise TypeError('stage should be integer.') + if self.nlp_sol is None: + raise ValueError('No solution available. Please call solve() first.') + dims = self.acados_ocp.dims + + if field == 'x': + sol_w = self.nlp_sol['x'] + return sol_w[stage*dims.nx:(stage+1)*dims.nx].full().flatten() + elif field == 'u': + sol_w = self.nlp_sol['x'] + offset_x = dims.nx*(self.acados_ocp.solver_options.N_horizon+1) + return sol_w[offset_x+stage*dims.nu: offset_x+(stage+1)*dims.nu].full().flatten() + else: + raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") + + def get_flat(self, field_: str) -> np.ndarray: + """ + Get concatenation of all stages of last solution of the solver. + + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global'] + + .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n + In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n + """ + raise NotImplementedError() + + + def set_flat(self, field_: str, value_: np.ndarray) -> None: + """ + Set concatenation solver initialization. + + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] + """ + raise NotImplementedError() + + + def load_iterate(self, filename:str, verbose: bool = True): + raise NotImplementedError() + + def store_iterate_to_obj(self) -> AcadosOcpIterate: + raise NotImplementedError() + + def load_iterate_from_obj(self, iterate: AcadosOcpIterate): + raise NotImplementedError() + + def store_iterate_to_flat_obj(self) -> AcadosOcpFlattenedIterate: + raise NotImplementedError() + + def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedIterate) -> None: + raise NotImplementedError() + + def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: + raise NotImplementedError() + + def get_cost(self) -> float: + raise NotImplementedError() + + def set(self, stage_: int, field_: str, value_: np.ndarray): + raise NotImplementedError() + + def cost_get(self, stage_: int, field_: str) -> np.ndarray: + raise NotImplementedError() + + def cost_set(self, stage_: int, field_: str, value_): + raise NotImplementedError() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 1d5e54310a..1be50c6967 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -29,7 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from typing import Optional, Union +from typing import Optional, Union, Tuple import numpy as np from scipy.linalg import block_diag @@ -44,6 +44,7 @@ from .acados_ocp_constraints import AcadosOcpConstraints from .acados_dims import AcadosOcpDims from .acados_ocp_options import AcadosOcpOptions +from .acados_ocp_iterate import AcadosOcpIterate from .utils import (get_acados_path, format_class_dict, make_object_json_dumpable, render_template, get_shared_lib_ext, is_column, is_empty, casadi_length, check_if_square, @@ -1528,11 +1529,11 @@ def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.SX, ca.MX]] = None, W: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: - raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") if cost_hessian == 'GAUSS_NEWTON': if self.cost.cost_type not in ['LINEAR_LS', 'NONLINEAR_LS']: - raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type = {self.cost.cost_type}.") + raise ValueError(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type = {self.cost.cost_type}.") casadi_symbolics_type = type(self.model.x) @@ -1540,19 +1541,19 @@ def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.S yref = self.cost.yref else: if yref.shape[0] != self.cost.yref.shape[0]: - raise Exception(f"yref has wrong shape, got {yref.shape}, expected {self.cost.yref.shape}.") + raise ValueError(f"yref has wrong shape, got {yref.shape}, expected {self.cost.yref.shape}.") if not isinstance(yref, casadi_symbolics_type): - raise Exception(f"yref has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") + raise TypeError(f"yref has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") if W is None: W = self.cost.W else: if W.shape != self.cost.W.shape: - raise Exception(f"W has wrong shape, got {W.shape}, expected {self.cost.W.shape}.") + raise ValueError(f"W has wrong shape, got {W.shape}, expected {self.cost.W.shape}.") if not isinstance(W, casadi_symbolics_type): - raise Exception(f"W has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") + raise TypeError(f"W has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") if self.cost.cost_type == "LINEAR_LS": self.model.cost_expr_ext_cost = \ @@ -1575,11 +1576,11 @@ def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.S def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, ca.MX]] = None, W_e: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: - raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") if cost_hessian == 'GAUSS_NEWTON': if self.cost.cost_type_e not in ['LINEAR_LS', 'NONLINEAR_LS']: - raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_e = {self.cost.cost_type_e}.") + raise ValueError(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_e = {self.cost.cost_type_e}.") casadi_symbolics_type = type(self.model.x) @@ -1587,19 +1588,19 @@ def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, yref_e = self.cost.yref_e else: if yref_e.shape[0] != self.cost.yref_e.shape[0]: - raise Exception(f"yref_e has wrong shape, got {yref_e.shape}, expected {self.cost.yref_e.shape}.") + raise ValueError(f"yref_e has wrong shape, got {yref_e.shape}, expected {self.cost.yref_e.shape}.") if not isinstance(yref_e, casadi_symbolics_type): - raise Exception(f"yref_e has wrong type, got {type(yref_e)}, expected {casadi_symbolics_type}.") + raise TypeError(f"yref_e has wrong type, got {type(yref_e)}, expected {casadi_symbolics_type}.") if W_e is None: W_e = self.cost.W_e else: if W_e.shape != self.cost.W_e.shape: - raise Exception(f"W_e has wrong shape, got {W_e.shape}, expected {self.cost.W_e.shape}.") + raise ValueError(f"W_e has wrong shape, got {W_e.shape}, expected {self.cost.W_e.shape}.") if not isinstance(W_e, casadi_symbolics_type): - raise Exception(f"W_e has wrong type, got {type(W_e)}, expected {casadi_symbolics_type}.") + raise TypeError(f"W_e has wrong type, got {type(W_e)}, expected {casadi_symbolics_type}.") if self.cost.cost_type_e == "LINEAR_LS": self.model.cost_expr_ext_cost_e = \ @@ -1622,11 +1623,11 @@ def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, @staticmethod def __translate_ls_cost_to_external_cost(x, u, z, Vx, Vu, Vz, yref, W): res = 0 - if Vx is not None: + if not is_empty(Vx): res += Vx @ x - if Vu is not None and casadi_length(u) > 0: + if not is_empty(Vu): res += Vu @ u - if Vz is not None and casadi_length(z) > 0: + if not is_empty(Vz): res += Vz @ z res -= yref @@ -2122,3 +2123,113 @@ def detect_cost_type(self, model: AcadosModel, cost: AcadosOcpCost, dims: Acados cost.cost_type_0 = 'EXTERNAL' print('--------------------------------------------------------------') + + + def get_initial_cost_expression(self): + model = self.model + if self.cost.cost_type == "LINEAR_LS": + y = self.cost.Vx_0 @ model.x + self.cost.Vu_0 @ model.u + + if not is_empty(self.cost.Vz_0): + y += self.cost.Vz @ model.z + residual = y - self.cost.yref_0 + cost_dot = 0.5 * (residual.T @ self.cost.W_0 @ residual) + + elif self.cost.cost_type == "NONLINEAR_LS": + residual = model.cost_y_expr_0 - self.cost.yref_0 + cost_dot = 0.5 * (residual.T @ self.cost.W_0 @ residual) + + elif self.cost.cost_type == "EXTERNAL": + cost_dot = model.cost_expr_ext_cost_0 + + elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": + cost_dot = ca.substitute( + model.cost_psi_expr_0, model.cost_r_in_psi_expr_0, model.cost_y_expr_0) + else: + raise ValueError("create_model_with_cost_state: Unknown cost type.") + + return cost_dot + + + def get_path_cost_expression(self): + model = self.model + if self.cost.cost_type == "LINEAR_LS": + y = self.cost.Vx @ model.x + self.cost.Vu @ model.u + + if not is_empty(self.cost.Vz): + y += self.cost.Vz @ model.z + residual = y - self.cost.yref + cost_dot = 0.5 * (residual.T @ self.cost.W @ residual) + + elif self.cost.cost_type == "NONLINEAR_LS": + residual = model.cost_y_expr - self.cost.yref + cost_dot = 0.5 * (residual.T @ self.cost.W @ residual) + + elif self.cost.cost_type == "EXTERNAL": + cost_dot = model.cost_expr_ext_cost + + elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": + cost_dot = ca.substitute( + model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) + else: + raise ValueError("create_model_with_cost_state: Unknown cost type.") + + return cost_dot + + + def get_terminal_cost_expression(self): + model = self.model + if self.cost.cost_type_e == "LINEAR_LS": + y = self.cost.Vx_e @ model.x + residual = y - self.cost.yref_e + cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) + + elif self.cost.cost_type == "NONLINEAR_LS": + residual = model.cost_y_expr_e - self.cost.yref_e + cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) + + elif self.cost.cost_type == "EXTERNAL": + cost_dot = model.cost_expr_ext_cost_e + + elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": + cost_dot = ca.substitute( + model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) + else: + raise ValueError("create_model_with_cost_state: Unknown terminal cost type.") + + return cost_dot + + + def create_default_initial_iterate(self) -> AcadosOcpIterate: + """ + Create a default initial iterate for the OCP. + """ + self.make_consistent() + dims = self.dims + + if self.constraints.has_x0: + x_traj = (self.solver_options.N_horizon+1) * [self.constraints.x0] + else: + x_traj = (self.solver_options.N_horizon+1) * [np.zeros(dims.nx)] + u_traj = self.solver_options.N_horizon * [np.zeros(self.dims.nu)] + z_traj = self.solver_options.N_horizon * [np.zeros(self.dims.nz)] + sl_traj = [np.zeros(self.dims.ns_0)] + (self.solver_options.N_horizon-1) * [np.zeros(self.dims.ns)] + [np.zeros(self.dims.ns_e)] + su_traj = [np.zeros(self.dims.ns_0)] + (self.solver_options.N_horizon-1) * [np.zeros(self.dims.ns)] + [np.zeros(self.dims.ns_e)] + + pi_traj = self.solver_options.N_horizon * [np.zeros(self.dims.nx)] + + ni_0 = dims.nbu + dims.nbx_0 + dims.nh_0 + dims.nphi_0 + dims.ng + ni = dims.nbu + dims.nbx + dims.nh + dims.nphi + dims.ng + ni_e = dims.nbx_e + dims.nh_e + dims.nphi_e + dims.ng_e + lam_traj = [np.zeros(2*ni_0)] + (self.solver_options.N_horizon-1) * [np.zeros(2*ni)] + [np.zeros(2*ni_e)] + + iterate = AcadosOcpIterate( + x_traj=x_traj, + u_traj=u_traj, + z_traj=z_traj, + sl_traj=sl_traj, + su_traj=su_traj, + pi_traj=pi_traj, + lam_traj=lam_traj, + ) + return iterate diff --git a/interfaces/acados_template/acados_template/mpc_utils.py b/interfaces/acados_template/acados_template/mpc_utils.py index 6cb70f088b..8f0c0f55bf 100644 --- a/interfaces/acados_template/acados_template/mpc_utils.py +++ b/interfaces/acados_template/acados_template/mpc_utils.py @@ -28,7 +28,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # -from array import array + from copy import deepcopy from typing import Tuple, Optional import casadi as ca @@ -77,11 +77,11 @@ def __init__(self, ocp: AcadosOcp, with_parametric_bounds: bool = False): self.__parameter_values = np.tile(ocp.parameter_values, (ocp.dims.np, ocp.dims.N)) self.__p_global_values = ocp.p_global_values - self.time_steps = ocp.solver_options.time_steps + self.cost_scaling = ocp.solver_options.cost_scaling # setup casadi functions for constraints and cost - cost_expr = get_path_cost_expression(ocp) - cost_expr_e = get_terminal_cost_expression(ocp) + cost_expr = ocp.get_path_cost_expression() + cost_expr_e = ocp.get_terminal_cost_expression() p_global = ocp.model.p_global cost_fun_args = [ocp.model.x, ocp.model.u, ocp.model.p, p_global] @@ -283,7 +283,7 @@ def evaluate(self, x: np.ndarray, cost_fun_args = [x, u, parameter_values, p_global_values] # evaluate cost - cost_without_slacks = self.cost_fun(*cost_fun_args).full() * self.time_steps[step] + cost_without_slacks = self.cost_fun(*cost_fun_args).full() * self.cost_scaling[step] # evaluate constraints lower_violation, upper_violation, lower_slack, upper_slack = ( @@ -306,8 +306,7 @@ def evaluate(self, x: np.ndarray, if self.__ocp.cost.zu.size > 0: upper_slack_cost += self.__ocp.cost.zu @ upper_slack.full() - slack_cost = (lower_slack_cost + upper_slack_cost) * self.time_steps[step] - + slack_cost = (lower_slack_cost + upper_slack_cost) * self.cost_scaling[step] if len(slack_cost) == 0: cost = cost_without_slacks @@ -340,6 +339,7 @@ def evaluate_ocp_cost( cost = 0 # the cost on the first step is different in the OCP + # TODO: this is not correct, since the cost on the first step might be different! step = 0 result = self.evaluate(acados_ocp_iterate.x_traj[0], acados_ocp_iterate.u_traj[0], step=step) cost += result['cost_without_slacks'] @@ -390,55 +390,6 @@ def evaluate_ocp_cost( return cost[0][0] -def get_path_cost_expression(ocp: AcadosOcp): - model = ocp.model - if ocp.cost.cost_type == "LINEAR_LS": - y = ocp.cost.Vx @ model.x + ocp.cost.Vu @ model.u - - if casadi_length(model.z) > 0: - y += ocp.cost.Vz @ model.z - residual = y - ocp.cost.yref - cost_dot = 0.5 * (residual.T @ ocp.cost.W @ residual) - - elif ocp.cost.cost_type == "NONLINEAR_LS": - residual = model.cost_y_expr - ocp.cost.yref - cost_dot = 0.5 * (residual.T @ ocp.cost.W @ residual) - - elif ocp.cost.cost_type == "EXTERNAL": - cost_dot = model.cost_expr_ext_cost - - elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": - cost_dot = ca.substitute( - model.cost_psi_expr, model.cost_r_in_psi_expr, model.cost_y_expr) - else: - raise ValueError("create_model_with_cost_state: Unknown cost type.") - - return cost_dot - - -def get_terminal_cost_expression(ocp: AcadosOcp): - model = ocp.model - if ocp.cost.cost_type_e == "LINEAR_LS": - y = ocp.cost.Vx_e @ model.x - residual = y - ocp.cost.yref_e - cost_dot = 0.5 * (residual.T @ ocp.cost.W_e @ residual) - - elif ocp.cost.cost_type == "NONLINEAR_LS": - residual = model.cost_y_expr_e - ocp.cost.yref_e - cost_dot = 0.5 * (residual.T @ ocp.cost.W_e @ residual) - - elif ocp.cost.cost_type == "EXTERNAL": - cost_dot = model.cost_expr_ext_cost_e - - elif ocp.cost.cost_type == "CONVEX_OVER_NONLINEAR": - cost_dot = ca.substitute( - model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) - else: - raise ValueError("create_model_with_cost_state: Unknown terminal cost type.") - - return cost_dot - - def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarray]: """ Creates a new AcadosModel with an extra state `cost_state`, @@ -455,7 +406,7 @@ def create_model_with_cost_state(ocp: AcadosOcp) -> Tuple[AcadosModel, np.ndarra cost_state = symbol("cost_state") cost_state_dot = symbol("cost_state_dot") - cost_dot = get_path_cost_expression(ocp) + cost_dot = ocp.get_path_cost_expression() i_slack = 0 for ibu in ocp.constraints.idxsbu: diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 480c1019c5..87a1a2d34c 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -205,6 +205,7 @@ def casadi_length(x): raise TypeError("casadi_length expects one of the following types: casadi.MX, casadi.SX." + " Got: " + str(type(x))) + def get_shared_lib_ext(): if sys.platform == 'darwin': return '.dylib' From d4c8da97ed6fa3b29430bfa3f980381482db7999 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 20 May 2025 09:57:57 +0200 Subject: [PATCH 049/164] Fix `p_global` precomputation in case sparse data is detected (#1528) By making it dense in the MATLAB & Python interfaces. --- acados/utils/external_function_generic.c | 1 + interfaces/acados_matlab_octave/GenerateContext.m | 6 ++++++ .../acados_template/casadi_function_generation.py | 13 ++++++++++--- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/acados/utils/external_function_generic.c b/acados/utils/external_function_generic.c index b9bb6e543e..629f28d03b 100644 --- a/acados/utils/external_function_generic.c +++ b/acados/utils/external_function_generic.c @@ -1398,6 +1398,7 @@ static void external_function_external_param_casadi_set_global_data_pointer(void if (!fun->args_dense[fun->idx_in_global_data]) { + // NOTE: sparsity is removed in MATLAB/Python interface printf("\nexternal_function_external_param_casadi_set_global_data_pointer: sparse global_data not supported!\n"); exit(1); } diff --git a/interfaces/acados_matlab_octave/GenerateContext.m b/interfaces/acados_matlab_octave/GenerateContext.m index f39f405853..6b0380107c 100644 --- a/interfaces/acados_matlab_octave/GenerateContext.m +++ b/interfaces/acados_matlab_octave/GenerateContext.m @@ -195,6 +195,12 @@ global_data_expr_list = cellfun(@(pair) pair{2}, precompute_pairs, 'UniformOutput', false); self.global_data_expr = cse(vertcat(global_data_expr_list{:})); + % make sure global_data is dense + if length(self.global_data_expr) > 0 + self.global_data_expr = sparsity_cast(self.global_data_expr, Sparsity.dense(self.global_data_expr.nnz())); + self.global_data_sym = sparsity_cast(self.global_data_sym, Sparsity.dense(self.global_data_sym.nnz())); + end + % Assert length match assert(length(self.global_data_expr) == length(self.global_data_sym), ... sprintf('Length mismatch: %d != %d', length(self.global_data_expr), length(self.global_data_sym))); diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 1f926a8fb8..706f52c0b9 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -99,7 +99,7 @@ def __generate_functions(self): fun = ca.Function(name, inputs, outputs, self.__casadi_fun_opts) # print(f"Generating function {name} with inputs {inputs}") except RuntimeError as e: - print(f"\nError while creating function {name} with inputs {inputs} and outputs {outputs}") + print(f"\nError while creating function {name} with inputs \n{inputs} \n and outputs \n {outputs}") print(e) raise e @@ -170,9 +170,16 @@ def __setup_p_global_precompute_fun(self): for sym, expr in zip(symbols_to_add, param_expr_to_add): precompute_pairs.append([sym, expr]) - global_data_sym_list = [input for input, _ in precompute_pairs] + global_data_sym_list = [ca.vec(input) for input, _ in precompute_pairs] + global_data_expr_list = [ca.vec(output) for _, output in precompute_pairs] + self.global_data_sym = ca.vertcat(*global_data_sym_list) - self.global_data_expr = ca.cse(ca.vertcat(*[output for _, output in precompute_pairs])) + self.global_data_expr = ca.cse(ca.vertcat(*global_data_expr_list)) + + # make sure global_data is dense -> convert to dense only taking the non-zero elements into account. + if casadi_length(self.global_data_expr) > 0: + self.global_data_expr = ca.sparsity_cast(self.global_data_expr, ca.Sparsity.dense(self.global_data_expr.nnz())) + self.global_data_sym = ca.sparsity_cast(self.global_data_sym, ca.Sparsity.dense(self.global_data_sym.nnz())) assert casadi_length(self.global_data_expr) == casadi_length(self.global_data_sym), f"Length mismatch: {casadi_length(self.global_data_expr)} != {casadi_length(self.global_data_sym)}" From e453701acc4b9d930d866edafbce9562b727bfc6 Mon Sep 17 00:00:00 2001 From: Jingtao <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 21 May 2025 15:43:29 +0200 Subject: [PATCH 050/164] Reorder variables in `AcadosCasadiOcpSolver` (#1527) - organized the variables along time nodes as $x_0, x_1,....,x_{N+1}$ and $u_0, u_1...., u_N$ and then vectorized them in the interleaved order as $x_0, u_0, x_1, u_1...., x_N, u_N,x_{N+1}$. - I also adjust the `get()` function to get solution values in an interleaved manner. --- .../acados_casadi_ocp_solver.py | 98 +++++++++++-------- 1 file changed, 56 insertions(+), 42 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 6fa8d38f1e..a477b0fee3 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -37,7 +37,6 @@ from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates, AcadosOcpFlattenedIterate - class AcadosCasadiOcpSolver: @classmethod @@ -59,32 +58,33 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: if any([dims.ns_0, dims.ns, dims.ns_e]): raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") - # create variables + # create variables indexed by shooting nodes, vectorize in the end ca_symbol = model.get_casadi_symbol() - xtraj = ca_symbol('x', dims.nx, solver_options.N_horizon+1) - utraj = ca_symbol('u', dims.nu, solver_options.N_horizon) + xtraj_node = [ca_symbol(f'x{i}', dims.nx, 1) for i in range(solver_options.N_horizon+1)] + utraj_node = [ca_symbol(f'u{i}', dims.nu, 1) for i in range(solver_options.N_horizon)] if dims.nz > 0: raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") # parameters - ptraj = ca_symbol('p', dims.np, solver_options.N_horizon+1) + ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(solver_options.N_horizon)] ### Constraints: bounds # setup state bounds - lb_xtraj = -np.inf * ca.DM.ones((dims.nx, solver_options.N_horizon+1)) - ub_xtraj = np.inf * ca.DM.ones((dims.nx, solver_options.N_horizon+1)) - lb_xtraj[constraints.idxbx_0, 0] = constraints.lbx_0 - ub_xtraj[constraints.idxbx_0, 0] = constraints.ubx_0 + lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(solver_options.N_horizon+1)] + ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(solver_options.N_horizon+1)] + lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 + ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 for i in range(1, solver_options.N_horizon): - lb_xtraj[constraints.idxbx, i] = constraints.lbx - ub_xtraj[constraints.idxbx, i] = constraints.ubx - lb_xtraj[constraints.idxbx_e, -1] = constraints.lbx_e - ub_xtraj[constraints.idxbx_e, -1] = constraints.ubx_e + lb_xtraj_node[i][constraints.idxbx] = constraints.lbx + ub_xtraj_node[i][constraints.idxbx] = constraints.ubx + lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e + ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e + # setup control bounds - lb_utraj = -np.inf * ca.DM.ones((dims.nu, solver_options.N_horizon)) - ub_utraj = np.inf * ca.DM.ones((dims.nu, solver_options.N_horizon)) + lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] + ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] for i in range(solver_options.N_horizon): - lb_utraj[constraints.idxbu, i] = constraints.lbu - ub_utraj[constraints.idxbu, i] = constraints.ubu + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu ### Nonlinear constraints g = [] @@ -103,22 +103,22 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: for i in range(solver_options.N_horizon): # add dynamics constraints if solver_options.integrator_type == "DISCRETE": - g.append(xtraj[:, i+1] - f_discr_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + g.apppend(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) elif solver_options.integrator_type == "ERK": - g.append(xtraj[:, i+1] - f_discr_fun(xtraj[:, i], utraj[:, i], solver_options.time_steps[i])) + g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i])) lbg.append(np.zeros((dims.nx, 1))) ubg.append(np.zeros((dims.nx, 1))) # nonlinear constraints -- initial stage h0_fun = ca.Function('h0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) - g.append(h0_fun(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global)) + g.append(h0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) lbg.append(constraints.lh_0) ubg.append(constraints.uh_0) if dims.nphi_0 > 0: conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) conl_constr_0_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr_0]) - g.append(conl_constr_0_fun(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global)) + g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) lbg.append(constraints.lphi_0) ubg.append(constraints.uphi_0) @@ -130,26 +130,26 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr]) for i in range(1, solver_options.N_horizon): - g.append(h_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + g.append(h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) lbg.append(constraints.lh) ubg.append(constraints.uh) if dims.nphi > 0: - g.append(conl_constr_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global)) + g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) lbg.append(constraints.lphi) ubg.append(constraints.uphi) # nonlinear constraints -- terminal stage h_e_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) - g.append(h_e_fun(xtraj[:, -1], ptraj[:, -1], model.p_global)) + g.append(h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) lbg.append(constraints.lh_e) ubg.append(constraints.uh_e) if dims.nphi_e > 0: conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) conl_constr_e_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_constr_expr_e]) - g.append(conl_constr_e_fun(xtraj[:, -1], ptraj[:, -1], model.p_global)) + g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) lbg.append(constraints.lphi_e) ubg.append(constraints.uphi_e) @@ -158,25 +158,41 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: nlp_cost = 0 cost_expr_0 = ocp.get_initial_cost_expression() cost_fun_0 = ca.Function('cost_fun_0', [model.x, model.u, model.p, model.p_global], [cost_expr_0]) - nlp_cost += solver_options.cost_scaling[0] * cost_fun_0(xtraj[:, 0], utraj[:, 0], ptraj[:, 0], model.p_global) + nlp_cost += solver_options.cost_scaling[0] * cost_fun_0(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) # intermediate cost term cost_expr = ocp.get_path_cost_expression() cost_fun = ca.Function('cost_fun', [model.x, model.u, model.p, model.p_global], [cost_expr]) for i in range(1, solver_options.N_horizon): - nlp_cost += solver_options.cost_scaling[i] * cost_fun(xtraj[:, i], utraj[:, i], ptraj[:, i], model.p_global) + nlp_cost += solver_options.cost_scaling[i] * cost_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) # terminal cost term cost_expr_e = ocp.get_terminal_cost_expression() cost_fun_e = ca.Function('cost_fun_e', [model.x, model.p, model.p_global], [cost_expr_e]) - nlp_cost += solver_options.cost_scaling[-1] * cost_fun_e(xtraj[:, -1], ptraj[:, -1], model.p_global) - - # call w all primal variables - w = ca.vertcat(ca.vec(xtraj), ca.vec(utraj)) - lbw = ca.vertcat(ca.vec(lb_xtraj), ca.vec(lb_utraj)) - ubw = ca.vertcat(ca.vec(ub_xtraj), ca.vec(ub_utraj)) - p_nlp = ca.vertcat(ca.vec(ptraj), model.p_global) + nlp_cost += solver_options.cost_scaling[-1] * cost_fun_e(xtraj_node[-1], ptraj_node[-1], model.p_global) + ### Formulation + # interleave primary variables w and bounds + w_interleaved = [] + lbw_interleaved = [] + ubw_interleaved = [] + for i in range(solver_options.N_horizon): + w_interleaved.append(xtraj_node[i]) + lbw_interleaved.append(lb_xtraj_node[i]) + ubw_interleaved.append(ub_xtraj_node[i]) + w_interleaved.append(utraj_node[i]) + lbw_interleaved.append(lb_utraj_node[i]) + ubw_interleaved.append(ub_utraj_node[i]) + w_interleaved.append(xtraj_node[-1]) + lbw_interleaved.append(lb_xtraj_node[-1]) + ubw_interleaved.append(ub_xtraj_node[-1]) + + # vectorize + w = ca.vertcat(*w_interleaved) + lbw = ca.vertcat(*lbw_interleaved) + ubw = ca.vertcat(*ubw_interleaved) + p_nlp = ca.vertcat(*ptraj_node, model.p_global) + # create NLP nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} @@ -184,14 +200,14 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: return nlp, bounds - def __init__(self, acados_ocp: AcadosOcp, verbose=True): + def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): if not isinstance(acados_ocp, AcadosOcp): raise TypeError('acados_ocp should be of type AcadosOcp.') self.acados_ocp = acados_ocp self.casadi_nlp, self.bounds = self.create_casadi_nlp_formulation(acados_ocp) - self.casadi_solver = ca.nlpsol("nlp_solver", 'ipopt', self.casadi_nlp) + self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp) self.nlp_sol = None @@ -206,7 +222,7 @@ def solve(self) -> int: :return: status of the solver """ self.nlp_sol = self.casadi_solver(lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], lbg=self.bounds['lbg'], ubg=self.bounds['ubg']) - + self.nlp_sol_x = self.nlp_sol['x'].full() # TODO: return correct status return 0 @@ -244,14 +260,12 @@ def get(self, stage: int, field: str): if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') dims = self.acados_ocp.dims + pivot = stage*(dims.nx+dims.nu) if field == 'x': - sol_w = self.nlp_sol['x'] - return sol_w[stage*dims.nx:(stage+1)*dims.nx].full().flatten() + return self.nlp_sol_x[pivot:pivot+dims.nx].flatten() elif field == 'u': - sol_w = self.nlp_sol['x'] - offset_x = dims.nx*(self.acados_ocp.solver_options.N_horizon+1) - return sol_w[offset_x+stage*dims.nu: offset_x+(stage+1)*dims.nu].full().flatten() + return self.nlp_sol_x[pivot+dims.nx:pivot+dims.nx+dims.nu].flatten() else: raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") From 9d328514b9c580ef45ba386cbd93afb7ed1384e3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 23 May 2025 14:42:16 +0200 Subject: [PATCH 051/164] MATLAB: Implement code reuse for `AcadosSimSolver` (#1530) - Introduce `solver_creation_opts` as done for `AcadosOcpSolver` in https://github.com/acados/acados/pull/1472 - Python: Add option to render MATLAB specific templates, by checking if `simulink_opts` are not `None`. - MATLAB: Move some internal functionality into the classes, remove folder `interfaces/acados_matlab_octave/acados_template_mex` --- .github/linux/export_paths.sh | 2 +- .github/workflows/full_build.yml | 14 +- .../test/create_sim_solver_code_reuse.m | 62 +++++++ .../test/run_matlab_tests.m | 1 + .../test/test_sim_code_reuse.m | 72 ++++++++ .../sim/code_reuse_py2matlab_sim.m | 64 +++++++ .../sim/extensive_example_sim.py | 4 +- .../sim/minimal_example_sim_cmake.py | 1 + interfaces/CMakeLists.txt | 3 - interfaces/acados_matlab_octave/AcadosOcp.m | 2 +- .../acados_matlab_octave/AcadosOcpSolver.m | 92 ++++++++-- interfaces/acados_matlab_octave/AcadosSim.m | 148 +++++++++++++++- .../acados_matlab_octave/AcadosSimSolver.m | 167 +++++++++++++++--- interfaces/acados_matlab_octave/acados_sim.m | 3 +- .../compile_ocp_shared_lib.m | 93 ---------- .../compile_sim_shared_lib.m | 76 -------- .../render_acados_sim_templates.m | 78 -------- .../sim_generate_c_code.m | 124 ------------- .../acados_template/acados_sim.py | 30 +++- .../acados_template/acados_sim_solver.py | 3 + 20 files changed, 607 insertions(+), 432 deletions(-) create mode 100644 examples/acados_matlab_octave/test/create_sim_solver_code_reuse.m create mode 100644 examples/acados_matlab_octave/test/test_sim_code_reuse.m create mode 100644 examples/acados_python/pendulum_on_cart/sim/code_reuse_py2matlab_sim.m delete mode 100644 interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m delete mode 100644 interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_sim_shared_lib.m delete mode 100644 interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/render_acados_sim_templates.m delete mode 100644 interfaces/acados_matlab_octave/sim_generate_c_code.m diff --git a/.github/linux/export_paths.sh b/.github/linux/export_paths.sh index 22d3800a10..cbd927d5b8 100755 --- a/.github/linux/export_paths.sh +++ b/.github/linux/export_paths.sh @@ -34,5 +34,5 @@ echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV -echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/pendulum_on_cart_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase" >> $GITHUB_ENV +echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/pendulum_on_cart_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase:${1}/acados/examples/acados_python/pendulum_on_cart/sim/c_generated_code" >> $GITHUB_ENV echo "ENV_RUN=true" >> $GITHUB_ENV diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index d6b106d297..ef185520d3 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -174,16 +174,26 @@ jobs: run: | ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} - - name: Run Python tests that need new CasADi + - name: Run Python tests that need new CasADi & test py2matlab working-directory: ${{runner.workspace}}/acados/build shell: bash run: | source ${{runner.workspace}}/acados/acadosenv/bin/activate cd ${{runner.workspace}}/acados/examples/acados_python/p_global_example python example_p_global.py - echo "\nPython run done; testing tranfer to Octave\n" + echo "\nPython run done; testing OCP tranfer to Octave\n" octave code_reuse_py2matlab.m + - name: Run Python to Octave sim transfer test + working-directory: ${{runner.workspace}}/acados/build + shell: bash + run: | + source ${{runner.workspace}}/acados/acadosenv/bin/activate + cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/sim + python minimal_example_sim_cmake.py + echo "\nPython run done; testing SIM tranfer to Octave\n" + octave code_reuse_py2matlab_sim.m + - name: Run more Python tests working-directory: ${{runner.workspace}}/acados/build shell: bash diff --git a/examples/acados_matlab_octave/test/create_sim_solver_code_reuse.m b/examples/acados_matlab_octave/test/create_sim_solver_code_reuse.m new file mode 100644 index 0000000000..46b316ad31 --- /dev/null +++ b/examples/acados_matlab_octave/test/create_sim_solver_code_reuse.m @@ -0,0 +1,62 @@ +% +% 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.; + + +function sim_solver = create_sim_solver_code_reuse(creation_mode) + + addpath('../pendulum_on_cart_model') + + check_acados_requirements() + + json_file = 'pendulum_ocp.json'; + solver_creation_opts = struct(); + solver_creation_opts.json_file = json_file; + if strcmp(creation_mode, 'standard') + disp('Standard creation mode'); + elseif strcmp(creation_mode, 'precompiled') || strcmp(creation_mode, 'no_sim') + solver_creation_opts.generate = false; + solver_creation_opts.build = false; + solver_creation_opts.compile_mex_wrapper = false; + else + error('Invalid creation mode') + end + + if strcmp(creation_mode, 'no_sim') + sim = []; + else + model = get_pendulum_on_cart_model(); + sim = AcadosSim(); + sim.model = model; + sim.solver_options.Tsim = 0.1; % simulation time + sim.solver_options.integrator_type = 'ERK'; + end + + %% create integrator + sim_solver = AcadosSimSolver(sim, solver_creation_opts); +end diff --git a/examples/acados_matlab_octave/test/run_matlab_tests.m b/examples/acados_matlab_octave/test/run_matlab_tests.m index ee19069e9b..3701a899fd 100644 --- a/examples/acados_matlab_octave/test/run_matlab_tests.m +++ b/examples/acados_matlab_octave/test/run_matlab_tests.m @@ -55,6 +55,7 @@ %% run all tests test_names = [ "test_code_reuse", + "test_sim_code_reuse", "run_test_dim_check", "run_test_ocp_mass_spring", % "run_test_ocp_pendulum", diff --git a/examples/acados_matlab_octave/test/test_sim_code_reuse.m b/examples/acados_matlab_octave/test/test_sim_code_reuse.m new file mode 100644 index 0000000000..bd46cc169a --- /dev/null +++ b/examples/acados_matlab_octave/test/test_sim_code_reuse.m @@ -0,0 +1,72 @@ +% +% 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.; + +import casadi.* + +check_acados_requirements() +creation_modes = {'standard', 'precompiled', 'no_sim'}; +for i = 1:length(creation_modes) + + % simulation parameters + N_sim = 100; + x0 = [0; 1e-1; 0; 0]; % initial state + u0 = 0; % control input + + sim_solver = create_sim_solver_code_reuse(creation_modes{i}); + nx = length(x0); + + %% simulate system in loop + x_sim = zeros(nx, N_sim+1); + x_sim(:,1) = x0; + + for ii=1:N_sim + + % set initial state + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u0); + + % solve + sim_solver.solve(); + + % get simulated state + x_sim(:,ii+1) = sim_solver.get('xn'); + end + + % forward sensitivities ( dxn_d[x0,u] ) + S_forw = sim_solver.get('S_forw'); + + if i == 1 + S_forw_ref = S_forw; + elseif max(abs(S_forw-S_forw_ref)) > 1e-6 + error('solvers should have the same output independent of compilation options'); + end + S_forw + + clear sim_solver +end diff --git a/examples/acados_python/pendulum_on_cart/sim/code_reuse_py2matlab_sim.m b/examples/acados_python/pendulum_on_cart/sim/code_reuse_py2matlab_sim.m new file mode 100644 index 0000000000..c5ef3dc503 --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/sim/code_reuse_py2matlab_sim.m @@ -0,0 +1,64 @@ +% +% 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.; + + + +check_acados_requirements() + +json_file = 'acados_sim.json'; +solver_creation_opts = struct(); +solver_creation_opts.json_file = json_file; +solver_creation_opts.generate = false; +solver_creation_opts.build = false; +solver_creation_opts.compile_mex_wrapper = false; + +sim = []; + +%% create integrator +sim_solver = AcadosSimSolver(sim, solver_creation_opts); + +% simulation parameters +N_sim = 100; +x0 = [0; 1e-1; 0; 0]; % initial state +u0 = 0; % control input +nx = length(sim_solver.get('x', 0)); +%% simulate system in loop +x_sim = zeros(nx, N_sim+1); +x_sim(:,1) = x0; +for ii=1:N_sim + % set initial state + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u0); + % solve + sim_solver.solve(); + % get simulated state + x_sim(:,ii+1) = sim_solver.get('xn'); +end +disp('simulated state') +disp(x_sim(:,end)) diff --git a/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py b/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py index c7e91ee87f..203dff23a6 100644 --- a/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py +++ b/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py @@ -30,6 +30,8 @@ # import sys, json +import time + sys.path.insert(0, '../common') from acados_template import AcadosSim, AcadosSimSolver, acados_dae_model_json_dump, sim_get_default_cmake_builder @@ -114,8 +116,6 @@ ## Single test call -import time - t0 = time.time() acados_integrator.set("seed_adj", np.ones((nx, 1))) acados_integrator.set("x", x0) diff --git a/examples/acados_python/pendulum_on_cart/sim/minimal_example_sim_cmake.py b/examples/acados_python/pendulum_on_cart/sim/minimal_example_sim_cmake.py index 3ebcb1f8fc..d8ac63e4c6 100644 --- a/examples/acados_python/pendulum_on_cart/sim/minimal_example_sim_cmake.py +++ b/examples/acados_python/pendulum_on_cart/sim/minimal_example_sim_cmake.py @@ -63,6 +63,7 @@ def main(build=True, generate=True, use_cmake=True, use_cython=False): AcadosSimSolver.build(sim.code_export_directory, with_cython=True) acados_integrator = AcadosSimSolver.create_cython_solver('acados_sim.json') else: + sim.simulink_opts = dict() acados_integrator = AcadosSimSolver(sim, cmake_builder=cmake_builder, generate=generate, build=build) x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 272cca6c7c..355892b488 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -349,9 +349,6 @@ add_test(NAME python_pendulum_ocp_example_cmake python main_convex_onesided.py) # Sim - add_test(NAME python_pendulum_sim_example_cmake - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/sim - python minimal_example_sim_cmake.py) add_test(NAME python_pendulum_ext_sim_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/sim python extensive_example_sim.py diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index a876326246..1b65fff41e 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -481,7 +481,7 @@ function make_consistent_slack_dimensions_terminal(self) constraints = self.constraints; dims = self.dims; cost = self.cost; - + nsbx_e = length(constraints.idxsbx_e); nsg_e = length(constraints.idxsg_e); nsh_e = length(constraints.idxsh_e); diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 9550ce2815..18a821f4d2 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -53,7 +53,7 @@ function obj = AcadosOcpSolver(ocp, varargin) %% optional arguments: - % varagin{1}: solver_creation_opts: this is a struct in which some of the fields can be defined to overwrite the default values. + % varargin{1}: solver_creation_opts: this is a struct in which some of the fields can be defined to overwrite the default values. % The fields are: % - json_file: path to the json file containing the ocp description % - build: boolean, if true, the problem specific shared library is compiled @@ -90,9 +90,8 @@ if isempty(ocp) json_file = solver_creation_opts.json_file; - else - % OCP / MOCP provided + % formulation provided if ~isempty(solver_creation_opts.json_file) ocp.json_file = solver_creation_opts.json_file; end @@ -103,16 +102,16 @@ if ~isempty(ocp.solver_options.compile_interface) solver_creation_opts.compile_interface = ocp.solver_options.compile_interface; end - + % make consistent + ocp.make_consistent(); end - %% compile mex interface if needed obj.compile_mex_interface_if_needed(solver_creation_opts); %% generate if solver_creation_opts.generate - obj.generate(ocp); + obj.generate(); end %% load json, store options in object @@ -135,7 +134,7 @@ %% compile problem specific shared library if solver_creation_opts.build - acados_template_mex.compile_ocp_shared_lib(code_export_directory) + obj.compile_ocp_shared_lib(code_export_directory); end %% create solver @@ -445,16 +444,14 @@ function set_p_global_and_precompute_dependencies(obj, val) end % methods methods (Access = private) - function generate(obj, ocp) - % detect dimensions & sanity checks - obj.ocp.make_consistent() + function generate(obj) % generate - check_dir_and_create(fullfile(pwd, ocp.code_export_directory)); - context = ocp.generate_external_functions(); + check_dir_and_create(fullfile(pwd, obj.ocp.code_export_directory)); + context = obj.ocp.generate_external_functions(); - ocp.dump_to_json() - ocp.render_templates() + obj.ocp.dump_to_json() + obj.ocp.render_templates() end function compile_mex_interface_if_needed(obj, solver_creation_opts) @@ -511,7 +508,72 @@ function compile_mex_interface_if_needed(obj, solver_creation_opts) disp('found compiled acados MEX interface') end end - end + + + function compile_ocp_shared_lib(self, export_dir) + return_dir = pwd; + cd(export_dir); + if isunix + %% old code for make + if ~is_octave() + % use Make build system + [ status, result ] = system('make ocp_shared_lib'); + if status + cd(return_dir); + error('Building templated code as shared library failed.\nGot status %d, result: %s',... + status, result); + end + else + % use CMake build system, has issues on Linux with MATLAB, see https://github.com/acados/acados/issues/1209 + [ status, result ] = system('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); + if status + cd(return_dir); + error('Generating buildsystem failed.\nGot status %d, result: %s',... + status, result); + end + [ status, result ] = system('cmake --build . --config Release'); + if status + cd(return_dir); + error('Building templated code as shared library failed.\nGot status %d, result: %s',... + status, result); + end + end + else + % check compiler + use_msvc = false; + if ~is_octave() + mexOpts = mex.getCompilerConfigurations('C', 'Selected'); + if contains(mexOpts.ShortName, 'MSVC') + use_msvc = true; + end + end + % compile on Windows platform + if use_msvc + % get env vars for MSVC + % msvc_env = fullfile(mexOpts.Location, 'VC\Auxiliary\Build\vcvars64.bat'); + % assert(isfile(msvc_env), 'Cannot find definition of MSVC env vars.'); + % detect MSVC version + msvc_ver_str = "Visual Studio " + mexOpts.Version(1:2) + " " + mexOpts.Name(22:25); + [ status, result ] = system(['cmake -G "' + msvc_ver_str + '" -A x64 -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .']); + else + [ status, result ] = system('cmake -G "MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); + end + if status + cd(return_dir); + error('Generating buildsystem failed.\nGot status %d, result: %s',... + status, result); + end + [ status, result ] = system('cmake --build . --config Release'); + if status + cd(return_dir); + error('Building templated code as shared library failed.\nGot status %d, result: %s',... + status, result); + end + end + cd(return_dir); + end + + end % private methods end % class diff --git a/interfaces/acados_matlab_octave/AcadosSim.m b/interfaces/acados_matlab_octave/AcadosSim.m index 4e0be80caf..1556249900 100644 --- a/interfaces/acados_matlab_octave/AcadosSim.m +++ b/interfaces/acados_matlab_octave/AcadosSim.m @@ -55,9 +55,9 @@ obj.acados_include_path = []; obj.acados_lib_path = []; obj.shared_lib_ext = []; - obj.json_file = []; + obj.json_file = 'acados_sim.json'; obj.cython_include_dirs = []; - obj.code_export_directory = []; + obj.code_export_directory = 'c_generated_code'; obj.dims = AcadosSimDims(); obj.model = AcadosModel(); @@ -82,10 +82,6 @@ function make_consistent(self) self.json_file = 'acados_sim.json'; end - if isempty(self.code_export_directory) - self.code_export_directory = fullfile(pwd(), 'c_generated_code'); - end - % model self.model.make_consistent(self.dims); @@ -156,9 +152,149 @@ function make_consistent(self) error(['ERK: num_stages = ', num2str(self.solver_options.num_stages) ' not available. Only number of stages = {1,2,3,4} implemented!']); end end + end + + function generate_external_functions(self) + if nargin < 2 + % options for code generation + code_gen_opts = struct(); + code_gen_opts.generate_hess = self.solver_options.sens_hess; + code_gen_opts.code_export_directory = self.code_export_directory; + code_gen_opts.ext_fun_expand_dyn = self.solver_options.ext_fun_expand_dyn; + code_gen_opts.ext_fun_expand_cost = false; + code_gen_opts.ext_fun_expand_constr = false; + code_gen_opts.ext_fun_expand_precompute = false; + + context = GenerateContext(self.model.p_global, self.model.name, code_gen_opts); + else + code_gen_opts = context.code_gen_opts; + end + + model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [self.model.name '_model']); + check_dir_and_create(model_dir); + + if strcmp(self.model.dyn_ext_fun_type, 'generic') + copyfile(fullfile(pwd, self.model.dyn_generic_source), model_dir); + context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); + + elseif strcmp(self.model.dyn_ext_fun_type, 'casadi') + import casadi.* + check_casadi_version(); + switch self.solver_options.integrator_type + case 'ERK' + generate_c_code_explicit_ode(context, self.model, model_dir); + case 'IRK' + generate_c_code_implicit_ode(context, self.model, model_dir); + case 'GNSF' + generate_c_code_gnsf(context, self.model, model_dir); + case 'DISCRETE' + error('Discrete dynamics not supported in AcadosSim yet.') + % generate_c_code_discrete_dynamics(context, self.model, model_dir); + otherwise + error('Unknown integrator type.') + end + else + error('Unknown dyn_ext_fun_type.') + end + context.finalize(); + self.external_function_files_model = context.get_external_function_file_list(false); + end + + function dump_to_json(self, json_file) + if nargin < 2 + json_file = self.json_file; + end + + %% remove CasADi objects from model + model.name = self.model.name; + model.dyn_ext_fun_type = self.model.dyn_ext_fun_type; + model.dyn_generic_source = self.model.dyn_generic_source; + model.dyn_disc_fun_jac_hess = self.model.dyn_disc_fun_jac_hess; + model.dyn_disc_fun_jac = self.model.dyn_disc_fun_jac; + model.dyn_disc_fun = self.model.dyn_disc_fun; + model.gnsf_nontrivial_f_LO = self.model.gnsf_nontrivial_f_LO; + model.gnsf_purely_linear = self.model.gnsf_purely_linear; + self.model = model; + % jsonlab + acados_folder = getenv('ACADOS_INSTALL_DIR'); + addpath(fullfile(acados_folder, 'external', 'jsonlab')) + + %% post process numerical data (mostly cast scalars to 1-dimensional cells) + % parameter values + self.parameter_values = reshape(num2cell(self.parameter_values), [1, self.dims.np]); + + %% dump JSON file + sim_json_struct = self.struct(); + sim_json_struct.dims = self.dims.struct(); + sim_json_struct.solver_options = self.solver_options.struct(); + + % add compilation information to json + libs = loadjson(fileread(fullfile(acados_folder, 'lib', 'link_libs.json'))); + sim_json_struct.acados_link_libs = libs; + if ismac + sim_json_struct.os = 'mac'; + elseif isunix + sim_json_struct.os = 'unix'; + else + sim_json_struct.os = 'pc'; + end + + json_string = savejson('', sim_json_struct, 'ForceRootName', 0); + + fid = fopen(self.json_file, 'w'); + if fid == -1, error('Cannot create JSON file'); end + fwrite(fid, json_string, 'char'); + fclose(fid); + end + + function render_templates(self) + + json_fullfile = fullfile(pwd, self.json_file); + + acados_root_dir = getenv('ACADOS_INSTALL_DIR'); + acados_template_folder = fullfile(acados_root_dir,... + 'interfaces', 'acados_template', 'acados_template'); + + t_renderer_location = get_tera(); + + %% load json data + acados_sim = loadjson(fileread(json_fullfile)); + model_name = acados_sim.model.name; + + %% render templates + matlab_template_path = 'matlab_templates'; + main_dir = pwd; + chdir(self.code_export_directory); + + % cell array with entries (template_file, output file) + template_list = { ... + {'main_sim.in.c', ['main_sim_', model_name, '.c']}, ... + {fullfile(matlab_template_path, 'mex_sim_solver.in.m'), [model_name, '_mex_sim_solver.m']}, ... + {fullfile(matlab_template_path, 'make_mex_sim.in.m'), ['make_mex_sim_', model_name, '.m']}, ... + {fullfile(matlab_template_path, 'acados_sim_create.in.c'), ['acados_sim_create_', model_name, '.c']}, ... + {fullfile(matlab_template_path, 'acados_sim_free.in.c'), ['acados_sim_free_', model_name, '.c']}, ... + {fullfile(matlab_template_path, 'acados_sim_set.in.c'), ['acados_sim_set_', model_name, '.c']}, ... + {'acados_sim_solver.in.c', ['acados_sim_solver_', model_name, '.c']}, ... + {'acados_sim_solver.in.h', ['acados_sim_solver_', model_name, '.h']}, ... + {fullfile(matlab_template_path, 'acados_sim_solver_sfun.in.c'), ['acados_sim_solver_sfunction_', model_name, '.c']}, ... + {fullfile(matlab_template_path, 'make_sfun_sim.in.m'), ['make_sfun_sim_', model_name, '.m']}, ... + {'Makefile.in', 'Makefile'}, ... + {'CMakeLists.in.txt', 'CMakeLists.txt'}}; + + num_entries = length(template_list); + for n=1:num_entries + entry = template_list{n}; + render_file( entry{1}, entry{2}, json_fullfile); + end + c_dir = pwd; + chdir([model_name, '_model']); + render_file( 'model.in.h', [model_name, '_model.h'], json_fullfile); + cd(c_dir); + fprintf('Successfully rendered acados templates!\n'); + cd(main_dir) end function s = struct(self) diff --git a/interfaces/acados_matlab_octave/AcadosSimSolver.m b/interfaces/acados_matlab_octave/AcadosSimSolver.m index 639f876653..fabc4e411f 100644 --- a/interfaces/acados_matlab_octave/AcadosSimSolver.m +++ b/interfaces/acados_matlab_octave/AcadosSimSolver.m @@ -31,36 +31,97 @@ classdef AcadosSimSolver < handle - properties - t_sim % templated solver + properties (Access = public) sim % MATLAB class AcadosSim describing the initial value problem - end % properties - + end + properties (Access = private) + t_sim % templated solver + name + end % properties methods - function obj = AcadosSimSolver(sim, output_dir) + function obj = AcadosSimSolver(sim, varargin) + %% optional arguments: + % varargin{1}: solver_creation_opts: this is a struct in which some of the fields can be defined to overwrite the default values. + % The fields are: + % - json_file: path to the json file containing the ocp description + % - build: boolean, if true, the problem specific shared library is compiled + % - generate: boolean, if true, the C code is generated + % - compile_mex_wrapper: boolean, if true, the mex wrapper is compiled + % - compile_interface: can be [], true or false. If [], the interface is compiled if it does not exist. + % - output_dir: path to the directory where the MEX interface is compiled + obj.sim = sim; - if nargin < 2 - output_dir = fullfile(pwd, 'build'); + % optional arguments + % solver creation options + default_solver_creation_opts = struct('json_file', '', ... + 'build', true, ... + 'generate', true, ... + 'compile_mex_wrapper', true, ... + 'compile_interface', [], ... + 'output_dir', fullfile(pwd, 'build')); + if length(varargin) > 0 + solver_creation_opts = varargin{1}; + % set non-specified opts to default + fields = fieldnames(default_solver_creation_opts); + for i = 1:length(fields) + if ~isfield(solver_creation_opts, fields{i}) + solver_creation_opts.(fields{i}) = default_solver_creation_opts.(fields{i}); + end + end + else + solver_creation_opts = default_solver_creation_opts; end - % check model consistency - obj.sim = sim; - sim.make_consistent(); + if isempty(sim) && isempty(solver_creation_opts.json_file) + error('AcadosSimSolver: provide either a sim object or a json file'); + end - % create template sim - sim_generate_c_code(obj.sim); + if isempty(sim) + json_file = solver_creation_opts.json_file; + else + % formulation provided + if ~isempty(solver_creation_opts.json_file) + sim.json_file = solver_creation_opts.json_file; + end + json_file = sim.json_file; + if ~isempty(sim.solver_options.compile_interface) && ~isempty(solver_creation_opts.compile_interface) + error('AcadosOcpSolver: provide either compile_interface in OCP object or solver_creation_opts'); + end + if ~isempty(sim.solver_options.compile_interface) + solver_creation_opts.compile_interface = sim.solver_options.compile_interface; + end + % make consistent + sim.make_consistent(); + end % compile mex sim interface if needed - obj.compile_mex_sim_interface_if_needed(output_dir) + obj.compile_mex_sim_interface_if_needed(solver_creation_opts); + + %% generate + if solver_creation_opts.generate + obj.generate(); + end - % templated MEX + % load json: TODO!? + acados_folder = getenv('ACADOS_INSTALL_DIR'); + addpath(fullfile(acados_folder, 'external', 'jsonlab')); + acados_sim_struct = loadjson(fileread(json_file), 'SimplifyCell', 0); + obj.name = acados_sim_struct.model.name; + code_export_directory = acados_sim_struct.code_export_directory; + + %% compile problem specific shared library + if solver_creation_opts.build + obj.compile_sim_shared_lib(code_export_directory); + end + + %% create solver return_dir = pwd(); - cd(obj.sim.code_export_directory) + cd(code_export_directory) - mex_sim_solver = str2func(sprintf('%s_mex_sim_solver', obj.sim.model.name)); + mex_sim_solver = str2func(sprintf('%s_mex_sim_solver', obj.name)); obj.t_sim = mex_sim_solver(); addpath(pwd()); @@ -92,32 +153,88 @@ function set(obj, field, value) end % methods methods (Access = private) - function compile_mex_sim_interface_if_needed(obj, output_dir) + function generate(obj) + % generate + check_dir_and_create(fullfile(pwd, obj.sim.code_export_directory)); + obj.sim.generate_external_functions(); + + obj.sim.dump_to_json() + obj.sim.render_templates() + end + + function compile_mex_sim_interface_if_needed(obj, solver_creation_opts) - [~,~] = mkdir(output_dir); - addpath(output_dir); + [~,~] = mkdir(solver_creation_opts.output_dir); + addpath(solver_creation_opts.output_dir); % check if path contains spaces - if ~isempty(strfind(output_dir, ' ')) + if ~isempty(strfind(solver_creation_opts.output_dir, ' ')) error(strcat('compile_mex_sim_interface_if_needed: Path should not contain spaces, got: ',... - output_dir)); + solver_creation_opts.output_dir)); end %% compile mex without model dependency % check if mex interface exists already - if isempty(obj.sim.solver_options.compile_interface) % auto-detect + if isempty(solver_creation_opts.compile_interface) % auto-detect if is_octave() extension = '.mex'; else extension = ['.' mexext]; end - obj.sim.solver_options.compile_interface = ~exist(fullfile(output_dir, ['/sim_create', extension]), 'file'); + solver_creation_opts.compile_interface = ~exist(fullfile(solver_creation_opts.output_dir, ['/sim_create', extension]), 'file'); end - if obj.sim.solver_options.compile_interface - sim_compile_interface(output_dir); + if solver_creation_opts.compile_interface + sim_compile_interface(solver_creation_opts.output_dir); end end + + function compile_sim_shared_lib(obj, export_dir) + return_dir = pwd; + cd(export_dir); + if isunix + [ status, result ] = system('make sim_shared_lib'); + if status + cd(return_dir); + error('Building templated code as shared library failed.\nGot status %d, result: %s',... + status, result); + end + else + % check compiler + use_msvc = false; + if ~is_octave() + mexOpts = mex.getCompilerConfigurations('C', 'Selected'); + if contains(mexOpts.ShortName, 'MSVC') + use_msvc = true; + end + end + % compile on Windows platform + if use_msvc + % get env vars for MSVC + % msvc_env = fullfile(mexOpts.Location, 'VC\Auxiliary\Build\vcvars64.bat'); + % assert(isfile(msvc_env), 'Cannot find definition of MSVC env vars.'); + % detect MSVC version + msvc_ver_str = "Visual Studio " + mexOpts.Version(1:2) + " " + mexOpts.Name(22:25); + [ status, result ] = system(['cmake -G "' + msvc_ver_str + '" -A x64 -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_SIM_SOLVER_LIB=ON -DBUILD_ACADOS_OCP_SOLVER_LIB=OFF -S . -B .']); + else + [ status, result ] = system('cmake -G "MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_SIM_SOLVER_LIB=ON -DBUILD_ACADOS_OCP_SOLVER_LIB=OFF -S . -B .'); + end + if status + cd(return_dir); + error('Generating buildsystem failed.\nGot status %d, result: %s',... + status, result); + end + [ status, result ] = system('cmake --build . --config Release'); + if status + cd(return_dir); + error('Building templated code as shared library failed.\nGot status %d, result: %s',... + status, result); + end + end + + cd(return_dir); + end % methods (Access = private) + end end % class diff --git a/interfaces/acados_matlab_octave/acados_sim.m b/interfaces/acados_matlab_octave/acados_sim.m index c1f23531b3..2ebd037e16 100644 --- a/interfaces/acados_matlab_octave/acados_sim.m +++ b/interfaces/acados_matlab_octave/acados_sim.m @@ -31,8 +31,7 @@ function solver = acados_sim(model, opts) sim = setup_AcadosSim_from_legacy_sim_description(model, opts); - output_dir = opts.opts_struct.output_dir; - solver = AcadosSimSolver(sim, output_dir); + solver = AcadosSimSolver(sim, struct('output_dir', opts.opts_struct.output_dir)); % warning('In acados v0.4.0, many changes to the MATLAB/Octave interface of acados have been introduced.', ... % 'We recommend directly using the new AcadosSimSolver and to check the examples for the intended use.') end \ No newline at end of file diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m deleted file mode 100644 index c23954bdfb..0000000000 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_ocp_shared_lib.m +++ /dev/null @@ -1,93 +0,0 @@ -% -% 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.; - -% - -function compile_ocp_shared_lib(export_dir) - return_dir = pwd; - cd(export_dir); - if isunix - %% old code for make - if ~is_octave() - % use Make build system - [ status, result ] = system('make ocp_shared_lib'); - if status - cd(return_dir); - error('Building templated code as shared library failed.\nGot status %d, result: %s',... - status, result); - end - else - % use CMake build system, has issues on Linux with MATLAB, see https://github.com/acados/acados/issues/1209 - [ status, result ] = system('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); - if status - cd(return_dir); - error('Generating buildsystem failed.\nGot status %d, result: %s',... - status, result); - end - [ status, result ] = system('cmake --build . --config Release'); - if status - cd(return_dir); - error('Building templated code as shared library failed.\nGot status %d, result: %s',... - status, result); - end - end - else - % check compiler - use_msvc = false; - if ~is_octave() - mexOpts = mex.getCompilerConfigurations('C', 'Selected'); - if contains(mexOpts.ShortName, 'MSVC') - use_msvc = true; - end - end - % compile on Windows platform - if use_msvc - % get env vars for MSVC - % msvc_env = fullfile(mexOpts.Location, 'VC\Auxiliary\Build\vcvars64.bat'); - % assert(isfile(msvc_env), 'Cannot find definition of MSVC env vars.'); - % detect MSVC version - msvc_ver_str = "Visual Studio " + mexOpts.Version(1:2) + " " + mexOpts.Name(22:25); - [ status, result ] = system(['cmake -G "' + msvc_ver_str + '" -A x64 -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .']); - else - [ status, result ] = system('cmake -G "MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); - end - if status - cd(return_dir); - error('Generating buildsystem failed.\nGot status %d, result: %s',... - status, result); - end - [ status, result ] = system('cmake --build . --config Release'); - if status - cd(return_dir); - error('Building templated code as shared library failed.\nGot status %d, result: %s',... - status, result); - end - end - cd(return_dir); -end diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_sim_shared_lib.m b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_sim_shared_lib.m deleted file mode 100644 index d3ee59a739..0000000000 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/compile_sim_shared_lib.m +++ /dev/null @@ -1,76 +0,0 @@ -% -% 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.; - -% - -function compile_sim_shared_lib(export_dir) - return_dir = pwd; - cd(export_dir); - if isunix - [ status, result ] = system('make sim_shared_lib'); - if status - cd(return_dir); - error('Building templated code as shared library failed.\nGot status %d, result: %s',... - status, result); - end - else - % check compiler - use_msvc = false; - if ~is_octave() - mexOpts = mex.getCompilerConfigurations('C', 'Selected'); - if contains(mexOpts.ShortName, 'MSVC') - use_msvc = true; - end - end - % compile on Windows platform - if use_msvc - % get env vars for MSVC - % msvc_env = fullfile(mexOpts.Location, 'VC\Auxiliary\Build\vcvars64.bat'); - % assert(isfile(msvc_env), 'Cannot find definition of MSVC env vars.'); - % detect MSVC version - msvc_ver_str = "Visual Studio " + mexOpts.Version(1:2) + " " + mexOpts.Name(22:25); - [ status, result ] = system(['cmake -G "' + msvc_ver_str + '" -A x64 -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_SIM_SOLVER_LIB=ON -DBUILD_ACADOS_OCP_SOLVER_LIB=OFF -S . -B .']); - else - [ status, result ] = system('cmake -G "MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_SIM_SOLVER_LIB=ON -DBUILD_ACADOS_OCP_SOLVER_LIB=OFF -S . -B .'); - end - if status - cd(return_dir); - error('Generating buildsystem failed.\nGot status %d, result: %s',... - status, result); - end - [ status, result ] = system('cmake --build . --config Release'); - if status - cd(return_dir); - error('Building templated code as shared library failed.\nGot status %d, result: %s',... - status, result); - end - end - - cd(return_dir); -end diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/render_acados_sim_templates.m b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/render_acados_sim_templates.m deleted file mode 100644 index 6a229035a5..0000000000 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/render_acados_sim_templates.m +++ /dev/null @@ -1,78 +0,0 @@ -% -% 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.; - -% - -function render_acados_sim_templates(acados_sim_json_file) - - acados_root_dir = getenv('ACADOS_INSTALL_DIR'); - acados_template_folder = fullfile(acados_root_dir,... - 'interfaces', 'acados_template', 'acados_template'); - - t_renderer_location = get_tera(); - - %% load json data - acados_sim = loadjson(fileread(acados_sim_json_file)); - model_name = acados_sim.model.name; - - %% render templates - matlab_template_path = 'matlab_templates'; - json_fullfile = fullfile(pwd, acados_sim_json_file); - main_dir = pwd; - chdir('c_generated_code'); - - % cell array with entries (template_file, output file) - template_list = { ... - {'main_sim.in.c', ['main_sim_', model_name, '.c']}, ... - {fullfile(matlab_template_path, 'mex_sim_solver.in.m'), [model_name, '_mex_sim_solver.m']}, ... - {fullfile(matlab_template_path, 'make_mex_sim.in.m'), ['make_mex_sim_', model_name, '.m']}, ... - {fullfile(matlab_template_path, 'acados_sim_create.in.c'), ['acados_sim_create_', model_name, '.c']}, ... - {fullfile(matlab_template_path, 'acados_sim_free.in.c'), ['acados_sim_free_', model_name, '.c']}, ... - {fullfile(matlab_template_path, 'acados_sim_set.in.c'), ['acados_sim_set_', model_name, '.c']}, ... - {'acados_sim_solver.in.c', ['acados_sim_solver_', model_name, '.c']}, ... - {'acados_sim_solver.in.h', ['acados_sim_solver_', model_name, '.h']}, ... - {fullfile(matlab_template_path, 'acados_sim_solver_sfun.in.c'), ['acados_sim_solver_sfunction_', model_name, '.c']}, ... - {fullfile(matlab_template_path, 'make_sfun_sim.in.m'), ['make_sfun_sim_', model_name, '.m']}, ... - {'Makefile.in', 'Makefile'}, ... - {'CMakeLists.in.txt', 'CMakeLists.txt'}}; - - num_entries = length(template_list); - for n=1:num_entries - entry = template_list{n}; - render_file( entry{1}, entry{2}, json_fullfile); - end - - c_dir = pwd; - chdir([model_name, '_model']); - render_file( 'model.in.h', [model_name, '_model.h'], json_fullfile); - cd(c_dir); - - fprintf('Successfully rendered acados templates!\n'); - cd(main_dir) -end diff --git a/interfaces/acados_matlab_octave/sim_generate_c_code.m b/interfaces/acados_matlab_octave/sim_generate_c_code.m deleted file mode 100644 index faeaeadd1e..0000000000 --- a/interfaces/acados_matlab_octave/sim_generate_c_code.m +++ /dev/null @@ -1,124 +0,0 @@ -% -% 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.; - -% - -function sim_generate_c_code(sim, context) - - if nargin < 2 - % options for code generation - code_gen_opts = struct(); - code_gen_opts.generate_hess = sim.solver_options.sens_hess; - code_gen_opts.code_export_directory = 'c_generated_code'; % TODO: for OCP this is part of OCP class - code_gen_opts.ext_fun_expand_dyn = sim.solver_options.ext_fun_expand_dyn; - code_gen_opts.ext_fun_expand_cost = false; - code_gen_opts.ext_fun_expand_constr = false; - code_gen_opts.ext_fun_expand_precompute = false; - - context = GenerateContext(sim.model.p_global, sim.model.name, code_gen_opts); - else - code_gen_opts = context.code_gen_opts; - end - - model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [sim.model.name '_model']); - check_dir_and_create(model_dir); - - if strcmp(sim.model.dyn_ext_fun_type, 'generic') - copyfile(fullfile(pwd, sim.model.dyn_generic_source), model_dir); - context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); - - elseif strcmp(sim.model.dyn_ext_fun_type, 'casadi') - import casadi.* - check_casadi_version(); - switch sim.solver_options.integrator_type - case 'ERK' - generate_c_code_explicit_ode(context, sim.model, model_dir); - case 'IRK' - generate_c_code_implicit_ode(context, sim.model, model_dir); - case 'GNSF' - generate_c_code_gnsf(context, sim.model, model_dir); - case 'DISCRETE' - generate_c_code_discrete_dynamics(context, sim.model, model_dir); - otherwise - error('Unknown integrator type.') - end - else - error('Unknown dyn_ext_fun_type.') - end - - context.finalize(); - sim.external_function_files_model = context.get_external_function_file_list(false); - - %% remove CasADi objects from model - model.name = sim.model.name; - model.dyn_ext_fun_type = sim.model.dyn_ext_fun_type; - model.dyn_generic_source = sim.model.dyn_generic_source; - model.dyn_disc_fun_jac_hess = sim.model.dyn_disc_fun_jac_hess; - model.dyn_disc_fun_jac = sim.model.dyn_disc_fun_jac; - model.dyn_disc_fun = sim.model.dyn_disc_fun; - model.gnsf_nontrivial_f_LO = sim.model.gnsf_nontrivial_f_LO; - model.gnsf_purely_linear = sim.model.gnsf_purely_linear; - sim.model = model; - %% post process numerical data (mostly cast scalars to 1-dimensional cells) - dims = sim.dims; - - %% load JSON layout - acados_folder = getenv('ACADOS_INSTALL_DIR'); - addpath(fullfile(acados_folder, 'external', 'jsonlab')) - - % parameter values - sim.parameter_values = reshape(num2cell(sim.parameter_values), [1, dims.np]); - - %% dump JSON file - sim_json_struct = sim.struct(); - sim_json_struct.dims = sim.dims.struct(); - sim_json_struct.solver_options = sim.solver_options.struct(); - - % add compilation information to json - libs = loadjson(fileread(fullfile(acados_folder, 'lib', 'link_libs.json'))); - sim_json_struct.acados_link_libs = libs; - if ismac - sim_json_struct.os = 'mac'; - elseif isunix - sim_json_struct.os = 'unix'; - else - sim_json_struct.os = 'pc'; - end - - json_string = savejson('', sim_json_struct, 'ForceRootName', 0); - - fid = fopen(sim.json_file, 'w'); - if fid == -1, error('Cannot create JSON file'); end - fwrite(fid, json_string, 'char'); - fclose(fid); - %% render templated code - acados_template_mex.render_acados_sim_templates(sim.json_file) - acados_template_mex.compile_sim_shared_lib(sim.code_export_directory) -end - diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 3807cf04b4..e35d2caba5 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -175,7 +175,7 @@ def num_threads_in_batch_solve(self): Default: 1. """ return self.__num_threads_in_batch_solve - + @property def with_batch_functionality(self): """ @@ -341,6 +341,9 @@ def __init__(self, acados_path=''): """Path to where code will be exported. Default: `c_generated_code`.""" self.shared_lib_ext = get_shared_lib_ext() + self.simulink_opts = None + """Options to configure Simulink S-function blocks, if not None, MATLAB related files will be generated. More options may be added in the future, similar to OCP interface""" + # get cython paths from sysconfig import get_paths self.cython_include_dirs = [np.get_include(), get_paths()['include']] @@ -394,16 +397,32 @@ def dump_to_json(self, json_file='acados_sim.json') -> None: def render_templates(self, json_file, cmake_options: CMakeBuilder = None): # setting up loader and environment json_path = os.path.join(os.getcwd(), json_file) + name = self.model.name if not os.path.exists(json_path): raise FileNotFoundError(f"{json_path} not found!") template_list = [ - ('acados_sim_solver.in.c', f'acados_sim_solver_{self.model.name}.c'), - ('acados_sim_solver.in.h', f'acados_sim_solver_{self.model.name}.h'), + ('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c'), + ('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h'), ('acados_sim_solver.in.pxd', 'acados_sim_solver.pxd'), - ('main_sim.in.c', f'main_sim_{self.model.name}.c'), + ('main_sim.in.c', f'main_sim_{name}.c'), ] + if self.simulink_opts is not None: + template_file = os.path.join('matlab_templates', 'mex_sim_solver.in.m') + template_list.append((template_file, f'{name}_mex_sim_solver.m')) + template_file = os.path.join('matlab_templates', 'make_mex_sim.in.m') + template_list.append((template_file, f'make_mex_sim_{name}.m')) + template_file = os.path.join('matlab_templates', 'acados_sim_create.in.c') + template_list.append((template_file, f'acados_sim_create_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_free.in.c') + template_list.append((template_file, f'acados_sim_free_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_set.in.c') + template_list.append((template_file, f'acados_sim_set_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') + template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) + template_file = os.path.join('matlab_templates', 'make_sfun_sim.in.m') + template_list.append((template_file, f'make_sfun_sim_{name}.m')) # Builder if cmake_options is not None: @@ -451,6 +470,9 @@ def generate_external_functions(self, ): generate_c_code_implicit_ode(context, self.model, model_dir) elif integrator_type == 'GNSF': generate_c_code_gnsf(context, self.model, model_dir) + else: + raise ValueError('Invalid integrator_type value. Possible values are:\n\n' \ + + ',\n'.join(['ERK', 'IRK', 'GNSF']) + '.\n\nYou have: ' + integrator_type + '.\n\n') context.finalize() self.__external_function_files_model = context.get_external_function_file_list(ocp_specific=False) diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index 75bf1da4d8..ccaa695baa 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -156,6 +156,9 @@ def __init__(self, acados_sim: AcadosSim, json_file='acados_sim.json', generate= print("Warning: An AcadosSimSolver is created from an AcadosOcp description.", "This only works if you created an AcadosOcpSolver before with the same description." "Otherwise it leads to undefined behavior. Using an AcadosSim description is recommended.") + if acados_sim.dims.np_global > 0: + raise ValueError("AcadosSimSolver: AcadosOcp with np_global > 0 is not supported.") + self.__T = acados_sim.solver_options.Tsim else: self.__T = acados_sim.solver_options.T From 24fa455dccf0933f2710fa2f261c507ba7d003b9 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 26 May 2025 09:41:55 +0200 Subject: [PATCH 052/164] C: precompute total dimensions (#1531) --- acados/ocp_nlp/ocp_nlp_common.c | 59 ++++++++++++++++++++- acados/ocp_nlp/ocp_nlp_common.h | 14 +++++ acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 4 ++ interfaces/acados_c/ocp_nlp_interface.c | 66 +++++------------------- 4 files changed, 88 insertions(+), 55 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index e878149025..f36909aab6 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -3154,7 +3154,7 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl { int N = dims->N; int status = ACADOS_SUCCESS; - int ii; + int ii, tmp; for (ii = 0; ii <= N; ii++) { @@ -3168,6 +3168,63 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl } } + // compute total dimensions + dims->nx_total = 0; + for (ii = 0; ii < N+1; ii++) + { + dims->nx_total += dims->nx[ii]; + } + dims->nu_total = 0; + for (ii = 0; ii < N; ii++) + { + dims->nu_total += dims->nu[ii]; + } + dims->nz_total = 0; + for (ii = 0; ii < N; ii++) + { + dims->nz_total += dims->nz[ii]; + } + dims->ni_total = 0; + for (ii = 0; ii < N+1; ii++) + { + dims->ni_total += dims->ni[ii]; + } + dims->ns_total = 0; + for (ii = 0; ii < N+1; ii++) + { + dims->ns_total += dims->ns[ii]; + } + dims->np_total = 0; + for (ii = 0; ii < N+1; ii++) + { + dims->np_total += dims->np[ii]; + } + dims->nbx_total = 0; + for (ii = 0; ii < N+1; ii++) + { + config->constraints[ii]->dims_get(config->constraints[ii], dims->constraints[ii], "nbx", &tmp); + dims->nbx_total += tmp; + } + dims->nbu_total = 0; + for (ii = 0; ii < N; ii++) + { + config->constraints[ii]->dims_get(config->constraints[ii], dims->constraints[ii], "nbu", &tmp); + dims->nbu_total += tmp; + } + dims->ng_total = 0; + for (ii = 0; ii < N+1; ii++) + { + dims->ng_total += dims->ng[ii]; + } + dims->nh_total = 0; + for (ii = 0; ii < N+1; ii++) + { + config->constraints[ii]->dims_get(config->constraints[ii], dims->constraints[ii], + "nh", &tmp); + dims->nh_total += tmp; + } + + // precompute for (ii = 0; ii < N; ii++) { diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index cc52c24881..30dd5fa6ae 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -163,6 +163,20 @@ typedef struct ocp_nlp_dims int n_global_data; // size of global_data; expressions that only depend on p_global; detected automatically during code generation int N; // number of shooting nodes + // total dimensions + int nx_total; // total number of states + int nu_total; // total number of controls + int ns_total; // total number of slack variables + int nz_total; // total number of algebraic variables + // int npi_total; // = nx_total - nx_0 + int ni_total; // total number of inequalities + int np_total; // total number of parameters + int nbx_total; // total number of state bounds + int nbu_total; // total number of control bounds + int ng_total; // total number of general linear constraints + int nh_total; // total number of nonlinear inequalities + int nphi_total; // total number of nonlinear inequalities + void *raw_memory; // Pointer to allocated memory, to be used for freeing } ocp_nlp_dims; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index 9516da69c3..ed81e44276 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -215,6 +215,10 @@ void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *fi { *value = dims->ng; } + else if (!strcmp(field, "nh")) + { + *value = 0; + } else if (!strcmp(field, "nphi")) { *value = dims->nphi; diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 8024ba5f17..5f380034b7 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -745,104 +745,62 @@ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou int ocp_nlp_dims_get_total_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, const char *field) { - int N = dims->N; - - int size = 0; - int stage; if (!strcmp(field, "x")) { - for (stage = 0; stage < N+1; stage++) - { - size += dims->nx[stage]; - } + return dims->nx_total; } else if (!strcmp(field, "u")) { - for (stage = 0; stage < N; stage++) - { - size += dims->nu[stage]; - } + return dims->nu_total; } else if (!strcmp(field, "sl") || !strcmp(field, "su") || !strcmp(field, "zl") || !strcmp(field, "zu") || !strcmp(field, "Zl") || !strcmp(field, "Zu") || !strcmp(field, "cost_z") || !strcmp(field, "cost_Z")) { - for (stage = 0; stage < N+1; stage++) - { - size += dims->ns[stage]; - } + return dims->ns_total; } else if (!strcmp(field, "s")) { - for (stage = 0; stage < N+1; stage++) - { - size += 2*dims->ns[stage]; - } + return 2*dims->ns_total; } else if (!strcmp(field, "z")) { - for (stage = 0; stage < N+1; stage++) - { - size += dims->nz[stage]; - } + return dims->nz_total; } else if (!strcmp(field, "pi")) { - for (stage = 0; stage < N; stage++) - { - size += dims->nx[stage+1]; - } + return dims->nx_total - dims->nx[0]; } else if (!strcmp(field, "lam")) { - for (stage = 0; stage < N+1; stage++) - { - size += 2*dims->ni[stage]; - } + return 2*dims->ni_total; } else if (!strcmp(field, "p")) { - for (stage = 0; stage < N+1; stage++) - { - size += dims->np[stage]; - } + return dims->np_total; } else if (!strcmp(field, "lbx") || !strcmp(field, "ubx") || !strcmp(field, "nbx")) { - for (stage = 0; stage < N+1; stage++) - { - size += ocp_nlp_dims_get_from_attr(config, dims, out, stage, "lbx"); - } + return dims->nbx_total; } else if (!strcmp(field, "lbu") || !strcmp(field, "ubu") || !strcmp(field, "nbu")) { - for (stage = 0; stage < N; stage++) - { - size += ocp_nlp_dims_get_from_attr(config, dims, out, stage, "lbu"); - } + return dims->nbu_total; } else if (!strcmp(field, "lg") || !strcmp(field, "ug") || !strcmp(field, "ng")) { - for (stage = 0; stage < N+1; stage++) - { - size += ocp_nlp_dims_get_from_attr(config, dims, out, stage, "lg"); - } + return dims->ng_total; } else if (!strcmp(field, "lh") || !strcmp(field, "uh") || !strcmp(field, "nh")) { - for (stage = 0; stage < N+1; stage++) - { - size += ocp_nlp_dims_get_from_attr(config, dims, out, stage, "lh"); - } + return dims->nh_total; } else { printf("\nerror: ocp_nlp_dims_get_total_from_attr: field %s not available\n", field); exit(1); } - - return size; } From 03882d106093d8b5266940b21a071743f6412c2d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 26 May 2025 10:02:01 +0200 Subject: [PATCH 053/164] Matlab interface: surpress output (#1532) --- interfaces/acados_matlab_octave/AcadosOcp.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 1b65fff41e..ba9eba1368 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -584,7 +584,7 @@ function make_consistent_discretization(self) end function make_consistent_simulation(self) - opts = self.solver_options + opts = self.solver_options; if opts.N_horizon == 0 return end @@ -1000,7 +1000,7 @@ function make_consistent(self, is_mocp_phase) function [] = detect_cost_and_constraints(self) % detect cost type - N = self.solver_options.N_horizon + N = self.solver_options.N_horizon; if N == 0 if strcmp(self.cost.cost_type_e, 'AUTO') detect_cost_type(self.model, self.cost, self.dims, 'terminal'); From 0b96bea262d6a9e991e9396eb38273ab8dafc11f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 26 May 2025 17:14:46 +0200 Subject: [PATCH 054/164] `AcadosCasadiOcpSolver`: add initial guess for primal variables (#1533) also minor: fix `get_terminal_cost_expression` for empty LS cost --- .../ocp/test_casadi_formulation.py | 3 +- .../acados_casadi_ocp_solver.py | 73 ++++++++++++------- .../acados_template/acados_ocp.py | 2 + 3 files changed, 50 insertions(+), 28 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py index 344b539ab1..f913747be7 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -52,7 +52,8 @@ def main(): diff_u = np.linalg.norm(u_casadi_sol - simU) print(f"Difference between casadi and acados solution: {diff_u}") - test_tol = 1e-5 + # TODO: set solver tolerance and reduce it here. + test_tol = 5e-5 if diff_x > test_tol or diff_u > test_tol: raise ValueError(f"Test failed: difference between casadi and acados solution should be smaller than {test_tol}, but got {diff_x} and {diff_u}.") diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index a477b0fee3..82e0088972 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -40,12 +40,12 @@ class AcadosCasadiOcpSolver: @classmethod - def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: + def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.ndarray]: """ Creates an equivalent CasADi NLP formulation of the OCP. Experimental, not fully implemented yet. - :return: nlp_dict, bounds_dict + :return: nlp_dict, bounds_dict, w0 (initial guess) """ ocp.make_consistent() @@ -78,7 +78,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: ub_xtraj_node[i][constraints.idxbx] = constraints.ubx lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e - + # setup control bounds lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] @@ -173,31 +173,42 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict]: ### Formulation # interleave primary variables w and bounds - w_interleaved = [] - lbw_interleaved = [] - ubw_interleaved = [] + # w = [x0, u0, x1, u1, ...] + w_sym_list = [] + lbw_list = [] + ubw_list = [] + w0_list = [] + x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) for i in range(solver_options.N_horizon): - w_interleaved.append(xtraj_node[i]) - lbw_interleaved.append(lb_xtraj_node[i]) - ubw_interleaved.append(ub_xtraj_node[i]) - w_interleaved.append(utraj_node[i]) - lbw_interleaved.append(lb_utraj_node[i]) - ubw_interleaved.append(ub_utraj_node[i]) - w_interleaved.append(xtraj_node[-1]) - lbw_interleaved.append(lb_xtraj_node[-1]) - ubw_interleaved.append(ub_xtraj_node[-1]) + # add x + w_sym_list.append(xtraj_node[i]) + lbw_list.append(lb_xtraj_node[i]) + ubw_list.append(ub_xtraj_node[i]) + w0_list.append(x_guess) + # add u + w_sym_list.append(utraj_node[i]) + lbw_list.append(lb_utraj_node[i]) + ubw_list.append(ub_utraj_node[i]) + w0_list.append(np.zeros((dims.nu,))) + ## terminal stage + # add x + w_sym_list.append(xtraj_node[-1]) + lbw_list.append(lb_xtraj_node[-1]) + ubw_list.append(ub_xtraj_node[-1]) + w0_list.append(x_guess) # vectorize - w = ca.vertcat(*w_interleaved) - lbw = ca.vertcat(*lbw_interleaved) - ubw = ca.vertcat(*ubw_interleaved) + w = ca.vertcat(*w_sym_list) + lbw = ca.vertcat(*lbw_list) + ubw = ca.vertcat(*ubw_list) p_nlp = ca.vertcat(*ptraj_node, model.p_global) - + # create NLP nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} + w0 = np.concatenate(w0_list) - return nlp, bounds + return nlp, bounds, w0 def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): @@ -206,7 +217,7 @@ def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): raise TypeError('acados_ocp should be of type AcadosOcp.') self.acados_ocp = acados_ocp - self.casadi_nlp, self.bounds = self.create_casadi_nlp_formulation(acados_ocp) + self.casadi_nlp, self.bounds, self.w0 = self.create_casadi_nlp_formulation(acados_ocp) self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp) self.nlp_sol = None @@ -221,7 +232,9 @@ def solve(self) -> int: :return: status of the solver """ - self.nlp_sol = self.casadi_solver(lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], lbg=self.bounds['lbg'], ubg=self.bounds['ubg']) + self.nlp_sol = self.casadi_solver(x0=self.w0, + lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], + lbg=self.bounds['lbg'], ubg=self.bounds['ubg']) self.nlp_sol_x = self.nlp_sol['x'].full() # TODO: return correct status return 0 @@ -260,12 +273,12 @@ def get(self, stage: int, field: str): if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') dims = self.acados_ocp.dims - pivot = stage*(dims.nx+dims.nu) + offset = stage*(dims.nx+dims.nu) if field == 'x': - return self.nlp_sol_x[pivot:pivot+dims.nx].flatten() + return self.nlp_sol_x[offset:offset+dims.nx].flatten() elif field == 'u': - return self.nlp_sol_x[pivot+dims.nx:pivot+dims.nx+dims.nu].flatten() + return self.nlp_sol_x[offset+dims.nx:offset+dims.nx+dims.nu].flatten() else: raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") @@ -311,8 +324,14 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: def get_cost(self) -> float: raise NotImplementedError() - def set(self, stage_: int, field_: str, value_: np.ndarray): - raise NotImplementedError() + def set(self, stage: int, field: str, value_: np.ndarray): + dims = self.acados_ocp.dims + offset = stage*(dims.nx+dims.nu) + + if field == 'x': + self.w0[offset:offset+dims.nx] = value_.flatten() + elif field == 'u': + self.w0[offset+dims.nx:offset+dims.nx+dims.nu] = value_.flatten() def cost_get(self, stage_: int, field_: str) -> np.ndarray: raise NotImplementedError() diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 1be50c6967..bc507aa6d9 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2180,6 +2180,8 @@ def get_path_cost_expression(self): def get_terminal_cost_expression(self): model = self.model if self.cost.cost_type_e == "LINEAR_LS": + if is_empty(self.cost.Vx_e): + return 0.0 y = self.cost.Vx_e @ model.x residual = y - self.cost.yref_e cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) From 2d39c800836de0cc5d1789da13a5f3cd5ea1296b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 28 May 2025 21:09:14 +0200 Subject: [PATCH 055/164] Fix integrator specific initializations for MOCP in setter (#1535) --- .../matlab_templates/acados_mex_set.in.c | 110 +++++++++--------- 1 file changed, 58 insertions(+), 52 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c index 9e29f6fd10..3ae034dd3b 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c @@ -350,7 +350,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) acados_size = ocp_nlp_dims_get_total_from_attr(config, dims, out, field_name); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); offset = 0; - for (ii=0; ii<=N; ii++) // TODO implement set_all + for (ii=0; ii<=N; ii++) { ocp_nlp_cost_model_set(config, dims, in, ii, field_name, value+offset); tmp_int = ocp_nlp_dims_get_from_attr(config, dims, out, ii, field_name); @@ -399,93 +399,99 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) } else if (!strcmp(field, "init_z")||!strcmp(field, "z")) { - sim_solver_plan_t sim_plan = plan->sim_solver_plan[0]; + sim_solver_plan_t sim_plan = plan->sim_solver_plan[s0]; sim_solver_t type = sim_plan.sim_solver; - if (type == IRK) + if (nrhs == min_nrhs) { - if (nrhs == min_nrhs) + {% if problem_class == "MOCP" %} + MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field) + {% else %} + int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z"); + acados_size = N*nz; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (ii=0; iisim_solver_plan[0]; + sim_solver_plan_t sim_plan = plan->sim_solver_plan[s0]; sim_solver_t type = sim_plan.sim_solver; - if (type == IRK) + if (nrhs == min_nrhs) { + {% if problem_class == "MOCP" %} + MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field) + {% else %} int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x"); - if (nrhs == min_nrhs) + acados_size = N*nx; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (ii=0; iisim_solver_plan[0]; + sim_solver_plan_t sim_plan = plan->sim_solver_plan[s0]; sim_solver_t type = sim_plan.sim_solver; - if (type == GNSF) + if (nrhs == min_nrhs) { + {% if problem_class == "MOCP" %} + MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field) + {% else %} int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi"); - - if (nrhs == min_nrhs) + acados_size = N*nout; + MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); + for (ii=0; ii Date: Mon, 2 Jun 2025 14:22:24 +0200 Subject: [PATCH 056/164] Update BLASFEO (#1536) includes more efficient infinity computation https://github.com/giaf/blasfeo/pull/196 --- external/blasfeo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/blasfeo b/external/blasfeo index 9c2a8345f9..e33fe98acc 160000 --- a/external/blasfeo +++ b/external/blasfeo @@ -1 +1 @@ -Subproject commit 9c2a8345f9de37fbb4b3cd84810b0250810de622 +Subproject commit e33fe98accb757e7d46611ab3ad2cf65f6f96a6d From 754fd58cc78d4b867d8b93401239d777a5258a83 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 3 Jun 2025 14:33:57 +0200 Subject: [PATCH 057/164] Matlab slack checks (#1538) --- interfaces/acados_matlab_octave/AcadosOcp.m | 132 +++++++++++++++++++- 1 file changed, 126 insertions(+), 6 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index ba9eba1368..fd324c2a13 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -365,7 +365,7 @@ function make_consistent_constraints_terminal(self) dims.nh_e = nh_e; end - function make_consistent_slack_dimensions_path(self) + function make_consistent_slacks_path(self) constraints = self.constraints; dims = self.dims; cost = self.cost; @@ -373,11 +373,65 @@ function make_consistent_slack_dimensions_path(self) return end + nbx = dims.nbx; nsbx = length(constraints.idxsbx); + if nsbx > nbx + error(['inconsistent dimension nsbx = ', num2str(nsbx), '. Is greater than nbx = ', num2str(nbx), '.']); + end + if any(constraints.idxsbx >= nbx) + error(['idxsbx = [', num2str(constraints.idxsbx(:).'), '] contains value >= nbx = ', num2str(nbx), '.']); + end + if any(constraints.idxsbx < 0) + error(['idxsbx = [', num2str(constraints.idxsbx(:).'), '] contains value < 0.']); + end + nsbu = length(constraints.idxsbu); + nbu = dims.nbu; + if nsbu > nbu + error(['inconsistent dimension nsbu = ', num2str(nsbu), '. Is greater than nbu = ', num2str(nbu), '.']); + end + if any(constraints.idxsbu >= nbu) + error(['idxsbu = [', num2str(constraints.idxsbu(:).'), '] contains value >= nbu = ', num2str(nbu), '.']); + end + if any(constraints.idxsbu < 0) + error(['idxsbu = [', num2str(constraints.idxsbu(:).'), '] contains value < 0.']); + end + nsg = length(constraints.idxsg); + ng = dims.ng; + if nsg > ng + error(['inconsistent dimension nsg = ', num2str(nsg), '. Is greater than ng = ', num2str(ng), '.']); + end + if any(constraints.idxsg >= ng) + error(['idxsg = [', num2str(constraints.idxsg(:).'), '] contains value >= ng = ', num2str(ng), '.']); + end + if any(constraints.idxsg < 0) + error(['idxsg = [', num2str(constraints.idxsg(:).'), '] contains value < 0.']); + end + nsh = length(constraints.idxsh); + nh = dims.nh; + if nsh > nh + error(['inconsistent dimension nsh = ', num2str(nsh), '. Is greater than nh = ', num2str(nh), '.']); + end + if any(constraints.idxsh >= nh) + error(['idxsh = [', num2str(constraints.idxsh(:).'), '] contains value >= nh = ', num2str(nh), '.']); + end + if any(constraints.idxsh < 0) + error(['idxsh = [', num2str(constraints.idxsh(:).'), '] contains value < 0.']); + end + nsphi = length(constraints.idxsphi); + nphi = dims.nphi; + if nsphi > nphi + error(['inconsistent dimension nsphi = ', num2str(nsphi), '. Is greater than nphi = ', num2str(nphi), '.']); + end + if any(constraints.idxsphi >= nphi) + error(['idxsphi = [', num2str(constraints.idxsphi(:).'), '] contains value >= nphi = ', num2str(nphi), '.']); + end + if any(constraints.idxsphi < 0) + error(['idxsphi = [', num2str(constraints.idxsphi(:).'), '] contains value < 0.']); + end ns = nsbx + nsbu + nsg + nsh + nsphi; wrong_field = ''; @@ -426,7 +480,7 @@ function make_consistent_slack_dimensions_path(self) dims.nsphi = nsphi; end - function make_consistent_slack_dimensions_initial(self) + function make_consistent_slacks_initial(self) constraints = self.constraints; dims = self.dims; cost = self.cost; @@ -436,8 +490,29 @@ function make_consistent_slack_dimensions_initial(self) return end + nh_0 = dims.nh_0; nsh_0 = length(constraints.idxsh_0); + if nsh_0 > nh_0 + error(['inconsistent dimension nsh_0 = ', num2str(nsh_0), '. Is greater than nh_0 = ', num2str(nh_0), '.']); + end + if any(constraints.idxsh_0 >= nh_0) + error(['idxsh_0 = [', num2str(constraints.idxsh_0(:).'), '] contains value >= nh_0 = ', num2str(nh_0), '.']); + end + if any(constraints.idxsh_0 < 0) + error(['idxsh_0 = [', num2str(constraints.idxsh_0(:).'), '] contains value < 0.']); + end + + nphi_0 = dims.nphi_0; nsphi_0 = length(constraints.idxsphi_0); + if nsphi_0 > nphi_0 + error(['inconsistent dimension nsphi_0 = ', num2str(nsphi_0), '. Is greater than nphi_0 = ', num2str(nphi_0), '.']); + end + if any(constraints.idxsphi_0 >= nphi_0) + error(['idxsphi_0 = [', num2str(constraints.idxsphi_0(:).'), '] contains value >= nphi_0 = ', num2str(nphi_0), '.']); + end + if any(constraints.idxsphi_0 < 0) + error(['idxsphi_0 = [', num2str(constraints.idxsphi_0(:).'), '] contains value < 0.']); + end ns_0 = nsbu + nsg + nsh_0 + nsphi_0; wrong_field = ''; @@ -477,15 +552,60 @@ function make_consistent_slack_dimensions_initial(self) dims.nsphi_0 = nsphi_0; end - function make_consistent_slack_dimensions_terminal(self) + function make_consistent_slacks_terminal(self) constraints = self.constraints; dims = self.dims; cost = self.cost; + nbx_e = dims.nbx_e; nsbx_e = length(constraints.idxsbx_e); + if nsbx_e > nbx_e + error(['inconsistent dimension nsbx_e = ', num2str(nsbx_e), '. Is greater than nbx_e = ', num2str(nbx_e), '.']); + end + if any(constraints.idxsbx_e >= nbx_e) + error(['idxsbx_e = [', num2str(constraints.idxsbx_e(:).'), '] contains value >= nbx_e = ', num2str(nbx_e), '.']); + end + if any(constraints.idxsbx_e < 0) + error(['idxsbx_e = [', num2str(constraints.idxsbx_e(:).'), '] contains value < 0.']); + end + + ng_e = dims.ng_e; nsg_e = length(constraints.idxsg_e); + if nsg_e > ng_e + error(['inconsistent dimension nsg_e = ', num2str(nsg_e), '. Is greater than ng_e = ', num2str(ng_e), '.']); + end + if any(constraints.idxsg_e >= ng_e) + error(['idxsg_e = [', num2str(constraints.idxsg_e(:).'), '] contains value >= ng_e = ', num2str(ng_e), '.']); + end + if any(constraints.idxsg_e < 0) + error(['idxsg_e = [', num2str(constraints.idxsg_e(:).'), '] contains value < 0.']); + end + + nh_e = dims.nh_e; nsh_e = length(constraints.idxsh_e); + if nsh_e > nh_e + error(['inconsistent dimension nsh_e = ', num2str(nsh_e), '. Is greater than nh_e = ', num2str(nh_e), '.']); + end + if any(constraints.idxsh_e >= nh_e) + error(['idxsh_e = [', num2str(constraints.idxsh_e(:).'), '] contains value >= nh_e = ', num2str(nh_e), '.']); + end + if any(constraints.idxsh_e < 0) + error(['idxsh_e = [', num2str(constraints.idxsh_e(:).'), '] contains value < 0.']); + end + + + nphi_e = dims.nphi_e; nsphi_e = length(constraints.idxsphi_e); + if nsphi_e > nphi_e + error(['inconsistent dimension nsphi_e = ', num2str(nsphi_e), '. Is greater than nphi_e = ', num2str(nphi_e), '.']); + end + if any(constraints.idxsphi_e >= nphi_e) + error(['idxsphi_e = [', num2str(constraints.idxsphi_e(:).'), '] contains value >= nphi_e = ', num2str(nphi_e), '.']); + end + if any(constraints.idxsphi_e < 0) + error(['idxsphi_e = [', num2str(constraints.idxsphi_e(:).'), '] contains value < 0.']); + end + ns_e = nsbx_e + nsg_e + nsh_e + nsphi_e; wrong_field = ''; @@ -747,9 +867,9 @@ function make_consistent(self, is_mocp_phase) self.make_consistent_constraints_terminal(); %% slack dimensions - self.make_consistent_slack_dimensions_path(); - self.make_consistent_slack_dimensions_initial(); - self.make_consistent_slack_dimensions_terminal(); + self.make_consistent_slacks_path(); + self.make_consistent_slacks_initial(); + self.make_consistent_slacks_terminal(); % check for ACADOS_INFTY if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) From 406174774ae44f381ed1fc639237f856417c23c3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 3 Jun 2025 17:38:25 +0200 Subject: [PATCH 058/164] fix `N_horizon` check in templates (#1539) `N_horizon` should get the default value 1 in templates only if we render them to get a sim solver. Previously 0 was overwritten by 1 there. --- .../acados_template/c_templates_tera/CMakeLists.in.txt | 2 +- .../acados_template/c_templates_tera/Makefile.in | 2 +- .../acados_template/acados_template/c_templates_tera/main.in.c | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 950eb9cae0..a9e728f174 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -34,7 +34,7 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} -{%- if solver_options.N_horizon %} +{% if problem_class != "SIM" %} {%- set N_horizon = solver_options.N_horizon %} {%- else %} {%- set N_horizon = 1 %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index 7f074d2010..041326c02f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -35,7 +35,7 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} -{%- if solver_options.N_horizon %} +{% if problem_class != "SIM" %} {%- set N_horizon = solver_options.N_horizon %} {%- else %} {%- set N_horizon = 1 %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/main.in.c b/interfaces/acados_template/acados_template/c_templates_tera/main.in.c index 5c99728bfb..74ac68a8dd 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/main.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/main.in.c @@ -77,6 +77,7 @@ int main() ocp_nlp_solver *nlp_solver = {{ name }}_acados_get_nlp_solver(acados_ocp_capsule); void *nlp_opts = {{ name }}_acados_get_nlp_opts(acados_ocp_capsule); +{%- if dims.nbx_0 > 0 %} // initial condition double lbx0[NBX0]; double ubx0[NBX0]; @@ -87,6 +88,7 @@ int main() ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); +{%- endif %} // initialization for state values double x_init[NX]; From 27f0fe07de900f6ea300225d58fada3a6b2d6941 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 3 Jun 2025 18:06:30 +0200 Subject: [PATCH 059/164] Fix compatibility of full condensing and SQP-WFQP (#1540) Make full condensing behave like partial one in terms of when dims need to be provided. Added corresponding test. Additionally: Implemented `AcadosOcpFlattenedIterate.allclose()` for testing. --- acados/ocp_qp/ocp_qp_full_condensing.c | 10 +- .../hs015_test.py | 0 .../hock_schittkowsky/hs016_test.py | 118 ++++++++++++++++++ .../linear_mass_model/sqp_wfqp_test.py | 5 +- interfaces/CMakeLists.txt | 7 +- .../acados_template/acados_ocp_iterate.py | 13 ++ 6 files changed, 141 insertions(+), 12 deletions(-) rename examples/acados_python/{nlp_hs015 => hock_schittkowsky}/hs015_test.py (100%) create mode 100644 examples/acados_python/hock_schittkowsky/hs016_test.py diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index b2c76290fe..904145319a 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -157,11 +157,7 @@ acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims_) // populate dimensions of reduced qp d_ocp_qp_dim_reduce_eq_dof(dims->orig_dims, dims->red_dims); // populate dimensions of new dense_qp -// d_cond_qp_compute_dim(dims->orig_dims, dims->fcond_dims); d_cond_qp_compute_dim(dims->red_dims, dims->fcond_dims); -//d_ocp_qp_dim_print(dims->orig_dims); -//d_ocp_qp_dim_print(dims->red_dims); -//exit(1); acados_size_t size = 0; @@ -304,12 +300,10 @@ acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims_, void *op ocp_qp_full_condensing_dims *dims = dims_; ocp_qp_full_condensing_opts *opts = opts_; - // TODO needed ??? // populate dimensions of reduced qp -// d_ocp_qp_dim_reduce_eq_dof(dims->orig_dims, dims->red_dims); + d_ocp_qp_dim_reduce_eq_dof(dims->orig_dims, dims->red_dims); // populate dimensions of new dense_qp -// d_cond_qp_compute_dim(dims->orig_dims, dims->fcond_dims); -// d_cond_qp_compute_dim(dims->red_dims, dims->fcond_dims); + d_cond_qp_compute_dim(dims->red_dims, dims->fcond_dims); acados_size_t size = 0; diff --git a/examples/acados_python/nlp_hs015/hs015_test.py b/examples/acados_python/hock_schittkowsky/hs015_test.py similarity index 100% rename from examples/acados_python/nlp_hs015/hs015_test.py rename to examples/acados_python/hock_schittkowsky/hs015_test.py diff --git a/examples/acados_python/hock_schittkowsky/hs016_test.py b/examples/acados_python/hock_schittkowsky/hs016_test.py new file mode 100644 index 0000000000..28291471fa --- /dev/null +++ b/examples/acados_python/hock_schittkowsky/hs016_test.py @@ -0,0 +1,118 @@ +# +# 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.; +# + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, ACADOS_INFTY, AcadosOcpFlattenedIterate +import numpy as np +from casadi import * +from matplotlib import pyplot as plt +from itertools import product + + +def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM'): + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = AcadosModel() + x = SX.sym('x', 2) + + # dynamics: identity + model.disc_dyn_expr = x + model.x = x + model.name = f'hs_016' + ocp.model = model + + + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = 100*(x[1] - x[0]**2)**2 + (1 - x[0])**2 + + # constraints + ocp.model.con_h_expr_e = vertcat(x[0]**2 + x[1], x[0] + x[1]**2) + ocp.constraints.lh_e = np.array([0.0, 0.0]) + ocp.constraints.uh_e = np.array([ACADOS_INFTY, ACADOS_INFTY]) + + # add bounds on x; + ocp.constraints.idxbx_e = np.arange(2) + ocp.constraints.ubx_e = np.array([0.5, 1.0]) + ocp.constraints.lbx_e = np.array([-0.5, -ACADOS_INFTY]) + + # set options + ocp.solver_options.N_horizon = 0 + ocp.solver_options.qp_solver = qp_solver + ocp.solver_options.qp_solver_mu0 = 1e3 + ocp.solver_options.hessian_approx = 'EXACT' + ocp.solver_options.regularize_method = 'MIRROR' + ocp.solver_options.print_level = 1 + ocp.solver_options.nlp_solver_max_iter = 1000 + ocp.solver_options.qp_solver_iter_max = 1000 + + # Search direction + ocp.solver_options.nlp_solver_type = 'SQP_WITH_FEASIBLE_QP' + + # Globalization + ocp.solver_options.globalization = 'FUNNEL_L1PEN_LINESEARCH' + ocp.solver_options.globalization_full_step_dual = True + ocp.solver_options.globalization_funnel_use_merit_fun_only = False + + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + + # initialize solver + xinit = np.array([-2, 1]) + ocp_solver.set(0, "x", xinit) + + # solve + status = ocp_solver.solve() + + # get solution + assert status == 0, f"Solver failed with status {status}" + + sol = ocp_solver.store_iterate_to_flat_obj() + + return sol + + +def main(): + sol_list = [] + for qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']: + print(f"Solving with {qp_solver}...") + sol = solve_problem(qp_solver) + sol_list.append(sol) + + ref_sol = sol_list[0] + for i, sol in enumerate(sol_list[1:]): + if not AcadosOcpFlattenedIterate.allclose(ref_sol, sol): + raise ValueError(f"Solution does not match reference close enough!") + else: + print(f"Solution {i+1} matches reference solution.") + +if __name__ == '__main__': + main() diff --git a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py index f29a0387ef..7c60b3c45d 100644 --- a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py +++ b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py @@ -28,11 +28,10 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, ACADOS_INFTY +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY import numpy as np import scipy.linalg -from linear_mass_model import * -from itertools import product +from linear_mass_model import export_linear_mass_model, plot_linear_mass_system_X_state_space # an OCP to test the behavior of the SQP_WITH_FEASIBLE_QP functionalities diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 355892b488..60421f9449 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -233,9 +233,14 @@ add_test(NAME py_sqp_wfqp_inconsistent_linearization python inconsistent_qp_linearization_test.py) add_test(NAME py_sqp_wfqp_problem_hs015 - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/nlp_hs015 + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky python hs015_test.py) + +add_test(NAME py_sqp_wfqp_problem_hs016 + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky + python hs016_test.py) + # CMake test add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp diff --git a/interfaces/acados_template/acados_template/acados_ocp_iterate.py b/interfaces/acados_template/acados_template/acados_ocp_iterate.py index a892d95ffa..e13675569e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_iterate.py +++ b/interfaces/acados_template/acados_template/acados_ocp_iterate.py @@ -48,6 +48,19 @@ class AcadosOcpFlattenedIterate: pi: np.ndarray lam: np.ndarray + def allclose(self, other, rtol=1e-5, atol=1e-8) -> bool: + if not isinstance(other, AcadosOcpFlattenedIterate): + raise TypeError(f"Expected AcadosOcpFlattenedIterate, got {type(other)}") + return ( + np.allclose(self.x, other.x, rtol=rtol, atol=atol) and + np.allclose(self.u, other.u, rtol=rtol, atol=atol) and + np.allclose(self.z, other.z, rtol=rtol, atol=atol) and + np.allclose(self.sl, other.sl, rtol=rtol, atol=atol) and + np.allclose(self.su, other.su, rtol=rtol, atol=atol) and + np.allclose(self.pi, other.pi, rtol=rtol, atol=atol) and + np.allclose(self.lam, other.lam, rtol=rtol, atol=atol) + ) + @dataclass class AcadosOcpFlattenedBatchIterate: From b4f13f670a4cd96f7e7878df77372d1cdc9861c2 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 4 Jun 2025 16:29:48 +0200 Subject: [PATCH 060/164] Fix QP residual computation taking masks into account (#1537) --- acados/dense_qp/dense_qp_common.c | 14 ++++++++++++++ acados/ocp_qp/ocp_qp_common.c | 23 +++++++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 705f621e89..a5d28d2df8 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -380,6 +380,20 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res // compute residuals d_dense_qp_res_compute(qp_in, qp_out, qp_res, res_ws); + + // mask out disregarded constraints + int nb = qp_res->dim->nb; + int nv = qp_res->dim->nv; + int ng = qp_res->dim->ng; + int ns = qp_res->dim->ns; + int ni = nb + ng + ns; + + // stationarity wrt slacks + blasfeo_dvecmul(2*ns, qp_in->d_mask, 2*nb+2*ng, qp_res->res_g, nv, qp_res->res_g, nv); + // ineq + blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_d, 0, qp_res->res_d, 0); + // comp + blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_m, 0, qp_res->res_m, 0); } diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 6fd0f91ad4..e21e3b25a5 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -167,6 +167,10 @@ void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, in } +static int ocp_qp_dims_get_ni(ocp_qp_dims *dims, int stage) +{ + return dims->nbu[stage] + dims->nbx[stage] + dims->ng[stage] + dims->ns[stage]; +} /************************************************ * in @@ -366,6 +370,13 @@ ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws) { + int *nx = qp_res->dim->nx; + int *nu = qp_res->dim->nu; + int *nb = qp_res->dim->nb; + int *ng = qp_res->dim->ng; + int *ns = qp_res->dim->ns; + int ni_stage; + qp_info *info = (qp_info *) qp_out->misc; if (info->t_computed == 0) @@ -376,6 +387,17 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res d_ocp_qp_res_compute(qp_in, qp_out, qp_res, res_ws); + // mask out disregarded constraints + for (int ii = 0; ii <= qp_res->dim->N; ii++) + { + ni_stage = ocp_qp_dims_get_ni(qp_res->dim, ii); + // stationarity wrt slacks + blasfeo_dvecmul(2*ns[ii], qp_in->d_mask+ii, 2*nb[ii]+2*ng[ii], &qp_res->res_g[ii], nx[ii]+nu[ii], &qp_res->res_g[ii], nx[ii]+nu[ii]); + // ineq + blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_d[ii], 0, &qp_res->res_d[ii], 0); + // comp + blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_m[ii], 0, &qp_res->res_m[ii], 0); + } return; } @@ -396,6 +418,7 @@ void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]) #if 1 + // compute infinity norms of residuals double tmp; res[0] = 0.0; From be57f1abd747857c34c20ef60059620a2f410a9e Mon Sep 17 00:00:00 2001 From: Jingtao <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 4 Jun 2025 16:31:43 +0200 Subject: [PATCH 061/164] Get & set Lagrange multipliers in `AcadosCasadiOcpSolver` (#1534) - Investigated the relation between lagrange multiplier in acados and casadi, which is $\lambda_{aca}^u-\lambda_{cas}^l=\lambda_{cas}$ - Setup index list for locating variables in solution - Added checks on supported features: no algebraic variables and slacks yet - Completed `get(), get_flat(), set(), set_flat()` functions - Completed save and load functions and tested in `test_casdi_formulation.py` --------- Co-authored-by: Jonathan Frey --- .../ocp/test_casadi_formulation.py | 41 +-- .../acados_casadi_ocp_solver.py | 260 +++++++++++++++--- .../acados_template/acados_ocp_solver.py | 1 - 3 files changed, 234 insertions(+), 68 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py index f913747be7..5e667f88aa 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -11,27 +11,9 @@ from utils import plot_pendulum -def get_x_u_traj(ocp_solver: Union[AcadosOcpSolver, AcadosCasadiOcpSolver], N_horizon: int): - ocp = ocp_solver.acados_ocp - simX = np.zeros((N_horizon+1, ocp.dims.nx)) - simU = np.zeros((N_horizon, ocp.dims.nu)) - for i in range(N_horizon): - simX[i,:] = ocp_solver.get(i, "x") - simU[i,:] = ocp_solver.get(i, "u") - simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") - - return simX, simU - - def main(): ocp = formulate_ocp("CONL") ocp.solver_options.tf = T_HORIZON - N_horizon = ocp.solver_options.N_horizon - - ## solve using casadi - casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) - casadi_ocp_solver.solve() - x_casadi_sol, u_casadi_sol = get_x_u_traj(casadi_ocp_solver, N_horizon) initial_iterate = ocp.create_default_initial_iterate() @@ -43,21 +25,26 @@ def main(): # solve with acados status = ocp_solver.solve() # get solution - simX, simU = get_x_u_traj(ocp_solver, N_horizon) + result_acados = ocp_solver.store_iterate_to_obj() + result_acados_flat = ocp_solver.store_iterate_to_flat_obj() + ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result_acados) + casadi_ocp_solver.solve() + result_casadi = casadi_ocp_solver.store_iterate_to_obj() + result_casadi_flat = casadi_ocp_solver.store_iterate_to_flat_obj() # evaluate difference - diff_x = np.linalg.norm(x_casadi_sol - simX) - print(f"Difference between casadi and acados solution: {diff_x}") - diff_u = np.linalg.norm(u_casadi_sol - simU) - print(f"Difference between casadi and acados solution: {diff_u}") - # TODO: set solver tolerance and reduce it here. test_tol = 5e-5 - if diff_x > test_tol or diff_u > test_tol: - raise ValueError(f"Test failed: difference between casadi and acados solution should be smaller than {test_tol}, but got {diff_x} and {diff_u}.") + for field in ["x", "u", "pi", "lam"]: + diff = np.linalg.norm(getattr(result_acados_flat, field) - getattr(result_casadi_flat, field)) + print(f"Difference between acados and casadi solution for {field:4}: {diff:.3e}") + if diff > test_tol: + raise ValueError(f"Test failed: difference between acados and casadi solution for {field} should be smaller than {test_tol}, but got {diff}.") - plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu, u_casadi_sol, x_casadi_sol, latexify=False) + # plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu, u_casadi_sol, x_casadi_sol, latexify=False) if __name__ == "__main__": diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 82e0088972..6b7143d521 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -55,6 +55,27 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n constraints = ocp.constraints solver_options = ocp.solver_options + # check what is not supported yet + if any([dims.ns_0, dims.ns, dims.ns_e]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support soft constraints yet.") + if dims.nz > 0: + raise NotImplementedError("AcadosCasadiOcpSolver does not support algebraic variables (z) yet.") + if any([dims.ng, dims.ng_e]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support general nonlinear constraints (g) yet.") + if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: + raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") + + # create index map for variables + index_map = { + # indices of variables within w + 'x_in_w': [], + 'u_in_w': [], + # indices of dynamic constraints within g in casadi formulation + 'pi_in_lam_g': [], + # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation + 'lam_gnl_in_lam_g': [] + } + if any([dims.ns_0, dims.ns, dims.ns_e]): raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") @@ -64,6 +85,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n utraj_node = [ca_symbol(f'u{i}', dims.nu, 1) for i in range(solver_options.N_horizon)] if dims.nz > 0: raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") + # parameters ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(solver_options.N_horizon)] @@ -73,6 +95,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(solver_options.N_horizon+1)] lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 + offset = 0 for i in range(1, solver_options.N_horizon): lb_xtraj_node[i][constraints.idxbx] = constraints.lbx ub_xtraj_node[i][constraints.idxbx] = constraints.ubx @@ -90,6 +113,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g = [] lbg = [] ubg = [] + offset = 0 # dynamics if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) @@ -103,17 +127,20 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n for i in range(solver_options.N_horizon): # add dynamics constraints if solver_options.integrator_type == "DISCRETE": - g.apppend(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + g.append(f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - xtraj_node[i+1]) elif solver_options.integrator_type == "ERK": - g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i])) + g.append(f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i]) - xtraj_node[i+1]) lbg.append(np.zeros((dims.nx, 1))) ubg.append(np.zeros((dims.nx, 1))) + index_map['pi_in_lam_g'].append(list(range(offset, offset+dims.nx))) + offset += dims.nx # nonlinear constraints -- initial stage h0_fun = ca.Function('h0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) g.append(h0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) lbg.append(constraints.lh_0) ubg.append(constraints.uh_0) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_0))) if dims.nphi_0 > 0: conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) @@ -121,6 +148,8 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) lbg.append(constraints.lphi_0) ubg.append(constraints.uphi_0) + index_map['lam_gnl_in_lam_g'][-1]=list(range(offset, offset+dims.nh_0+dims.nphi_0)) + offset += dims.nh_0 + dims.nphi_0 # nonlinear constraints -- intermediate stages h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) @@ -133,11 +162,14 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g.append(h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) lbg.append(constraints.lh) ubg.append(constraints.uh) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh))) if dims.nphi > 0: g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) lbg.append(constraints.lphi) ubg.append(constraints.uphi) + index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh+dims.nphi)) + offset += dims.nphi + dims.nh # nonlinear constraints -- terminal stage h_e_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) @@ -145,6 +177,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g.append(h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) lbg.append(constraints.lh_e) ubg.append(constraints.uh_e) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_e))) if dims.nphi_e > 0: conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) @@ -152,6 +185,8 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) lbg.append(constraints.lphi_e) ubg.append(constraints.uphi_e) + index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh_e+dims.nphi_e)) + offset += dims.nh_e + dims.nphi_e ### Cost # initial cost term @@ -178,6 +213,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n lbw_list = [] ubw_list = [] w0_list = [] + offset = 0 x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) for i in range(solver_options.N_horizon): # add x @@ -185,17 +221,23 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n lbw_list.append(lb_xtraj_node[i]) ubw_list.append(ub_xtraj_node[i]) w0_list.append(x_guess) + index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) + offset += dims.nx # add u w_sym_list.append(utraj_node[i]) lbw_list.append(lb_utraj_node[i]) ubw_list.append(ub_utraj_node[i]) w0_list.append(np.zeros((dims.nu,))) + index_map['u_in_w'].append(list(range(offset, offset + dims.nu))) + offset += dims.nu ## terminal stage # add x w_sym_list.append(xtraj_node[-1]) lbw_list.append(lb_xtraj_node[-1]) ubw_list.append(ub_xtraj_node[-1]) w0_list.append(x_guess) + index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) + offset += dims.nx # vectorize w = ca.vertcat(*w_sym_list) @@ -208,7 +250,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} w0 = np.concatenate(w0_list) - return nlp, bounds, w0 + return nlp, bounds, w0, index_map def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): @@ -217,13 +259,15 @@ def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): raise TypeError('acados_ocp should be of type AcadosOcp.') self.acados_ocp = acados_ocp - self.casadi_nlp, self.bounds, self.w0 = self.create_casadi_nlp_formulation(acados_ocp) + self.casadi_nlp, self.bounds, self.w0, self.index_map = self.create_casadi_nlp_formulation(acados_ocp) self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp) + self.lam_x0 = np.empty(self.casadi_nlp['x'].shape).flatten() + self.lam_g0 = np.empty(self.casadi_nlp['g'].shape).flatten() self.nlp_sol = None def solve_for_x0(self, x0_bar, fail_on_nonzero_status=True, print_stats_on_failure=True): - raise NotImplementedError() + raise NotImplementedError() def solve(self) -> int: @@ -233,9 +277,13 @@ def solve(self) -> int: :return: status of the solver """ self.nlp_sol = self.casadi_solver(x0=self.w0, + lam_g0=self.lam_g0, lam_x0=self.lam_x0, lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], - lbg=self.bounds['lbg'], ubg=self.bounds['ubg']) - self.nlp_sol_x = self.nlp_sol['x'].full() + lbg=self.bounds['lbg'], ubg=self.bounds['ubg'] + ) + self.nlp_sol_w = self.nlp_sol['x'].full() + self.nlp_sol_lam_g = self.nlp_sol['lam_g'].full() + self.nlp_sol_lam_x = self.nlp_sol['lam_x'].full() # TODO: return correct status return 0 @@ -248,37 +296,49 @@ def get_dim_flat(self, field: str): raise NotImplementedError() - - def get(self, stage: int, field: str): """ Get the last solution of the solver. :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'sens_u', 'sens_pi', 'sens_x', 'sens_lam', 'sens_sl', 'sens_su'] - - .. note:: regarding lam: \n - the inequalities are internally organized in the following order: \n - [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n - lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] - - .. note:: pi: multipliers for dynamics equality constraints \n - lam: multipliers for inequalities \n - t: slack variables corresponding to evaluation of all inequalities (at the solution) \n - sl: slack variables of soft lower inequality constraints \n - su: slack variables of soft upper inequality constraints \n + :param field: string in ['x', 'u', 'pi', 'lam'] + """ if not isinstance(stage, int): raise TypeError('stage should be integer.') if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') dims = self.acados_ocp.dims - offset = stage*(dims.nx+dims.nu) - if field == 'x': - return self.nlp_sol_x[offset:offset+dims.nx].flatten() + return self.nlp_sol_w[self.index_map['x_in_w'][stage]].flatten() elif field == 'u': - return self.nlp_sol_x[offset+dims.nx:offset+dims.nx+dims.nu].flatten() + return self.nlp_sol_w[self.index_map['u_in_w'][stage]].flatten() + elif field == 'pi': + return self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() + elif field == 'lam': + if stage == 0: + bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_0 else np.empty((0, 1)) + bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] + elif stage < dims.N: + bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx else np.empty((0, 1)) + bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] + elif stage == dims.N: + bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_e else np.empty((0, 1)) + bu_lam = np.empty((0, 1)) + g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] + + lbx_lam = np.maximum(0, -bx_lam) + lbu_lam = np.maximum(0, -bu_lam) + lg_lam = np.maximum(0, -g_lam) + ubx_lam = np.maximum(0, bx_lam) + ubu_lam = np.maximum(0, bu_lam) + ug_lam = np.maximum(0, g_lam) + lam = np.concatenate((lbu_lam, lbx_lam, lg_lam, ubu_lam, ubx_lam, ug_lam)) + return lam.flatten() + elif field in ['sl', 'su', 'z']: + return np.empty((0, 1)) # Only empty is supported for now. TODO: extend. else: raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") @@ -286,37 +346,117 @@ def get_flat(self, field_: str) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p', 'p_global'] + :param field: string in ['x', 'u', 'pi', 'lam', 'z', 'sl', 'su'] .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n """ - raise NotImplementedError() - + if self.nlp_sol is None: + raise ValueError('No solution available. Please call solve() first.') + dims = self.acados_ocp.dims + result = [] + + if field_ in ['x', 'lam', 'sl', 'su']: + for i in range(dims.N+1): + result.append(self.get(i, field_)) + return np.concatenate(result) + elif field_ in ['u', 'pi', 'z']: + for i in range(dims.N): + result.append(self.get(i, field_)) + return np.concatenate(result) + # casadi variables. TODO: maybe remove this. + elif field_ == 'lam_x': + return self.nlp_sol_lam_x.flatten() + elif field_ == 'lam_g': + return self.nlp_sol_lam_g.flatten() + elif field_ == 'lam_p': + return self.nlp_sol['lam_p'].full().flatten() + else: + raise NotImplementedError(f"Field '{field_}' is not implemented in get_flat().") def set_flat(self, field_: str, value_: np.ndarray) -> None: """ Set concatenation solver initialization. - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] + :param field: string in ['x', 'u', 'lam', pi] """ - raise NotImplementedError() - + dims = self.acados_ocp.dims + if field_ == 'x': + for i in range(dims.N+1): + self.set(i, 'x', value_[i*dims.nx:(i+1)*dims.nx]) + elif field_ == 'u': + for i in range(dims.N): + self.set(i, 'u', value_[i*dims.nu:(i+1)*dims.nu]) + elif field_ == 'pi': + for i in range(dims.N): + self.set(i, 'pi', value_[i*dims.nx:(i+1)*dims.nx]) + elif field_ == 'lam': + offset = 0 + for i in range(dims.N+1): + if i == 0: + self.set(i, 'lam', value_[offset:offset+2*(dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0)]) + offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + elif i < dims.N: + self.set(i, 'lam', value_[offset:offset+2*(dims.nbx+dims.nbu+dims.ng+dims.nh+dims.nphi)]) + offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + elif i == dims.N: + self.set(i, 'lam', value_[offset:offset+2*(dims.nbx_e+dims.ng_e+dims.nh_e+dims.nphi_e)]) + offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + else: + raise NotImplementedError(f"Field '{field_}' is not yet implemented in set_flat().") def load_iterate(self, filename:str, verbose: bool = True): raise NotImplementedError() def store_iterate_to_obj(self) -> AcadosOcpIterate: - raise NotImplementedError() + """ + Returns the current iterate of the OCP solver as an AcadosOcpIterate. + """ + d = {} + for field in ["x", "u", "z", "sl", "su", "pi", "lam"]: + traj = [] + for n in range(self.acados_ocp.dims.N+1): + if n < self.acados_ocp.dims.N or not (field in ["u", "pi", "z"]): + traj.append(self.get(n, field)) - def load_iterate_from_obj(self, iterate: AcadosOcpIterate): - raise NotImplementedError() + d[f"{field}_traj"] = traj + + return AcadosOcpIterate(**d) + + def load_iterate_from_obj(self, iterate: AcadosOcpIterate) -> None: + """ + Loads the provided iterate into the OCP solver. + Note: The iterate object does not contain the the parameters. + """ + # TODO: add slacks + for key, traj in iterate.__dict__.items(): + field = key.replace('_traj', '') + + for n, val in enumerate(traj): + if field in ['x', 'u', 'pi', 'lam']: + self.set(n, field, val) def store_iterate_to_flat_obj(self) -> AcadosOcpFlattenedIterate: - raise NotImplementedError() + """ + Returns the current iterate of the OCP solver as an AcadosOcpFlattenedIterate. + """ + return AcadosOcpFlattenedIterate(x = self.get_flat("x"), + u = self.get_flat("u"), + pi = self.get_flat("pi"), + lam = self.get_flat("lam"), + sl = self.get_flat("sl"), + su = self.get_flat("su"), + z = self.get_flat("z")) def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedIterate) -> None: - raise NotImplementedError() + """ + Loads the provided iterate into the OCP solver. + Note: The iterate object does not contain the the parameters. + """ + self.set_flat("x", iterate.x) + self.set_flat("u", iterate.u) + self.set_flat("pi", iterate.pi) + self.set_flat("lam", iterate.lam) def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: raise NotImplementedError() @@ -325,16 +465,56 @@ def get_cost(self) -> float: raise NotImplementedError() def set(self, stage: int, field: str, value_: np.ndarray): + """ + Set solver initialization to stages. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam'] + :value_: + """ dims = self.acados_ocp.dims - offset = stage*(dims.nx+dims.nu) if field == 'x': - self.w0[offset:offset+dims.nx] = value_.flatten() + self.w0[self.index_map['x_in_w'][stage]] = value_.flatten() elif field == 'u': - self.w0[offset+dims.nx:offset+dims.nx+dims.nu] = value_.flatten() + self.w0[self.index_map['u_in_w'][stage]] = value_.flatten() + elif field == 'pi': + self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = value_.flatten() + elif field == 'lam': + if stage == 0: + bx_length = dims.nbx_0 + bu_length = dims.nbu + h_length = dims.ng + dims.nh_0 + dims.nphi_0 + elif stage < dims.N: + bx_length = dims.nbx + bu_length = dims.nbu + h_length = dims.ng + dims.nh + dims.nphi + elif stage == dims.N: + bx_length = dims.nbx_e + bu_length = 0 + h_length = dims.ng_e + dims.nh_e + dims.nphi_e + + offset_u = (bx_length+bu_length+h_length) + lbu_lam = value_[:bu_length] if bu_length else np.empty((dims.nu,)) + lbx_lam = value_[bu_length:bu_length+bx_length] if bx_length else np.empty((dims.nx,)) + lg_lam = value_[bu_length+bx_length:bu_length+bx_length+h_length] + ubu_lam = value_[offset_u:offset_u+bu_length] if bu_length else np.empty((dims.nu,)) + ubx_lam = value_[offset_u+bu_length:offset_u+bu_length+bx_length] if bx_length else np.empty((dims.nx,)) + ug_lam = value_[offset_u+bu_length+bx_length:offset_u+bu_length+bx_length+h_length] + if stage != dims.N: + self.lam_x0[self.index_map['x_in_w'][stage]+self.index_map['u_in_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam + else: + self.lam_x0[self.index_map['x_in_w'][stage]] = ubx_lam-lbx_lam + self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam + elif field in ['sl', 'su']: + # do nothing for now, only empty is supported + pass + else: + raise NotImplementedError(f"Field '{field}' is not yet implemented in set().") def cost_get(self, stage_: int, field_: str) -> np.ndarray: raise NotImplementedError() def cost_set(self, stage_: int, field_: str, value_): - raise NotImplementedError() \ No newline at end of file + raise NotImplementedError() diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index ce8f33df11..aea347fb1c 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1022,7 +1022,6 @@ def get(self, stage_: int, field_: str): .. note:: pi: multipliers for dynamics equality constraints \n lam: multipliers for inequalities \n - t: slack variables corresponding to evaluation of all inequalities (at the solution) \n sl: slack variables of soft lower inequality constraints \n su: slack variables of soft upper inequality constraints \n """ From 59dc7853ebf23715c8767268cbf40948ba1829bb Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 5 Jun 2025 10:49:05 +0200 Subject: [PATCH 062/164] Fix Hock Schittkowsky test on actions (#1542) by using different `code_export_directory` --- examples/acados_python/hock_schittkowsky/hs015_test.py | 1 + examples/acados_python/hock_schittkowsky/hs016_test.py | 2 ++ 2 files changed, 3 insertions(+) diff --git a/examples/acados_python/hock_schittkowsky/hs015_test.py b/examples/acados_python/hock_schittkowsky/hs015_test.py index 6e41dfad72..31bc7fc4ca 100644 --- a/examples/acados_python/hock_schittkowsky/hs015_test.py +++ b/examples/acados_python/hock_schittkowsky/hs015_test.py @@ -118,6 +118,7 @@ def solve_infeasible_linearization(setting): ocp.solver_options.search_direction_mode = "BYRD_OMOJOKUN" ocp.solver_options.use_constraint_hessian_in_feas_qp = False + ocp.code_export_directory = f'c_generated_code_{model.name}' ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver diff --git a/examples/acados_python/hock_schittkowsky/hs016_test.py b/examples/acados_python/hock_schittkowsky/hs016_test.py index 28291471fa..e5311f4b3b 100644 --- a/examples/acados_python/hock_schittkowsky/hs016_test.py +++ b/examples/acados_python/hock_schittkowsky/hs016_test.py @@ -83,6 +83,8 @@ def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM'): ocp.solver_options.globalization_full_step_dual = True ocp.solver_options.globalization_funnel_use_merit_fun_only = False + ocp.code_export_directory = f'c_generated_code_{model.name}' + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver From 59560bd1ce176cf7c67ea39643af3f1373b5aaeb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20K=C3=BCbel?= <89879541+johanneskbl@users.noreply.github.com> Date: Thu, 5 Jun 2025 17:58:41 +0900 Subject: [PATCH 063/164] [docs] Fix mistake in doc string of acados sim object for num of stages (#1541) The number of stages is per default 4 and not 1 as stated in the doc string. Please look at the screenshot ([or code](https://github.com/acados/acados/blob/main/interfaces/acados_template/acados_template/acados_sim.py#L82)) to confirm. To me this was a bit surprising since I tried to play around with this property. ![Screenshot from 2025-06-05 14-50-26](https://github.com/user-attachments/assets/adbdad20-7629-436a-8aad-b2a1954ef2b6) --- interfaces/acados_template/acados_template/acados_sim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index e35d2caba5..f6ba29fd11 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -78,7 +78,7 @@ def integrator_type(self): @property def num_stages(self): - """Number of stages in the integrator. Default: 1""" + """Number of stages in the integrator. Default: 4""" return self.__sim_method_num_stages @property From 41cfb3e3167e9b99fdfa357cd1f06f27849ce35f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 5 Jun 2025 14:12:15 +0200 Subject: [PATCH 064/164] Extend `AcadosCasadiOcpSolver` with options and support fatrop (#1543) --- .../ocp/ocp_example_cost_formulations.py | 16 +- .../ocp/test_casadi_formulation.py | 34 ++++- .../acados_casadi_ocp_solver.py | 139 ++++++++++-------- 3 files changed, 119 insertions(+), 70 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py index 074c41b425..8e0fea3403 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py @@ -49,7 +49,7 @@ NU = 1 FMAX = 80 -def formulate_ocp(cost_version: str) -> AcadosOcp: +def formulate_ocp(cost_version: str, constraint_version="bu") -> AcadosOcp: # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -233,10 +233,18 @@ def formulate_ocp(cost_version: str) -> AcadosOcp: raise Exception('Unknown cost_version.') # set constraints - ocp.constraints.lbu = np.array([-FMAX]) - ocp.constraints.ubu = np.array([+FMAX]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) - ocp.constraints.idxbu = np.array([0]) + if constraint_version == "bu": + ocp.constraints.lbu = np.array([-FMAX]) + ocp.constraints.ubu = np.array([+FMAX]) + ocp.constraints.idxbu = np.array([0]) + elif constraint_version == "h": + ocp.constraints.lh = np.array([-FMAX]) + ocp.constraints.uh = np.array([+FMAX]) + ocp.model.con_h_expr = ocp.model.u[0] # h = u[0] + ocp.constraints.lh_0 = np.array([-FMAX]) + ocp.constraints.uh_0 = np.array([+FMAX]) + ocp.model.con_h_expr_0 = ocp.model.u[0] # h = u[0] return ocp diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py index 5e667f88aa..6c7e7708ad 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -7,12 +7,16 @@ from typing import Union from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver -from ocp_example_cost_formulations import formulate_ocp, T_HORIZON +from ocp_example_cost_formulations import formulate_ocp, T_HORIZON, FMAX from utils import plot_pendulum -def main(): - ocp = formulate_ocp("CONL") +def raise_test_failure_message(msg: str): + # print(f"ERROR: {msg}") + raise Exception(msg) + +def main(cost_version="CONL", constraint_version='h', casadi_solver_name="ipopt"): + ocp = formulate_ocp(cost_version=cost_version, constraint_version=constraint_version) ocp.solver_options.tf = T_HORIZON initial_iterate = ocp.create_default_initial_iterate() @@ -24,13 +28,22 @@ def main(): ocp_solver.load_iterate_from_obj(initial_iterate) # solve with acados status = ocp_solver.solve() + ocp_solver.print_statistics() + if status != 0: + raise_test_failure_message(f"acados solver returned status {status}.") # get solution result_acados = ocp_solver.store_iterate_to_obj() result_acados_flat = ocp_solver.store_iterate_to_flat_obj() ## solve using casadi - casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) - casadi_ocp_solver.load_iterate_from_obj(result_acados) + casadi_solver_opts = {} + if casadi_solver_name == "fatrop": + casadi_solver_opts["expand"] = True + casadi_solver_opts["fatrop"] = {"mu_init": 0.1} + casadi_solver_opts["structure_detection"] = "auto" + casadi_solver_opts["debug"] = True + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False, solver=casadi_solver_name, casadi_solver_opts=casadi_solver_opts) + # casadi_ocp_solver.load_iterate_from_obj(result_acados) casadi_ocp_solver.solve() result_casadi = casadi_ocp_solver.store_iterate_to_obj() result_casadi_flat = casadi_ocp_solver.store_iterate_to_flat_obj() @@ -42,10 +55,15 @@ def main(): diff = np.linalg.norm(getattr(result_acados_flat, field) - getattr(result_casadi_flat, field)) print(f"Difference between acados and casadi solution for {field:4}: {diff:.3e}") if diff > test_tol: - raise ValueError(f"Test failed: difference between acados and casadi solution for {field} should be smaller than {test_tol}, but got {diff}.") + raise_test_failure_message(f"Test failed: difference between acados and casadi solution for {field} should be smaller than {test_tol}, but got {diff}.") - # plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu, u_casadi_sol, x_casadi_sol, latexify=False) + # plot_pendulum(ocp.solver_options.shooting_nodes, FMAX, + # np.array(result_casadi.u_traj), np.array(result_casadi.x_traj), latexify=False) + del ocp_solver, casadi_ocp_solver if __name__ == "__main__": - main() \ No newline at end of file + main(cost_version="CONL", constraint_version='bu') + main(cost_version="CONL", constraint_version='h') + main(cost_version="CONL", constraint_version='bu', casadi_solver_name="fatrop") + main(cost_version="CONL", constraint_version='h', casadi_solver_name="fatrop") diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 6b7143d521..64e3855809 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -31,9 +31,11 @@ import casadi as ca -from typing import Union, Tuple +from typing import Union, Tuple, Optional import numpy as np + +from .utils import casadi_length from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates, AcadosOcpFlattenedIterate @@ -114,6 +116,8 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n lbg = [] ubg = [] offset = 0 + + # create constraint functions # dynamics if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) @@ -124,69 +128,74 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n else: raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") - for i in range(solver_options.N_horizon): - # add dynamics constraints - if solver_options.integrator_type == "DISCRETE": - g.append(f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - xtraj_node[i+1]) - elif solver_options.integrator_type == "ERK": - g.append(f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i]) - xtraj_node[i+1]) - lbg.append(np.zeros((dims.nx, 1))) - ubg.append(np.zeros((dims.nx, 1))) - index_map['pi_in_lam_g'].append(list(range(offset, offset+dims.nx))) - offset += dims.nx - - # nonlinear constraints -- initial stage + # initial h0_fun = ca.Function('h0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) - g.append(h0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) - lbg.append(constraints.lh_0) - ubg.append(constraints.uh_0) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_0))) - - if dims.nphi_0 > 0: - conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) - conl_constr_0_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr_0]) - g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) - lbg.append(constraints.lphi_0) - ubg.append(constraints.uphi_0) - index_map['lam_gnl_in_lam_g'][-1]=list(range(offset, offset+dims.nh_0+dims.nphi_0)) - offset += dims.nh_0 + dims.nphi_0 - - # nonlinear constraints -- intermediate stages - h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) + # intermediate + h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) if dims.nphi > 0: conl_constr_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr]) - for i in range(1, solver_options.N_horizon): - g.append(h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) - lbg.append(constraints.lh) - ubg.append(constraints.uh) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh))) - - if dims.nphi > 0: - g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) - lbg.append(constraints.lphi) - ubg.append(constraints.uphi) - index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh+dims.nphi)) - offset += dims.nphi + dims.nh - - # nonlinear constraints -- terminal stage + # terminal h_e_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) - - g.append(h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) - lbg.append(constraints.lh_e) - ubg.append(constraints.uh_e) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_e))) - if dims.nphi_e > 0: conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) conl_constr_e_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_constr_expr_e]) - g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) - lbg.append(constraints.lphi_e) - ubg.append(constraints.uphi_e) - index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh_e+dims.nphi_e)) - offset += dims.nh_e + dims.nphi_e + + for i in range(solver_options.N_horizon+1): + # add dynamics constraints + if i < solver_options.N_horizon: + if solver_options.integrator_type == "DISCRETE": + g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + elif solver_options.integrator_type == "ERK": + g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i])) + lbg.append(np.zeros((dims.nx, 1))) + ubg.append(np.zeros((dims.nx, 1))) + index_map['pi_in_lam_g'].append(list(range(offset, offset+dims.nx))) + offset += dims.nx + + # nonlinear constraints + # initial stage + if i == 0 and solver_options.N_horizon > 0: + g.append(h0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) + lbg.append(constraints.lh_0) + ubg.append(constraints.uh_0) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_0))) + + if dims.nphi_0 > 0: + conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) + conl_constr_0_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr_0]) + g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) + lbg.append(constraints.lphi_0) + ubg.append(constraints.uphi_0) + index_map['lam_gnl_in_lam_g'][-1]=list(range(offset, offset+dims.nh_0+dims.nphi_0)) + offset += dims.nh_0 + dims.nphi_0 + elif i < solver_options.N_horizon: + g.append(h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + lbg.append(constraints.lh) + ubg.append(constraints.uh) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh))) + + if dims.nphi > 0: + g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + lbg.append(constraints.lphi) + ubg.append(constraints.uphi) + index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh+dims.nphi)) + offset += dims.nphi + dims.nh + else: + # terminal stage + g.append(h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) + lbg.append(constraints.lh_e) + ubg.append(constraints.uh_e) + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_e))) + + if dims.nphi_e > 0: + g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) + lbg.append(constraints.lphi_e) + ubg.append(constraints.uphi_e) + index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh_e+dims.nphi_e)) + offset += dims.nh_e + dims.nphi_e ### Cost # initial cost term @@ -253,14 +262,28 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n return nlp, bounds, w0, index_map - def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True): + def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True, + casadi_solver_opts: Optional[dict] = None): if not isinstance(acados_ocp, AcadosOcp): raise TypeError('acados_ocp should be of type AcadosOcp.') self.acados_ocp = acados_ocp + + # create casadi NLP formulation self.casadi_nlp, self.bounds, self.w0, self.index_map = self.create_casadi_nlp_formulation(acados_ocp) - self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp) + + # create NLP solver + if casadi_solver_opts is None: + casadi_solver_opts = {} + + if solver == "fatrop": + pi_in_lam_g_flat = [idx for sublist in self.index_map['pi_in_lam_g'] for idx in sublist] + is_equality_array = [True if i in pi_in_lam_g_flat else False for i in range(casadi_length(self.casadi_nlp['g']))] + casadi_solver_opts['equality'] = is_equality_array + self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp, casadi_solver_opts) + + # create solution and initial guess self.lam_x0 = np.empty(self.casadi_nlp['x'].shape).flatten() self.lam_g0 = np.empty(self.casadi_nlp['g'].shape).flatten() self.nlp_sol = None @@ -314,7 +337,7 @@ def get(self, stage: int, field: str): elif field == 'u': return self.nlp_sol_w[self.index_map['u_in_w'][stage]].flatten() elif field == 'pi': - return self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() + return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() elif field == 'lam': if stage == 0: bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_0 else np.empty((0, 1)) @@ -479,7 +502,7 @@ def set(self, stage: int, field: str, value_: np.ndarray): elif field == 'u': self.w0[self.index_map['u_in_w'][stage]] = value_.flatten() elif field == 'pi': - self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = value_.flatten() + self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = -value_.flatten() elif field == 'lam': if stage == 0: bx_length = dims.nbx_0 From 7564bd80e4c07fd9f719746ee5832e0e1708505b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 5 Jun 2025 16:50:06 +0200 Subject: [PATCH 065/164] `AcadosCasadiOcpSolver`: add option to create acados Hessian (#1544) Allows to use IPOPT with Gauss-Newton Hessian approximation --- .../ocp/test_casadi_formulation.py | 36 ++- .../acados_casadi_ocp_solver.py | 248 ++++++++++++------ .../casadi_function_generation.py | 7 +- .../acados_template/acados_template/utils.py | 6 + 4 files changed, 205 insertions(+), 92 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py index 6c7e7708ad..58aa448f0f 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -15,7 +15,7 @@ def raise_test_failure_message(msg: str): # print(f"ERROR: {msg}") raise Exception(msg) -def main(cost_version="CONL", constraint_version='h', casadi_solver_name="ipopt"): +def main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=False): ocp = formulate_ocp(cost_version=cost_version, constraint_version=constraint_version) ocp.solver_options.tf = T_HORIZON @@ -42,12 +42,21 @@ def main(cost_version="CONL", constraint_version='h', casadi_solver_name="ipopt" casadi_solver_opts["fatrop"] = {"mu_init": 0.1} casadi_solver_opts["structure_detection"] = "auto" casadi_solver_opts["debug"] = True - casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False, solver=casadi_solver_name, casadi_solver_opts=casadi_solver_opts) - # casadi_ocp_solver.load_iterate_from_obj(result_acados) - casadi_ocp_solver.solve() + + if use_acados_hessian: + # ocp.solver_options.hessian_approx = "EXACT" + ocp.solver_options.fixed_hess = False + + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False, solver=casadi_solver_name, casadi_solver_opts=casadi_solver_opts, use_acados_hessian=use_acados_hessian) + casadi_ocp_solver.load_iterate_from_obj(result_acados) + status = casadi_ocp_solver.solve() + print(f"casadi solver returned status {status}.") result_casadi = casadi_ocp_solver.store_iterate_to_obj() result_casadi_flat = casadi_ocp_solver.store_iterate_to_flat_obj() + nlp_iter_ca = casadi_ocp_solver.get_stats("nlp_iter") + print(f"Casadi solver finished after {nlp_iter_ca} iterations.") + # evaluate difference # TODO: set solver tolerance and reduce it here. test_tol = 5e-5 @@ -57,13 +66,24 @@ def main(cost_version="CONL", constraint_version='h', casadi_solver_name="ipopt" if diff > test_tol: raise_test_failure_message(f"Test failed: difference between acados and casadi solution for {field} should be smaller than {test_tol}, but got {diff}.") + # additional checks: + if cost_version == "LS" and constraint_version == "h": + if use_acados_hessian: + if nlp_iter_ca < 10: + raise_test_failure_message(f"Expected more iterations for casadi solver with acados (GN) Hessian, but got {nlp_iter_ca} iterations.") + else: + if nlp_iter_ca > 10: + raise_test_failure_message(f"Expected less iterations for casadi solver with Hessian, but got {nlp_iter_ca} iterations.") + # plot_pendulum(ocp.solver_options.shooting_nodes, FMAX, # np.array(result_casadi.u_traj), np.array(result_casadi.x_traj), latexify=False) del ocp_solver, casadi_ocp_solver if __name__ == "__main__": - main(cost_version="CONL", constraint_version='bu') - main(cost_version="CONL", constraint_version='h') - main(cost_version="CONL", constraint_version='bu', casadi_solver_name="fatrop") - main(cost_version="CONL", constraint_version='h', casadi_solver_name="fatrop") + # TODO: refactor to create acados solver only once fore each fomulation version. + main(cost_version="LS", constraint_version='bu') + main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=True) + main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=False) + main(cost_version="LS", constraint_version='bu', casadi_solver_name="fatrop") + main(cost_version="LS", constraint_version='h', casadi_solver_name="fatrop") diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 64e3855809..3b4445915e 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -35,14 +35,14 @@ import numpy as np -from .utils import casadi_length +from .utils import casadi_length, is_casadi_SX from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates, AcadosOcpFlattenedIterate class AcadosCasadiOcpSolver: @classmethod - def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.ndarray]: + def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tuple[dict, dict, np.ndarray, dict, Optional[ca.Function]]: """ Creates an equivalent CasADi NLP formulation of the OCP. Experimental, not fully implemented yet. @@ -56,6 +56,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n dims = ocp.dims constraints = ocp.constraints solver_options = ocp.solver_options + N_horizon = solver_options.N_horizon # check what is not supported yet if any([dims.ns_0, dims.ns, dims.ns_e]): @@ -81,43 +82,76 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n if any([dims.ns_0, dims.ns, dims.ns_e]): raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") - # create variables indexed by shooting nodes, vectorize in the end + # create primal variables indexed by shooting nodes ca_symbol = model.get_casadi_symbol() - xtraj_node = [ca_symbol(f'x{i}', dims.nx, 1) for i in range(solver_options.N_horizon+1)] - utraj_node = [ca_symbol(f'u{i}', dims.nu, 1) for i in range(solver_options.N_horizon)] + xtraj_node = [ca_symbol(f'x{i}', dims.nx, 1) for i in range(N_horizon+1)] + utraj_node = [ca_symbol(f'u{i}', dims.nu, 1) for i in range(N_horizon)] if dims.nz > 0: raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") # parameters - ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(solver_options.N_horizon)] + ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon)] - ### Constraints: bounds # setup state bounds - lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(solver_options.N_horizon+1)] - ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(solver_options.N_horizon+1)] + lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] + ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 offset = 0 - for i in range(1, solver_options.N_horizon): + for i in range(1, N_horizon): lb_xtraj_node[i][constraints.idxbx] = constraints.lbx ub_xtraj_node[i][constraints.idxbx] = constraints.ubx lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e # setup control bounds - lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] - ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(solver_options.N_horizon)] - for i in range(solver_options.N_horizon): + lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + for i in range(N_horizon): lb_utraj_node[i][constraints.idxbu] = constraints.lbu ub_utraj_node[i][constraints.idxbu] = constraints.ubu - ### Nonlinear constraints - g = [] - lbg = [] - ubg = [] + ### Concatenate primal variables and bounds + # w = [x0, u0, x1, u1, ...] + w_sym_list = [] + lbw_list = [] + ubw_list = [] + w0_list = [] offset = 0 + x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) + for i in range(N_horizon): + # add x + w_sym_list.append(xtraj_node[i]) + lbw_list.append(lb_xtraj_node[i]) + ubw_list.append(ub_xtraj_node[i]) + w0_list.append(x_guess) + index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) + offset += dims.nx + # add u + w_sym_list.append(utraj_node[i]) + lbw_list.append(lb_utraj_node[i]) + ubw_list.append(ub_utraj_node[i]) + w0_list.append(np.zeros((dims.nu,))) + index_map['u_in_w'].append(list(range(offset, offset + dims.nu))) + offset += dims.nu + ## terminal stage + # add x + w_sym_list.append(xtraj_node[-1]) + lbw_list.append(lb_xtraj_node[-1]) + ubw_list.append(ub_xtraj_node[-1]) + w0_list.append(x_guess) + index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) + offset += dims.nx + + nw = offset # number of primal variables + + # vectorize + w = ca.vertcat(*w_sym_list) + lbw = ca.vertcat(*lbw_list) + ubw = ca.vertcat(*ubw_list) + p_nlp = ca.vertcat(*ptraj_node, model.p_global) - # create constraint functions + ### Nonlinear constraints # dynamics if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) @@ -129,7 +163,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") # initial - h0_fun = ca.Function('h0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) + h_0_fun = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) # intermediate h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) @@ -143,25 +177,51 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) conl_constr_e_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_constr_expr_e]) - for i in range(solver_options.N_horizon+1): + # create nonlinear constraints + g = [] + lbg = [] + ubg = [] + offset = 0 + if with_hessian: + lam_g = [] + hess_l = ca.DM.zeros((nw, nw)) + + for i in range(N_horizon+1): # add dynamics constraints - if i < solver_options.N_horizon: + if i < N_horizon: if solver_options.integrator_type == "DISCRETE": - g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) elif solver_options.integrator_type == "ERK": - g.append(xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i])) + dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i]) + g.append(dyn_equality) lbg.append(np.zeros((dims.nx, 1))) ubg.append(np.zeros((dims.nx, 1))) index_map['pi_in_lam_g'].append(list(range(offset, offset+dims.nx))) offset += dims.nx + if with_hessian: + # add hessian of dynamics constraints + lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) + lam_g.append(lam_g_dyn) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_dyn: + adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + # nonlinear constraints # initial stage - if i == 0 and solver_options.N_horizon > 0: - g.append(h0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) + if i == 0 and N_horizon > 0: + # h_0 + h_0_nlp_expr = h_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) + g.append(h_0_nlp_expr) lbg.append(constraints.lh_0) ubg.append(constraints.uh_0) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_0))) + if with_hessian and dims.nh_0 > 0: + lam_h_0 = ca_symbol(f'lam_h_0', dims.nh_0, 1) + lam_g.append(lam_h_0) + # add hessian contribution + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_0_nlp_expr, w, lam_h_0, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi_0 > 0: conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) @@ -169,32 +229,73 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) lbg.append(constraints.lphi_0) ubg.append(constraints.uphi_0) - index_map['lam_gnl_in_lam_g'][-1]=list(range(offset, offset+dims.nh_0+dims.nphi_0)) + if with_hessian: + lam_phi_0 = ca_symbol(f'lam_phi_0', dims.nphi_0, 1) + lam_g.append(lam_phi_0) + # always use CONL Hessian approximation here, disregarding inner derivative + hess = ca.vertcat(*[ca.hessian(model.con_phi_expr_0[i], model.con_r_in_phi_0)[0] for i in range(dims.nphi_0)]) + hess = ca.substitute(hess, model.con_r_in_phi_0, model.con_r_expr_0) + hess_l += hess + + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_0 + dims.nphi_0))) offset += dims.nh_0 + dims.nphi_0 - elif i < solver_options.N_horizon: - g.append(h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) + + elif i < N_horizon: + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + g.append(h_i_nlp_expr) lbg.append(constraints.lh) ubg.append(constraints.uh) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh))) + if with_hessian and dims.nh > 0: + # add hessian contribution + lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) + lam_g.append(lam_h) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi > 0: g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) lbg.append(constraints.lphi) ubg.append(constraints.uphi) - index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh+dims.nphi)) + if with_hessian: + lam_phi = ca_symbol(f'lam_phi', dims.nphi, 1) + lam_g.append(lam_phi) + # always use CONL Hessian approximation here, disregarding inner derivative + hess = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) + hess = ca.substitute(hess, model.con_r_in_phi, model.con_r_expr) + hess_l += hess + + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh + dims.nphi))) offset += dims.nphi + dims.nh + else: # terminal stage - g.append(h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) + h_e_nlp_expr = h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global) + g.append(h_e_nlp_expr) lbg.append(constraints.lh_e) ubg.append(constraints.uh_e) - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset+dims.nh_e))) + if with_hessian and dims.nh_e > 0: + # add hessian contribution + lam_h_e = ca_symbol(f'lam_h_e', dims.nh_e, 1) + lam_g.append(lam_h_e) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + breakpoint() + adj = ca.jtimes(h_e_nlp_expr, w, lam_h_e, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi_e > 0: g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) lbg.append(constraints.lphi_e) ubg.append(constraints.uphi_e) - index_map['lam_gnl_in_lam_g'][-1] = list(range(offset, offset+dims.nh_e+dims.nphi_e)) + if with_hessian: + lam_phi_e = ca_symbol(f'lam_phi_e', dims.nphi_e, 1) + lam_g.append(lam_phi_e) + # always use CONL Hessian approximation here, disregarding inner derivative + hess = ca.vertcat(*[ca.hessian(model.con_phi_expr_e[i], model.con_r_in_phi_e)[0] for i in range(dims.nphi_e)]) + hess = ca.substitute(hess, model.con_r_in_phi_e, model.con_r_expr_e) + hess_l += hess + + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_e + dims.nphi_e))) offset += dims.nh_e + dims.nphi_e ### Cost @@ -207,7 +308,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n # intermediate cost term cost_expr = ocp.get_path_cost_expression() cost_fun = ca.Function('cost_fun', [model.x, model.u, model.p, model.p_global], [cost_expr]) - for i in range(1, solver_options.N_horizon): + for i in range(1, N_horizon): nlp_cost += solver_options.cost_scaling[i] * cost_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) # terminal cost term @@ -215,55 +316,32 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp) -> Tuple[dict, dict, np.n cost_fun_e = ca.Function('cost_fun_e', [model.x, model.p, model.p_global], [cost_expr_e]) nlp_cost += solver_options.cost_scaling[-1] * cost_fun_e(xtraj_node[-1], ptraj_node[-1], model.p_global) - ### Formulation - # interleave primary variables w and bounds - # w = [x0, u0, x1, u1, ...] - w_sym_list = [] - lbw_list = [] - ubw_list = [] - w0_list = [] - offset = 0 - x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) - for i in range(solver_options.N_horizon): - # add x - w_sym_list.append(xtraj_node[i]) - lbw_list.append(lb_xtraj_node[i]) - ubw_list.append(ub_xtraj_node[i]) - w0_list.append(x_guess) - index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) - offset += dims.nx - # add u - w_sym_list.append(utraj_node[i]) - lbw_list.append(lb_utraj_node[i]) - ubw_list.append(ub_utraj_node[i]) - w0_list.append(np.zeros((dims.nu,))) - index_map['u_in_w'].append(list(range(offset, offset + dims.nu))) - offset += dims.nu - ## terminal stage - # add x - w_sym_list.append(xtraj_node[-1]) - lbw_list.append(lb_xtraj_node[-1]) - ubw_list.append(ub_xtraj_node[-1]) - w0_list.append(x_guess) - index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) - offset += dims.nx + if with_hessian: + lam_f = ca_symbol('lam_f', 1, 1) + if ocp.solver_options.hessian_approx == 'EXACT' or \ + (ocp.cost.cost_type == "LINEAR_LS" and ocp.cost.cost_type_0 == "LINEAR_LS" and ocp.cost.cost_type_e == "LINEAR_LS"): + hess_l += lam_f * ca.hessian(nlp_cost, w)[0] + else: + raise NotImplementedError("Hessian approximation not implemented for this cost type.") + lam_g_vec = ca.vertcat(*lam_g) + nlp_hess_l_custom = ca.Function('nlp_hess_l', [w, p_nlp, lam_f, lam_g_vec], [ca.triu(hess_l)]) + assert casadi_length(lam_g_vec) == casadi_length(ca.vertcat(*g)), f"Number of nonlinear constraints does not match the expected number, got {casadi_length(lam_g_vec)} != {casadi_length(ca.vertcat(*g))}." + else: + nlp_hess_l_custom = None - # vectorize - w = ca.vertcat(*w_sym_list) - lbw = ca.vertcat(*lbw_list) - ubw = ca.vertcat(*ubw_list) - p_nlp = ca.vertcat(*ptraj_node, model.p_global) + # sanity check # create NLP nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} w0 = np.concatenate(w0_list) - return nlp, bounds, w0, index_map + return nlp, bounds, w0, index_map, nlp_hess_l_custom def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True, - casadi_solver_opts: Optional[dict] = None): + casadi_solver_opts: Optional[dict] = None, + use_acados_hessian: bool = False): if not isinstance(acados_ocp, AcadosOcp): raise TypeError('acados_ocp should be of type AcadosOcp.') @@ -271,7 +349,7 @@ def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True, self.acados_ocp = acados_ocp # create casadi NLP formulation - self.casadi_nlp, self.bounds, self.w0, self.index_map = self.create_casadi_nlp_formulation(acados_ocp) + self.casadi_nlp, self.bounds, self.w0, self.index_map, self.nlp_hess_l_custom = self.create_casadi_nlp_formulation(acados_ocp, with_hessian=use_acados_hessian) # create NLP solver if casadi_solver_opts is None: @@ -281,6 +359,9 @@ def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True, pi_in_lam_g_flat = [idx for sublist in self.index_map['pi_in_lam_g'] for idx in sublist] is_equality_array = [True if i in pi_in_lam_g_flat else False for i in range(casadi_length(self.casadi_nlp['g']))] casadi_solver_opts['equality'] = is_equality_array + + if use_acados_hessian: + casadi_solver_opts["cache"] = {"nlp_hess_l": self.nlp_hess_l_custom} self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp, casadi_solver_opts) # create solution and initial guess @@ -307,8 +388,15 @@ def solve(self) -> int: self.nlp_sol_w = self.nlp_sol['x'].full() self.nlp_sol_lam_g = self.nlp_sol['lam_g'].full() self.nlp_sol_lam_x = self.nlp_sol['lam_x'].full() - # TODO: return correct status - return 0 + + # statistics + solver_stats = self.casadi_solver.stats() + # timing = solver_stats['t_proc_total'] + self.status = solver_stats['return_status'] + self.nlp_iter = solver_stats['iter_count'] + # nlp_res = ca.norm_inf(sol['g']).full()[0][0] + # cost_val = ca.norm_inf(sol['f']).full()[0][0] + return self.status def get_dim_flat(self, field: str): """ @@ -482,7 +570,11 @@ def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedIterate) -> None self.set_flat("lam", iterate.lam) def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: - raise NotImplementedError() + + if field_ == "nlp_iter": + return self.nlp_iter + else: + raise NotImplementedError() def get_cost(self) -> float: raise NotImplementedError() diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 706f52c0b9..3267c0edcf 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -33,16 +33,11 @@ import os, warnings import casadi as ca -from .utils import is_empty, casadi_length, check_casadi_version_supports_p_global, print_casadi_expression, set_directory +from .utils import is_empty, casadi_length, check_casadi_version_supports_p_global, print_casadi_expression, set_directory, is_casadi_SX from .acados_model import AcadosModel from .acados_ocp_constraints import AcadosOcpConstraints -def is_casadi_SX(x): - if isinstance(x, ca.SX): - return True - return False - @dataclass class AcadosCodegenOptions: ext_fun_expand_constr: bool = False diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 87a1a2d34c..0acc0e13c8 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -156,6 +156,12 @@ def get_simulink_default_opts() -> dict: return simulink_default_opts +def is_casadi_SX(x): + if isinstance(x, ca.SX): + return True + return False + + def is_column(x): if isinstance(x, np.ndarray): if x.ndim == 1: From a38e55981702cf3e49224c351bf59911c2a5f9ed Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 6 Jun 2025 15:51:45 +0200 Subject: [PATCH 066/164] `AcadosCasadiOcpSolver`: fix `set_flat` and CONL constraint Hessian (#1545) --- .../acados_casadi_ocp_solver.py | 76 ++++++++++--------- 1 file changed, 40 insertions(+), 36 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 3b4445915e..c330aa571b 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -232,10 +232,12 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu if with_hessian: lam_phi_0 = ca_symbol(f'lam_phi_0', dims.nphi_0, 1) lam_g.append(lam_phi_0) - # always use CONL Hessian approximation here, disregarding inner derivative - hess = ca.vertcat(*[ca.hessian(model.con_phi_expr_0[i], model.con_r_in_phi_0)[0] for i in range(dims.nphi_0)]) - hess = ca.substitute(hess, model.con_r_in_phi_0, model.con_r_expr_0) - hess_l += hess + # always use CONL Hessian approximation here, disregarding inner second derivative + outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr_0[i], model.con_r_in_phi_0)[0] for i in range(dims.nphi_0)]) + outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi_0, model.con_r_expr_0) + r_in_nlp = ca.substitute(model.con_r_expr_0, model.x, xtraj_node[-1]) + dr_dw = ca.jacobian(r_in_nlp, w) + hess_l += dr_dw.T @ outer_hess_r @ dr_dw index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_0 + dims.nphi_0))) offset += dims.nh_0 + dims.nphi_0 @@ -260,10 +262,12 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu if with_hessian: lam_phi = ca_symbol(f'lam_phi', dims.nphi, 1) lam_g.append(lam_phi) - # always use CONL Hessian approximation here, disregarding inner derivative - hess = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) - hess = ca.substitute(hess, model.con_r_in_phi, model.con_r_expr) - hess_l += hess + # always use CONL Hessian approximation here, disregarding inner second derivative + outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) + outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi, model.con_r_expr) + r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_node[-1]) + dr_dw = ca.jacobian(r_in_nlp, w) + hess_l += dr_dw.T @ outer_hess_r @ dr_dw index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh + dims.nphi))) offset += dims.nphi + dims.nh @@ -279,7 +283,6 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu lam_h_e = ca_symbol(f'lam_h_e', dims.nh_e, 1) lam_g.append(lam_h_e) if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - breakpoint() adj = ca.jtimes(h_e_nlp_expr, w, lam_h_e, True) hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) @@ -290,10 +293,12 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu if with_hessian: lam_phi_e = ca_symbol(f'lam_phi_e', dims.nphi_e, 1) lam_g.append(lam_phi_e) - # always use CONL Hessian approximation here, disregarding inner derivative - hess = ca.vertcat(*[ca.hessian(model.con_phi_expr_e[i], model.con_r_in_phi_e)[0] for i in range(dims.nphi_e)]) - hess = ca.substitute(hess, model.con_r_in_phi_e, model.con_r_expr_e) - hess_l += hess + # always use CONL Hessian approximation here, disregarding inner second derivative + outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr_e[i], model.con_r_in_phi_e)[0] for i in range(dims.nphi_e)]) + outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi_e, model.con_r_expr_e) + r_in_nlp = ca.substitute(model.con_r_expr_e, model.x, xtraj_node[-1]) + dr_dw = ca.jacobian(r_in_nlp, w) + hess_l += dr_dw.T @ outer_hess_r @ dr_dw index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_e + dims.nphi_e))) offset += dims.nh_e + dims.nphi_e @@ -505,14 +510,13 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: offset = 0 for i in range(dims.N+1): if i == 0: - self.set(i, 'lam', value_[offset:offset+2*(dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0)]) - offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + n_lam_i = 2 * (dims.nbx_0 + dims.nbu + dims.ng + dims.nh_0 + dims.nphi_0) elif i < dims.N: - self.set(i, 'lam', value_[offset:offset+2*(dims.nbx+dims.nbu+dims.ng+dims.nh+dims.nphi)]) - offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + n_lam_i = 2 * (dims.nbx + dims.nbu + dims.ng + dims.nh + dims.nphi) elif i == dims.N: - self.set(i, 'lam', value_[offset:offset+2*(dims.nbx_e+dims.ng_e+dims.nh_e+dims.nphi_e)]) - offset += 2 * (dims.nbx_0+dims.nbu+dims.ng+dims.nh_0+dims.nphi_0) + n_lam_i = 2 * (dims.nbx_e + dims.ng_e + dims.nh_e + dims.nphi_e) + self.set(i, 'lam', value_[offset : offset + n_lam_i]) + offset += n_lam_i else: raise NotImplementedError(f"Field '{field_}' is not yet implemented in set_flat().") @@ -597,25 +601,25 @@ def set(self, stage: int, field: str, value_: np.ndarray): self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = -value_.flatten() elif field == 'lam': if stage == 0: - bx_length = dims.nbx_0 - bu_length = dims.nbu - h_length = dims.ng + dims.nh_0 + dims.nphi_0 + nbx = dims.nbx_0 + nbu = dims.nbu + n_ghphi = dims.ng + dims.nh_0 + dims.nphi_0 elif stage < dims.N: - bx_length = dims.nbx - bu_length = dims.nbu - h_length = dims.ng + dims.nh + dims.nphi + nbx = dims.nbx + nbu = dims.nbu + n_ghphi = dims.ng + dims.nh + dims.nphi elif stage == dims.N: - bx_length = dims.nbx_e - bu_length = 0 - h_length = dims.ng_e + dims.nh_e + dims.nphi_e - - offset_u = (bx_length+bu_length+h_length) - lbu_lam = value_[:bu_length] if bu_length else np.empty((dims.nu,)) - lbx_lam = value_[bu_length:bu_length+bx_length] if bx_length else np.empty((dims.nx,)) - lg_lam = value_[bu_length+bx_length:bu_length+bx_length+h_length] - ubu_lam = value_[offset_u:offset_u+bu_length] if bu_length else np.empty((dims.nu,)) - ubx_lam = value_[offset_u+bu_length:offset_u+bu_length+bx_length] if bx_length else np.empty((dims.nx,)) - ug_lam = value_[offset_u+bu_length+bx_length:offset_u+bu_length+bx_length+h_length] + nbx = dims.nbx_e + nbu = 0 + n_ghphi = dims.ng_e + dims.nh_e + dims.nphi_e + + offset_u = (nbx+nbu+n_ghphi) + lbu_lam = value_[:nbu] if nbu else np.empty((dims.nu,)) + lbx_lam = value_[nbu:nbu+nbx] if nbx else np.empty((dims.nx,)) + lg_lam = value_[nbu+nbx:nbu+nbx+n_ghphi] + ubu_lam = value_[offset_u:offset_u+nbu] if nbu else np.empty((dims.nu,)) + ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] if nbx else np.empty((dims.nx,)) + ug_lam = value_[offset_u+nbu+nbx:offset_u+nbu+nbx+n_ghphi] if stage != dims.N: self.lam_x0[self.index_map['x_in_w'][stage]+self.index_map['u_in_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam From b9638d2c928e5c71fd25060709ed00791b9c946c Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 6 Jun 2025 16:34:40 +0200 Subject: [PATCH 067/164] Create `AcadosCasadiOcp` class to create casadi formulation without solver (#1546) --- .../acados_template/__init__.py | 2 +- .../acados_casadi_ocp_solver.py | 117 +++++++++++++----- 2 files changed, 89 insertions(+), 30 deletions(-) diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index 6d2dcdd2ae..b1ceb96989 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -43,7 +43,7 @@ from .acados_multiphase_ocp import AcadosMultiphaseOcp from .acados_ocp_solver import AcadosOcpSolver -from .acados_casadi_ocp_solver import AcadosCasadiOcpSolver +from .acados_casadi_ocp_solver import AcadosCasadiOcpSolver, AcadosCasadiOcp from .acados_sim_solver import AcadosSimSolver from .acados_sim_batch_solver import AcadosSimBatchSolver from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \ diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index c330aa571b..18f26fd405 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -31,18 +31,17 @@ import casadi as ca -from typing import Union, Tuple, Optional +from typing import Union, Optional import numpy as np from .utils import casadi_length, is_casadi_SX from .acados_ocp import AcadosOcp -from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpIterates, AcadosOcpFlattenedIterate +from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpFlattenedIterate -class AcadosCasadiOcpSolver: +class AcadosCasadiOcp: - @classmethod - def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tuple[dict, dict, np.ndarray, dict, Optional[ca.Function]]: + def __init__(self, ocp: AcadosOcp, with_hessian=False): """ Creates an equivalent CasADi NLP formulation of the OCP. Experimental, not fully implemented yet. @@ -51,6 +50,17 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu """ ocp.make_consistent() + # create index map for variables + index_map = { + # indices of variables within w + 'x_in_w': [], + 'u_in_w': [], + # indices of dynamic constraints within g in casadi formulation + 'pi_in_lam_g': [], + # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation + 'lam_gnl_in_lam_g': [] + } + # unpack model = ocp.model dims = ocp.dims @@ -67,18 +77,6 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu raise NotImplementedError("AcadosCasadiOcpSolver does not support general nonlinear constraints (g) yet.") if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") - - # create index map for variables - index_map = { - # indices of variables within w - 'x_in_w': [], - 'u_in_w': [], - # indices of dynamic constraints within g in casadi formulation - 'pi_in_lam_g': [], - # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation - 'lam_gnl_in_lam_g': [] - } - if any([dims.ns_0, dims.ns, dims.ns_e]): raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") @@ -333,6 +331,7 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu assert casadi_length(lam_g_vec) == casadi_length(ca.vertcat(*g)), f"Number of nonlinear constraints does not match the expected number, got {casadi_length(lam_g_vec)} != {casadi_length(ca.vertcat(*g))}." else: nlp_hess_l_custom = None + hess_l = None # sanity check @@ -341,20 +340,80 @@ def create_casadi_nlp_formulation(cls, ocp: AcadosOcp, with_hessian=False) -> Tu bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} w0 = np.concatenate(w0_list) - return nlp, bounds, w0, index_map, nlp_hess_l_custom + self.__nlp = nlp + self.__bounds = bounds + self.__w0 = w0 + self.__index_map = index_map + self.__nlp_hess_l_custom = nlp_hess_l_custom + self.__hess_approx_expr = hess_l + + @property + def nlp(self): + """ + Dict containing all symbolics needed to create a `casadi.nlpsol` solver. + """ + return self.__nlp + + @property + def w0(self): + """ + Default initial guess for primal variable vector w for given NLP. + """ + return self.__w0 + + @property + def bounds(self): + """ + Dict containing all bounds needed to call a `casadi.nlpsol` solver. + """ + return self.__bounds + + @property + def index_map(self): + """ + Dict containing indices corresponding to stage-wise values of the original OCP, specifically: + - 'x_in_w': indices of x variables within primal variable vector w + - 'u_in_w': indices of u variables within primal variable vector w + - 'pi_in_lam_g': indices of dynamic constraints within g in casadi formulation + - 'lam_gnl_in_lam_g' indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation + """ + return self.__index_map + @property + def nlp_hess_l_custom(self): + """ + CasADi Function that computes the Hessian approximation of the Lagrangian in the format required by `casadi.nlpsol`, i.e. as upper triangular matrix. + The Hessian is set up to match the Hessian that would be used in acados and depends on the solver options. + """ + return self.__nlp_hess_l_custom - def __init__(self, acados_ocp: AcadosOcp, solver: str = "ipopt", verbose=True, + @property + def hess_approx_expr(self): + """ + CasADi expression corresponding to the Hessian approximation of the Lagrangian. + Expression corresponding to what is output by the `nlp_hess_l_custom` function. + """ + return self.__hess_approx_expr + +class AcadosCasadiOcpSolver: + + def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, casadi_solver_opts: Optional[dict] = None, use_acados_hessian: bool = False): - if not isinstance(acados_ocp, AcadosOcp): - raise TypeError('acados_ocp should be of type AcadosOcp.') + if not isinstance(ocp, AcadosOcp): + raise TypeError('ocp should be of type AcadosOcp.') - self.acados_ocp = acados_ocp + self.ocp = ocp # create casadi NLP formulation - self.casadi_nlp, self.bounds, self.w0, self.index_map, self.nlp_hess_l_custom = self.create_casadi_nlp_formulation(acados_ocp, with_hessian=use_acados_hessian) + casadi_nlp_obj = AcadosCasadiOcp(ocp, with_hessian=use_acados_hessian) + + self.casadi_nlp = casadi_nlp_obj.nlp + self.bounds = casadi_nlp_obj.bounds + self.w0 = casadi_nlp_obj.w0 + self.index_map = casadi_nlp_obj.index_map + self.nlp_hess_l_custom = casadi_nlp_obj.nlp_hess_l_custom # create NLP solver if casadi_solver_opts is None: @@ -424,7 +483,7 @@ def get(self, stage: int, field: str): raise TypeError('stage should be integer.') if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') - dims = self.acados_ocp.dims + dims = self.ocp.dims if field == 'x': return self.nlp_sol_w[self.index_map['x_in_w'][stage]].flatten() elif field == 'u': @@ -469,7 +528,7 @@ def get_flat(self, field_: str) -> np.ndarray: """ if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') - dims = self.acados_ocp.dims + dims = self.ocp.dims result = [] if field_ in ['x', 'lam', 'sl', 'su']: @@ -496,7 +555,7 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: :param field: string in ['x', 'u', 'lam', pi] """ - dims = self.acados_ocp.dims + dims = self.ocp.dims if field_ == 'x': for i in range(dims.N+1): self.set(i, 'x', value_[i*dims.nx:(i+1)*dims.nx]) @@ -530,8 +589,8 @@ def store_iterate_to_obj(self) -> AcadosOcpIterate: d = {} for field in ["x", "u", "z", "sl", "su", "pi", "lam"]: traj = [] - for n in range(self.acados_ocp.dims.N+1): - if n < self.acados_ocp.dims.N or not (field in ["u", "pi", "z"]): + for n in range(self.ocp.dims.N+1): + if n < self.ocp.dims.N or not (field in ["u", "pi", "z"]): traj.append(self.get(n, field)) d[f"{field}_traj"] = traj @@ -591,7 +650,7 @@ def set(self, stage: int, field: str, value_: np.ndarray): :param field: string in ['x', 'u', 'pi', 'lam'] :value_: """ - dims = self.acados_ocp.dims + dims = self.ocp.dims if field == 'x': self.w0[self.index_map['x_in_w'][stage]] = value_.flatten() From dd7827a559b6f5b8b68fd12d17b2b700eff641e2 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 6 Jun 2025 16:45:04 +0200 Subject: [PATCH 068/164] Implement Anderson acceleration for fixed step SQP-type algorithms (#1521) Only take 1 pervious step into account, as in this case there is an explicit formula to compute the step. This formula is e.g. given in eq. (4.4) in [Pollock2021 -- Anderson acceleration for contractive and noncontractive operators](https://academic.oup.com/imajna/article/41/4/2841/6065020) This feature can be used by setting `with_anderson_acceleration = True`. A plot of the SCQP benchmark example is below: ![anderson_scqp_convergence](https://github.com/user-attachments/assets/68e3d04b-dc5b-4912-9b20-ee1b234855e5) Plots corresponding to the convergence experiment with the Furuta pendulum are below: ![contraction_rates_furuta_pendulum](https://github.com/user-attachments/assets/67959d74-16ba-450f-919e-aeb9a60456e7) ![convergence_furuta_pendulum](https://github.com/user-attachments/assets/82a8924f-455c-4737-928b-a8137883f852) --- .github/workflows/full_build.yml | 1 + acados/ocp_nlp/ocp_nlp_common.c | 101 +++++++- acados/ocp_nlp/ocp_nlp_common.h | 15 +- .../ocp_nlp_globalization_fixed_step.c | 53 ++++- acados/ocp_nlp/ocp_nlp_sqp.c | 7 +- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 2 +- acados/ocp_qp/ocp_qp_common.c | 178 +++++++++++++- acados/ocp_qp/ocp_qp_common.h | 12 + .../anderson_scqp_experiment.py | 208 +++++++++++++++++ .../scqp_test_problem.py | 217 ++++++++++++++++++ .../furuta_pendulum/convergence_experiment.py | 108 +++++++++ .../{furuta_model.py => furuta_common.py} | 65 ++++++ .../furuta_pendulum/integrator_experiment.py | 2 +- .../furuta_pendulum/main_closed_loop.py | 86 ++----- interfaces/acados_matlab_octave/AcadosOcp.m | 10 + .../acados_matlab_octave/AcadosOcpOptions.m | 2 + .../acados_template/__init__.py | 2 +- .../acados_template/acados_ocp.py | 7 + .../acados_template/acados_ocp_options.py | 23 +- .../acados_template/acados_ocp_solver.py | 6 + .../c_templates_tera/acados_multi_solver.in.c | 3 + .../c_templates_tera/acados_solver.in.c | 3 + .../acados_template/plot_utils.py | 55 ++++- 23 files changed, 1063 insertions(+), 103 deletions(-) create mode 100755 examples/acados_python/anderson_acceleration/anderson_scqp_experiment.py create mode 100644 examples/acados_python/anderson_acceleration/scqp_test_problem.py create mode 100644 examples/acados_python/furuta_pendulum/convergence_experiment.py rename examples/acados_python/furuta_pendulum/{furuta_model.py => furuta_common.py} (50%) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index ef185520d3..9674ecbf53 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -236,6 +236,7 @@ jobs: source ${{runner.workspace}}/acados/acadosenv/bin/activate cd ${{runner.workspace}}/acados/examples/acados_python/furuta_pendulum python main_closed_loop.py + python convergence_experiment.py - name: Python evaluator test working-directory: ${{runner.workspace}}/acados/build diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index f36909aab6..b848a4d9d0 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1492,6 +1492,11 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value int* with_value_sens_wrt_params = (int *) value; opts->with_value_sens_wrt_params = *with_value_sens_wrt_params; } + else if (!strcmp(field, "with_anderson_acceleration")) + { + bool* with_anderson_acceleration = (bool *) value; + opts->with_anderson_acceleration = *with_anderson_acceleration; + } else { printf("\nerror: ocp_nlp_opts_set: wrong field: %s\n", field); @@ -1568,13 +1573,18 @@ acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims acados_size_t size = sizeof(ocp_nlp_memory); - // qp in + // qp_in size += ocp_qp_in_calculate_size(dims->qp_solver->orig_dims); - // qp out + // qp_out size += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); - // qp solver + if (opts->with_anderson_acceleration) + { + size += 2*ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); // prev_qp_out, anderson_step + } + + // qp_solver size += qp_solver->memory_calculate_size(qp_solver, dims->qp_solver, opts->qp_solver_opts); // relaxed qp solver memory in sqp_with_feasible_qp.c @@ -1743,6 +1753,14 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims mem->qp_out = ocp_qp_out_assign(dims->qp_solver->orig_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); + if (opts->with_anderson_acceleration) + { + mem->prev_qp_out = ocp_qp_out_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); + mem->anderson_step = ocp_qp_out_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); + } + // QP solver mem->qp_solver_mem = qp_solver->memory_assign(qp_solver, dims->qp_solver, opts->qp_solver_opts, c_ptr); c_ptr += qp_solver->memory_calculate_size(qp_solver, dims->qp_solver, opts->qp_solver_opts); @@ -1958,9 +1976,6 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d // weight_merit_fun size += ocp_nlp_out_calculate_size(config, dims); - // tmp_qp_in - size += ocp_qp_in_calculate_size(dims->qp_solver->orig_dims); - // tmp_qp_out size += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); @@ -2212,10 +2227,6 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims work->tmp_nlp_out = ocp_nlp_out_assign(config, dims, c_ptr); c_ptr += ocp_nlp_out_calculate_size(config, dims); - // tmp qp in - work->tmp_qp_in = ocp_qp_in_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_in_calculate_size(dims->qp_solver->orig_dims); - // tmp qp out work->tmp_qp_out = ocp_qp_out_assign(dims->qp_solver->orig_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); @@ -3149,6 +3160,71 @@ void ocp_nlp_initialize_qp_from_nlp(ocp_nlp_config *config, ocp_nlp_dims *dims, } +double ocp_nlp_compute_anderson_gamma(ocp_nlp_workspace *work, ocp_qp_out *new_qp_step, ocp_qp_out *new_minus_old_qp_step) +{ + double gamma = ocp_qp_out_ddot(new_qp_step, new_minus_old_qp_step, &work->tmp_2ni) / + ocp_qp_out_ddot(new_minus_old_qp_step, new_minus_old_qp_step, &work->tmp_2ni); + return gamma; +} + + +void ocp_nlp_convert_primaldelta_absdual_step_to_delta_step(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_out *out, ocp_qp_out *step) +{ + int N = dims->N; + int *nx = dims->nx; + int *ni = dims->ni; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + // for all x in delta format: convert as x_step = x_step - x_iterate + // dual variables + blasfeo_dvecad(2*ni[i], -1.0, out->lam+i, 0, step->lam+i, 0); + if (i < N) + { + blasfeo_dvecad(nx[i+1], -1.0, out->pi+i, 0, step->pi+i, 0); + } + } +} + + +void ocp_nlp_update_variables_sqp_delta_primal_dual(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, double alpha, ocp_qp_out *step) +{ + int N = dims->N; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + int *ni = dims->ni; + int *nz = dims->nz; + + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + // step in primal variables + blasfeo_daxpy(nv[i], alpha, step->ux+i, 0, out->ux+i, 0, out->ux+i, 0); + + blasfeo_daxpy(2*ni[i], alpha, step->lam+i, 0, out->lam+i, 0, out->lam+i, 0); + if (i < N) + { + // update duals with alpha step + blasfeo_daxpy(nx[i+1], alpha, step->pi+i, 0, out->pi+i, 0, out->pi+i, 0); + // linear update of algebraic variables using state and input sensitivity + // out->z = mem->z_alg + alpha * dzdux * qp_out->ux + blasfeo_dgemv_t(nu[i]+nx[i], nz[i], alpha, mem->dzduxt+i, 0, 0, + step->ux+i, 0, 1.0, mem->z_alg+i, 0, out->z+i, 0); + } + } +} + + + int ocp_nlp_precompute_common(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) { @@ -3477,6 +3553,8 @@ void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out) } +/* Helper functions */ + double ocp_nlp_compute_delta_dual_norm_inf(ocp_nlp_dims *dims, ocp_nlp_workspace *work, ocp_nlp_out *nlp_out, ocp_qp_out *qp_out) { /* computes the inf norm of multipliers in qp_out and nlp_out */ @@ -3754,7 +3832,6 @@ void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims, exit(1); } - // d_ocp_qp_print(tmp_qp_in->dim, tmp_qp_in); // d_ocp_qp_seed_print(qp_seed->dim, qp_seed); config->qp_solver->eval_forw_sens(config->qp_solver, dims->qp_solver, mem->qp_in, qp_seed, tmp_qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); @@ -3809,7 +3886,7 @@ void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dim blasfeo_dveccp(nv[i], sens_nlp_out->ux + i, 0, qp_seed->seed_g + i, 0); // NOTE: noone needs sensitivities in adj dir pi, lam, t wrt. p // if (i < N) - // blasfeo_dveccp(nx[i + 1], sens_nlp_out->pi + i, 0, tmp_qp_in->b + i, 0); + // blasfeo_dveccp(nx[i + 1], sens_nlp_out->pi + i, 0, qp_seed->b + i, 0); // blasfeo_dveccp(2 * ni[i], sens_nlp_out->lam + i, ?); // blasfeo_dveccp(2 * ni[i], sens_nlp_out->t + i, ?); } diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 30dd5fa6ae..9c3c8e8d28 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -333,6 +333,7 @@ typedef struct ocp_nlp_opts bool store_iterates; // flag indicating whether intermediate iterates should be stored + bool with_anderson_acceleration; } ocp_nlp_opts; @@ -431,6 +432,10 @@ typedef struct ocp_nlp_memory ocp_qp_in *qp_in; ocp_qp_out *qp_out; + // for Anderson acceleration + ocp_qp_out *prev_qp_out; + ocp_qp_out *anderson_step; + // QP stuff not entering the qp_in struct struct blasfeo_dmat *dzduxt; // dzdux transposed struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables @@ -489,8 +494,7 @@ typedef struct ocp_nlp_workspace void **cost; // cost_workspace void **constraints; // constraints_workspace - // temp QP in & out (to be used as workspace in param sens) and merit line search - ocp_qp_in *tmp_qp_in; + // temp QP out ocp_qp_out *tmp_qp_out; // qp residuals @@ -555,6 +559,11 @@ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, void *in_, void *out_, void *opts_, void *mem_, void *work_, void *out_destination_, void *solver_mem, double alpha, bool full_step_dual); // +void ocp_nlp_convert_primaldelta_absdual_step_to_delta_step(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_out *out, ocp_qp_out *step); +// +double ocp_nlp_compute_anderson_gamma(ocp_nlp_workspace *work, ocp_qp_out *new_qp_step, ocp_qp_out *new_minus_old_qp_step); +// int ocp_nlp_precompute_common(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); @@ -624,6 +633,8 @@ void ocp_nlp_dump_qp_in_to_file(ocp_qp_in *qp_in, int sqp_iter, int soc); void ocp_nlp_common_print_iteration_header(); void ocp_nlp_common_print_iteration(int iter_count, ocp_nlp_res *nlp_res); +void ocp_nlp_update_variables_sqp_delta_primal_dual(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, double alpha, ocp_qp_out *step); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c index 77ac192d0a..64d7ba0324 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c @@ -157,8 +157,8 @@ void *ocp_nlp_globalization_fixed_step_memory_assign(void *config_, void *dims_, ************************************************/ int ocp_nlp_globalization_fixed_step_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size) { - ocp_nlp_config *nlp_config = nlp_config_; - ocp_nlp_dims *nlp_dims = nlp_dims_; + ocp_nlp_config *config = nlp_config_; + ocp_nlp_dims *dims = nlp_dims_; ocp_nlp_in *nlp_in = nlp_in_; ocp_nlp_out *nlp_out = nlp_out_; ocp_nlp_memory *nlp_mem = nlp_mem_; @@ -166,8 +166,53 @@ int ocp_nlp_globalization_fixed_step_find_acceptable_iterate(void *nlp_config_, ocp_nlp_opts *nlp_opts = nlp_opts_; ocp_nlp_globalization_fixed_step_opts *opts = nlp_opts->globalization; - nlp_config->step_update(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, opts->step_length, opts->globalization_opts->full_step_dual); - *step_size = opts->step_length; + ocp_qp_out *qp_out = nlp_mem->qp_out; + double alpha = opts->step_length; + + if (nlp_opts->with_anderson_acceleration) + { + // convert qp_out to delta primal-dual step + ocp_nlp_convert_primaldelta_absdual_step_to_delta_step(config, dims, nlp_out, qp_out); + if (nlp_mem->iter == 0) + { + // store in anderson_step, prev_qp_out + ocp_qp_out_copy(qp_out, nlp_mem->anderson_step); + // update variables (TODO: DDP primals are different) + ocp_nlp_update_variables_sqp_delta_primal_dual(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha, nlp_mem->anderson_step); + } + else + { + // tmp_qp_out = d_{k+1} - d_k: qp_step - prev_qp_out + ocp_qp_out_axpy(-1.0, nlp_mem->prev_qp_out, qp_out, nlp_work->tmp_qp_out); + // compute gamma + double gamma = ocp_nlp_compute_anderson_gamma(nlp_work, qp_out, nlp_work->tmp_qp_out); + /* update anderson_step */ + // anderson_step *= -gamma + ocp_qp_out_sc(-gamma, nlp_mem->anderson_step); + // anderson_step += alpha * gamma * prev_qp_out + ocp_qp_out_add(gamma*alpha, nlp_mem->prev_qp_out, nlp_mem->anderson_step); + // anderson_step += (alpha - alpha * gamma) * qp_out + ocp_qp_out_add(alpha-gamma*alpha, qp_out, nlp_mem->anderson_step); + // update variables (TODO: DDP primals are different) + ocp_nlp_update_variables_sqp_delta_primal_dual(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha, nlp_mem->anderson_step); + } + // store prev qp step + ocp_qp_out_copy(qp_out, nlp_mem->prev_qp_out); + // step norm + if (nlp_opts->log_primal_step_norm) + { + nlp_mem->primal_step_norm[nlp_mem->iter] = ocp_qp_out_compute_primal_nrm_inf(nlp_mem->anderson_step); + } + if (nlp_opts->log_dual_step_norm) + { + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_qp_out_compute_dual_nrm_inf(nlp_mem->anderson_step); + } + } + else + { + config->step_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, alpha, opts->globalization_opts->full_step_dual); + } + *step_size = alpha; return ACADOS_SUCCESS; } diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index d582c5faaa..cd82915811 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -818,12 +818,9 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, if (nlp_opts->log_primal_step_norm) nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } - if (nlp_opts->log_dual_step_norm) + if (nlp_opts->log_dual_step_norm && !nlp_opts->with_anderson_acceleration) { - if (nlp_opts->log_dual_step_norm) - { - nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); - } + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); } /* end solve QP */ diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index f44acb229a..528c943bd9 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1678,7 +1678,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, if (nlp_opts->log_primal_step_norm) nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } - if (nlp_opts->log_dual_step_norm) + if (nlp_opts->log_dual_step_norm && !nlp_opts->with_anderson_acceleration) { nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); } diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index e21e3b25a5..0d25a74898 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -277,7 +277,7 @@ double ocp_qp_out_compute_primal_nrm_inf(ocp_qp_out* qp_out) for (int i = 0; i <= N; i++) { blasfeo_dvecnrm_inf(nx[i]+nu[i]+2*ns[i], qp_out->ux+i, 0, &res_stage); - res += res_stage; + res = res > res_stage ? res : res_stage; } return res; } @@ -310,6 +310,182 @@ ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory) return qp_seed; } +double ocp_qp_out_compute_dual_nrm_inf(ocp_qp_out* qp_out) +{ + double res = 0; + double res_stage = 0; + ocp_qp_dims *dims = qp_out->dim; + int N = dims->N; + int *nx = dims->nx; + + for (int i = 0; i < N; i++) + { + blasfeo_dvecnrm_inf(nx[i+1], qp_out->pi+i, 0, &res_stage); + res = res > res_stage ? res : res_stage; + } + for (int i = 0; i <= N; i++) + { + int ni_stage = ocp_qp_dims_get_ni(dims, i); + blasfeo_dvecnrm_inf(2*ni_stage, qp_out->lam+i, 0, &res_stage); + res = res > res_stage ? res : res_stage; + } + return res; +} + + +void ocp_qp_out_copy(ocp_qp_out* from, ocp_qp_out* to) +{ + d_ocp_qp_sol_copy_all(from, to); +} + + +void ocp_qp_out_axpy(double alpha, ocp_qp_out* x, ocp_qp_out* y, ocp_qp_out* z) +{ + ocp_qp_dims *dims = x->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int ni_stage; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + ni_stage = ocp_qp_dims_get_ni(dims, i); + blasfeo_daxpy(nx[i]+nu[i]+2*ns[i], alpha, x->ux+i, 0, y->ux+i, 0, z->ux+i, 0); + blasfeo_daxpy(2*ni_stage, alpha, x->lam+i, 0, y->lam+i, 0, z->lam+i, 0); + if (i < N) + { + blasfeo_daxpy(nx[i+1], alpha, x->pi+i, 0, y->pi+i, 0, z->pi+i, 0); + } + blasfeo_daxpy(2*ni_stage, alpha, x->t+i, 0, y->t+i, 0, z->t+i, 0); + } +} + + +double ocp_qp_out_ddot(ocp_qp_out *x, ocp_qp_out *y, struct blasfeo_dvec *work_tmp_2ni) +{ + ocp_qp_dims *dims = x->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nbx = dims->nbx; + int *nbu = dims->nbu; + int *ng = dims->ng; + int *ns = dims->ns; + int tmp_nbg; + double out = 0.0; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + // primal + out += blasfeo_ddot(nx[i]+nu[i]+2*ns[i], x->ux+i, 0, y->ux+i, 0); + // dual + tmp_nbg = nbu[i]+nbx[i]+ng[i]; + /* setup multipliers as lower - upper bound */ + // work_tmp_2ni = x.lam_lower - x.lam_upper + blasfeo_daxpy(tmp_nbg, -1.0, x->lam+i, tmp_nbg, x->lam+i, 0, work_tmp_2ni, 0); + // work_tmp_2ni[nbg:] = y.lam_lower - y.lam_upper + blasfeo_daxpy(tmp_nbg, -1.0, y->lam+i, tmp_nbg, y->lam+i, 0, work_tmp_2ni, tmp_nbg); + /* compute ddot of split multipliers*/ + // add dot product + out += blasfeo_ddot(tmp_nbg, work_tmp_2ni, 0, work_tmp_2ni, tmp_nbg); + // multipliers wrt slack bounds + out += blasfeo_ddot(2*ns[i], x->lam+i, 2*tmp_nbg, y->lam+i, 2*tmp_nbg); + + if (i < N) + { + out += blasfeo_ddot(nx[i+1], x->pi+i, 0, y->pi+i, 0); + } + } + return out; +} + + +void ocp_qp_out_set_to_zero(ocp_qp_out* x) +{ + ocp_qp_dims *dims = x->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int ni_stage; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + ni_stage = ocp_qp_dims_get_ni(dims, i); + blasfeo_dvecse(nx[i]+nu[i]+2*ns[i], 0.0, x->ux+i, 0); + blasfeo_dvecse(2*ni_stage, 0.0, x->lam+i, 0); + if (i < N) + { + blasfeo_dvecse(nx[i+1], 0.0, x->pi+i, 0); + } + blasfeo_dvecse(2*ni_stage, 0.0, x->t+i, 0); + } +} + + +void ocp_qp_out_sc(double alpha, ocp_qp_out* x) +{ + ocp_qp_dims *dims = x->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int ni_stage; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + ni_stage = ocp_qp_dims_get_ni(dims, i); + blasfeo_dvecsc(nx[i]+nu[i]+2*ns[i], alpha, x->ux+i, 0); + blasfeo_dvecsc(2*ni_stage, alpha, x->lam+i, 0); + if (i < N) + { + blasfeo_dvecsc(nx[i+1], alpha, x->pi+i, 0); + } + blasfeo_dvecsc(2*ni_stage, alpha, x->t+i, 0); + } +} + + +// y += alpha * x +void ocp_qp_out_add(double alpha, ocp_qp_out* x, ocp_qp_out* y) +{ + ocp_qp_dims *dims = x->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int ni_stage; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + ni_stage = ocp_qp_dims_get_ni(dims, i); + blasfeo_dvecad(nx[i]+nu[i]+2*ns[i], alpha, x->ux+i, 0, y->ux+i, 0); + blasfeo_dvecad(2*ni_stage, alpha, x->lam+i, 0, y->lam+i, 0); + if (i < N) + { + blasfeo_dvecad(nx[i+1], alpha, x->pi+i, 0, y->pi+i, 0); + } + blasfeo_dvecad(2*ni_stage, alpha, x->t+i, 0, y->t+i, 0); + } +} + + /************************************************ * res diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index 6c660e2284..bf62e82874 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -160,6 +160,18 @@ acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims); ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory); // double ocp_qp_out_compute_primal_nrm_inf(ocp_qp_out* qp_out); +// +double ocp_qp_out_compute_dual_nrm_inf(ocp_qp_out* qp_out); +// +void ocp_qp_out_copy(ocp_qp_out* from, ocp_qp_out* to); +// +void ocp_qp_out_axpy(double alpha, ocp_qp_out* x, ocp_qp_out* y, ocp_qp_out* z); +// +void ocp_qp_out_set_to_zero(ocp_qp_out* qp_out); +void ocp_qp_out_add(double alpha, ocp_qp_out* x, ocp_qp_out* y); +void ocp_qp_out_sc(double alpha, ocp_qp_out* x); +double ocp_qp_out_ddot(ocp_qp_out *x, ocp_qp_out *y, struct blasfeo_dvec *work_tmp_2ni); + /* res */ // diff --git a/examples/acados_python/anderson_acceleration/anderson_scqp_experiment.py b/examples/acados_python/anderson_acceleration/anderson_scqp_experiment.py new file mode 100755 index 0000000000..200cd1455f --- /dev/null +++ b/examples/acados_python/anderson_acceleration/anderson_scqp_experiment.py @@ -0,0 +1,208 @@ +# +# 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.; +# + + +from dataclasses import dataclass +import numpy as np +from acados_template import AcadosOcpSolver, AcadosOcpFlattenedIterate, AcadosCasadiOcpSolver, plot_convergence + +from scqp_test_problem import build_acados_test_problem, plot_pendulum + +import matplotlib.pyplot as plt + +np.random.seed(0) + +@dataclass +class ExperimentAcadosSettings: + method: str + with_anderson_acceleration: bool + globalization: str = "FIXED_STEP" + max_iter: int = 200 + + def get_label(self): + label = self.method + if self.with_anderson_acceleration: + label = 'AA(1)-' + label + if self.globalization != "FIXED_STEP": + label = label + '-' + self.globalization + return label + +@dataclass +class ExperimentResults: + kkt_norms: np.ndarray + sol: AcadosOcpFlattenedIterate + + +def solve_with_acados(settings: ExperimentAcadosSettings, + initial_guess: AcadosOcpFlattenedIterate = None): + ocp = build_acados_test_problem(mode=settings.method, + with_anderson_acceleration=settings.with_anderson_acceleration, + globalization=settings.globalization, + max_iter=settings.max_iter, + ) + acados_solver = AcadosOcpSolver(ocp, verbose=False) + if initial_guess is not None: + acados_solver.load_iterate_from_flat_obj(initial_guess) + else: + pass + + # solve + status = acados_solver.solve() + acados_solver.print_statistics() + res_all = acados_solver.get_stats('res_all') + kkt_norms = np.linalg.norm(res_all, axis=1) + sol = acados_solver.store_iterate_to_flat_obj() + acados_solver.dump_last_qp_to_json("qp.json", overwrite=True) + # qp_diag = acados_solver.qp_diagnostics() + # print("qp_diag: ", qp_diag) + + cost = acados_solver.get_cost() + print("cost: ", cost) + + results = ExperimentResults(kkt_norms=kkt_norms, sol=sol) + + del acados_solver + return results + + +def solve_acados_formulation_with_ipopt(initial_guess: AcadosOcpFlattenedIterate = None, + mode="EXACT"): + ocp = build_acados_test_problem(mode=mode) + + solver = AcadosCasadiOcpSolver(ocp, use_acados_hessian=True) + + if initial_guess is not None: + solver.load_iterate_from_flat_obj(initial_guess) + solver.solve() + + sol = solver.store_iterate_to_flat_obj() + + results = ExperimentResults(kkt_norms=None, sol=sol) + return results + +def raise_test_failure_message(msg: str): + # print(f"ERROR: {msg}") + raise Exception(msg) + +def main(plot_sol=False): + ref_settings = ExperimentAcadosSettings(method='SCQP', with_anderson_acceleration=False, globalization='MERIT_BACKTRACKING') + ref_res = solve_with_acados(ref_settings) + sol = ref_res.sol + + # ipopt_res = solve_acados_formulation_with_ipopt(initial_guess=sol, mode='EXACT') + # NOTE: the above converges to a worse local optimum. + ipopt_res = solve_acados_formulation_with_ipopt(initial_guess=sol, mode='SCQP') + + if plot_sol: + ocp = build_acados_test_problem() + ocp.make_consistent() + xtraj = ipopt_res.sol.x.reshape((ocp.dims.N+1, ocp.dims.nx)) + utraj = ipopt_res.sol.u.reshape((ocp.dims.N, ocp.dims.nu)) + plot_pendulum(ocp.solver_options.shooting_nodes, + utraj, xtraj, plt_show=False) + plt.show() + + # start acados at ipopt solution + ref_res_2 = solve_with_acados(ref_settings, initial_guess=ipopt_res.sol) + + # compare with ipopt + print("compare: IPOPT vs acados") + diff_x = np.linalg.norm(ref_res.sol.x - ipopt_res.sol.x) + diff_u = np.linalg.norm(ref_res.sol.u - ipopt_res.sol.u) + print("diff x: ", diff_x) + print("diff u: ", diff_u) + # print("diff x: ", ref_res.sol.x - ipopt_res.sol.x) + # print("diff u: ", ref_res.sol.u - ipopt_res.sol.u) + print("compare: IPOPT vs acados started at IPOPT solution") + diff_x = np.linalg.norm(ref_res_2.sol.x - ipopt_res.sol.x) + diff_u = np.linalg.norm(ref_res_2.sol.u - ipopt_res.sol.u) + print("diff x: ", diff_x) + print("diff u: ", diff_u) + + # disturb again + sol = ref_res_2.sol + perturb_scale = 1e-3 + acados_guess = AcadosOcpFlattenedIterate( + x = sol.x + perturb_scale * np.random.randn(sol.x.shape[0]), + u = sol.u + perturb_scale * np.random.randn(sol.u.shape[0]), + z = sol.z + 0 * np.random.randn(sol.z.shape[0]), + sl = sol.sl + 0 * np.random.randn(sol.sl.shape[0]), + su = sol.su + 0 * np.random.randn(sol.su.shape[0]), + pi = sol.pi, # perturb_scale * np.random.randn(sol.pi.shape[0]), + lam = np.abs(sol.lam + 1 * np.random.randn(sol.lam.shape[0])), + ) + + # Experiment + settings = [ + ExperimentAcadosSettings(method='EXACT', with_anderson_acceleration=False, max_iter=100), + ExperimentAcadosSettings(method='GN', with_anderson_acceleration=False, max_iter=60), + ExperimentAcadosSettings(method='SCQP', with_anderson_acceleration=False, max_iter=60), + ExperimentAcadosSettings(method='SCQP', with_anderson_acceleration=True), + # ExperimentAcadosSettings(method='GN', with_anderson_acceleration=True), + ] + # Evaluation + labels = [s.get_label() for s in settings] + results: list[ExperimentResults] = [] + for i, setting in enumerate(settings): + res = solve_with_acados(setting, initial_guess=acados_guess) + results.append(res) + plot_convergence([r.kkt_norms for r in results], labels) + + # asserts to check behavior + res_anderson = results[-1] + res_exact = results[0] + res_scqp = results[-2] + + def assert_convergence_iterations(res, min_iter: int, max_iter: int, method: str): + n_iter = len(res.kkt_norms) + if n_iter > max_iter or n_iter < min_iter: + raise_test_failure_message(f"Number of iterations {n_iter} not in expected range [{min_iter}, {max_iter}] for {method}") + + # assert all solutions are the same + ref_sol = results[0].sol + for i, (res, setting) in enumerate(zip(results[1:], settings[1:])): + if setting.method == "GN": + print(f"Skipping GN comparison for {setting.get_label()} expect non converged result.") + continue + diff_x = np.linalg.norm(ref_sol.x - res.sol.x) + diff_u = np.linalg.norm(ref_sol.u - res.sol.u) + print(f"diff x for {labels[i+1]}: ", diff_x) + print(f"diff u for {labels[i+1]}: ", diff_u) + if diff_x > 1e-6 or diff_u > 1e-6: + raise_test_failure_message(f"Solution mismatch for {labels[i+1]}: x diff {diff_x}, u diff {diff_u}") + else: + print(f"Solution for {labels[i+1]} matches reference solution.") + + assert_convergence_iterations(res_anderson, 8, 12, "Anderson") + assert_convergence_iterations(res_exact, 4, 6, "Exact") + assert_convergence_iterations(res_scqp, 25, 40, "SCQP") + +if __name__ == "__main__": + main() diff --git a/examples/acados_python/anderson_acceleration/scqp_test_problem.py b/examples/acados_python/anderson_acceleration/scqp_test_problem.py new file mode 100644 index 0000000000..d846a329c5 --- /dev/null +++ b/examples/acados_python/anderson_acceleration/scqp_test_problem.py @@ -0,0 +1,217 @@ +# +# 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.; +# + + +import casadi as ca +import numpy as np +from acados_template import AcadosModel, AcadosOcp, ACADOS_INFTY, latexify_plot +import matplotlib.pyplot as plt + + +NX = 4 +GOAL_POSITION_RADIUS = (5e-02)**2 +LENGTH_PENDULUM = 0.8 + +def create_pendulum_model(): + M = 1.0 + m = 0.1 + g = 9.81 + states = ca.MX.sym("x", NX) + states_dot = ca.MX.sym('xdot', NX) + [p, pDot, theta, thetaDot] = ca.vertsplit(states) + controls = ca.MX.sym("u") + F = controls + denominator = M + m - m*ca.cos(theta)*ca.cos(theta) + f_x_p = pDot + f_x_pDot = (-m*LENGTH_PENDULUM*ca.sin(theta)*thetaDot*thetaDot + m*g*ca.cos(theta)*ca.sin(theta)+F)/denominator + f_x_theta = thetaDot + f_x_thetaDot = (-m*LENGTH_PENDULUM*ca.cos(theta)*ca.sin(theta)*thetaDot*thetaDot + F*ca.cos(theta)+(M+m)*g*ca.sin(theta))/(LENGTH_PENDULUM*denominator) + + f_x = ca.vertcat(f_x_p, f_x_pDot, f_x_theta, f_x_thetaDot) + f_impl = states_dot - f_x + + model = AcadosModel() + model.f_impl_expr = f_impl + model.f_expl_expr = f_x + model.x = states + model.xdot = states_dot + model.u = controls + model.name = 'pendulum' + return model + +def pendulum_position(p, theta): + return ca.vertcat(p-LENGTH_PENDULUM*ca.sin(theta), LENGTH_PENDULUM*ca.cos(theta)) + +def pendulum_final_position_constraint(p, theta): + pendulum_final_position = pendulum_position(p, theta) + return ca.sumsqr(pendulum_final_position - ca.vertcat(LENGTH_PENDULUM, LENGTH_PENDULUM)) - GOAL_POSITION_RADIUS + +def build_acados_test_problem(mode='GN', with_anderson_acceleration=False, globalization="FIXED_STEP", max_iter=400) -> AcadosOcp: + print(f"Building acados test problem with mode {mode} and with_anderson_acceleration {with_anderson_acceleration}, {globalization}") + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = create_pendulum_model() + + ocp.model = model + + # Define cost + R_mat = np.array([[1e-4]]) + + ocp.cost.cost_type_0 = 'LINEAR_LS' + ocp.cost.W_0 = R_mat + ocp.cost.Vx_0 = np.zeros((1, NX)) + ocp.cost.Vu_0 = np.array([[1]]) + ocp.cost.yref_0 = np.zeros((1, )) + + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.W = R_mat + ocp.cost.Vx = np.zeros((1, NX)) + ocp.cost.Vu = np.array([[1]]) + ocp.cost.yref = np.zeros((1, )) + + # Define constraints + ocp.constraints.x0 = np.array([0.0, 0.0, np.pi, 0.0]) + + # Convex over Nonlinear Constraints + if mode in ['GN', 'EXACT']: + ocp.model.con_h_expr_e = pendulum_final_position_constraint(model.x[0], model.x[2]) + ocp.constraints.lh_e = np.array([-ACADOS_INFTY]) + ocp.constraints.uh_e = np.array([0.0]) + + elif mode == 'SCQP': + r = ca.MX.sym('r', 2, 1) + ocp.model.con_phi_expr_e = ca.sumsqr(r) + ocp.model.con_r_in_phi_e = r + ocp.model.con_r_expr_e = pendulum_position(model.x[0], model.x[2]) - ca.vertcat(LENGTH_PENDULUM, LENGTH_PENDULUM) + ocp.constraints.lphi_e = np.array([-ACADOS_INFTY]) + ocp.constraints.uphi_e = np.array([GOAL_POSITION_RADIUS]) + else: + raise ValueError("Wrong mode name!") + + # set solver options + N_horizon = 20 + dt = 0.05 + + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_max_iter = 400 + ocp.solver_options.qp_solver_iter_max = 1000 + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.sim_method_num_steps = 20 + ocp.solver_options.sim_method_num_stages = 4 + ocp.solver_options.N_horizon = N_horizon + ocp.solver_options.tf = dt*N_horizon + ocp.solver_options.tol = 1e-12 + ocp.solver_options.qp_tol = 1e-1 * ocp.solver_options.tol + # ocp.solver_options.cost_scaling = np.ones((N_horizon+1, )) + ocp.solver_options.with_anderson_acceleration = with_anderson_acceleration + ocp.solver_options.globalization = globalization + ocp.solver_options.qp_solver_ric_alg = 0 + ocp.solver_options.qp_solver_cond_ric_alg = 0 + ocp.solver_options.nlp_solver_max_iter = max_iter + # ocp.solver_options.qp_solver = 'FULL_CONDENSING_DAQP' + ocp.solver_options.hessian_approx = 'EXACT' if mode == "EXACT" else 'GAUSS_NEWTON' + if mode == "EXACT": + ocp.solver_options.exact_hess_dyn = 1 + + if max_iter < 10: + ocp.solver_options.print_level = 4 + + return ocp + + + +def plot_pendulum(shooting_nodes, U, X_true, X_est=None, Y_measured=None, latexify=True, plt_show=True, X_true_label=None, + time_label='$t$', x_labels=['$x$', r'$\theta$', '$v$', r'$\dot{\theta}$'], + title = None + ): + """ + Params: + shooting_nodes: time values of the discretization + u_max: maximum absolute value of u + U: arrray with shape (N_sim-1, nu) or (N_sim, nu) + X_true: arrray with shape (N_sim, nx) + X_est: arrray with shape (N_sim-N_mhe, nx) + Y_measured: array with shape (N_sim, ny) + latexify: latex style plots + """ + + if latexify: + latexify_plot() + + WITH_ESTIMATION = X_est is not None and Y_measured is not None + + N_sim = X_true.shape[0] + nx = X_true.shape[1] + + Tf = shooting_nodes[N_sim-1] + t = shooting_nodes + + Ts = t[1] - t[0] + if WITH_ESTIMATION: + N_mhe = N_sim - X_est.shape[0] + t_mhe = np.linspace(N_mhe * Ts, Tf, N_sim-N_mhe) + + plt.figure() + plt.subplot(nx+1, 1, 1) + line, = plt.step(t, np.append([U[0]], U)) + if X_true_label is not None: + line.set_label(X_true_label) + else: + line.set_color('r') + if title is not None: + plt.title(title) + plt.ylabel('$u$') + plt.xlabel(time_label) + plt.grid() + + + for i in range(nx): + plt.subplot(nx+1, 1, i+2) + line, = plt.plot(t, X_true[:, i], label='true') + if X_true_label is not None: + line.set_label(X_true_label) + + if WITH_ESTIMATION: + plt.plot(t_mhe, X_est[:, i], '--', label='estimated') + plt.plot(t, Y_measured[:, i], 'x', label='measured') + + plt.ylabel(x_labels[i]) + plt.xlabel('$t$') + plt.grid() + plt.legend(loc=1) + + plt.subplots_adjust(left=None, bottom=None, right=None, top=None, hspace=0.4) + + if plt_show: + plt.show() + return diff --git a/examples/acados_python/furuta_pendulum/convergence_experiment.py b/examples/acados_python/furuta_pendulum/convergence_experiment.py new file mode 100644 index 0000000000..51c55a321d --- /dev/null +++ b/examples/acados_python/furuta_pendulum/convergence_experiment.py @@ -0,0 +1,108 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +from furuta_common import setup_ocp_solver +import numpy as np + +from acados_template import AcadosOcpFlattenedIterate, plot_convergence, plot_contraction_rates +from typing import Tuple +import matplotlib.pyplot as plt + + +def test_solver(with_anderson_acceleration: bool) -> Tuple[AcadosOcpFlattenedIterate, np.ndarray]: + x0 = np.array([0.0, np.pi, 0.0, 0.0]) + umax = .45 + + Tf = .350 # total prediction time + N_horizon = 8 # number of shooting intervals + dt_0 = 0.025 # sampling time = length of first shooting interval + + solver = setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, with_anderson_acceleration=with_anderson_acceleration, nlp_solver_max_iter = 500, tol = 1e-8) + + status = solver.solve() + solver.print_statistics() + solution = solver.store_iterate_to_flat_obj() + + res_all = solver.get_stats('res_all') + kkt_norms = np.linalg.norm(res_all, axis=1) + + return solution, kkt_norms + +def raise_test_failure_message(msg: str): + # print(f"ERROR: {msg}") + raise Exception(msg) + +def main(): + # test with anderson acceleration + kkt_norm_list = [] + contraction_rates_list = [] + sol_list = [] + labels = [] + for with_anderson_acceleration in [True, False]: + sol, kkt_norms = test_solver(with_anderson_acceleration=with_anderson_acceleration) + # compute contraction rates + contraction_rates = kkt_norms[1:-1]/kkt_norms[0:-2] + # append results + kkt_norm_list.append(kkt_norms) + contraction_rates_list.append(contraction_rates) + sol_list.append(sol) + labels.append("AA(1)-GN" if with_anderson_acceleration else "GN") + # checks + n_iter = len(kkt_norms) + if with_anderson_acceleration: + assert n_iter < 27, f"Expected less than 27 iterations with Anderson acceleration, got {n_iter}" + else: + assert n_iter > 200, f"Expected more than 200 iterations without Anderson acceleration, got {n_iter}" + + # checks + ref_sol = sol_list[0] + for i, sol in enumerate(sol_list[1:]): + if not ref_sol.allclose(sol, atol=1e-6): + raise_test_failure_message(f"Solution mismatch for {labels[i]}: {sol} vs {ref_sol}") + else: + print(f"Solution for {labels[i]} matches reference solution.") + + # plot results + plot_convergence( + kkt_norm_list, + labels, + # fig_filename="convergence_furuta_pendulum.png" + ) + plot_contraction_rates( + contraction_rates_list, + labels, + # fig_filename="contraction_rates_furuta_pendulum.png" + ) + plt.show() + +if __name__ == "__main__": + main() + diff --git a/examples/acados_python/furuta_pendulum/furuta_model.py b/examples/acados_python/furuta_pendulum/furuta_common.py similarity index 50% rename from examples/acados_python/furuta_pendulum/furuta_model.py rename to examples/acados_python/furuta_pendulum/furuta_common.py index 5532af7fa6..52bb649489 100644 --- a/examples/acados_python/furuta_pendulum/furuta_model.py +++ b/examples/acados_python/furuta_pendulum/furuta_common.py @@ -2,6 +2,9 @@ import casadi as ca import numpy as np +from acados_template import AcadosOcp, AcadosOcpSolver +import scipy.linalg + def get_furuta_model(): @@ -78,5 +81,67 @@ def get_furuta_model(): return model +def setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, RTI=False, timeout_max_time=0.0, heuristic="ZERO", with_anderson_acceleration=False, nlp_solver_max_iter = 20, tol = 1e-6): + ocp = AcadosOcp() + + model = get_furuta_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + + ocp.solver_options.N_horizon = N_horizon + + # set cost module + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + Q_mat = np.diag([50., 500., 1., 1.]) + R_mat = np.diag([1e3]) + + ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) + ocp.cost.W_e = Q_mat + + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.model.cost_y_expr_e = model.x + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) + + # set constraints + ocp.constraints.lbu = np.array([-umax]) + ocp.constraints.ubu = np.array([+umax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = x0 + + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'ERK' + + # NOTE we use a nonuniform grid! + ocp.solver_options.time_steps = np.array([dt_0] + [(Tf-dt_0)/(N_horizon-1)]*(N_horizon-1)) + ocp.solver_options.sim_method_num_steps = np.array([1] + [2]*(N_horizon-1)) + ocp.solver_options.levenberg_marquardt = 1e-6 + ocp.solver_options.nlp_solver_max_iter = nlp_solver_max_iter + ocp.solver_options.with_anderson_acceleration = with_anderson_acceleration + + ocp.solver_options.nlp_solver_type = 'SQP_RTI' if RTI else 'SQP' + ocp.solver_options.qp_solver_cond_N = N_horizon + ocp.solver_options.tol = tol + + ocp.solver_options.tf = Tf + + # timeout + ocp.solver_options.timeout_max_time = timeout_max_time + ocp.solver_options.timeout_heuristic = heuristic + + solver_json = 'acados_ocp_' + model.name + '.json' + ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False) + + return ocp_solver + + if __name__ == "__main__": get_furuta_model() diff --git a/examples/acados_python/furuta_pendulum/integrator_experiment.py b/examples/acados_python/furuta_pendulum/integrator_experiment.py index faf35dade4..a01c75da48 100644 --- a/examples/acados_python/furuta_pendulum/integrator_experiment.py +++ b/examples/acados_python/furuta_pendulum/integrator_experiment.py @@ -32,7 +32,7 @@ # authors: Katrin Baumgaertner, Jonathan Frey import os, pickle -from furuta_model import get_furuta_model +from furuta_common import get_furuta_model import numpy as np from matplotlib.lines import Line2D diff --git a/examples/acados_python/furuta_pendulum/main_closed_loop.py b/examples/acados_python/furuta_pendulum/main_closed_loop.py index a8ea584735..3ada3182e5 100644 --- a/examples/acados_python/furuta_pendulum/main_closed_loop.py +++ b/examples/acados_python/furuta_pendulum/main_closed_loop.py @@ -29,82 +29,19 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver from utils import plot_furuta_pendulum, plot_time_per_solve -from furuta_model import get_furuta_model +from furuta_common import get_furuta_model, setup_ocp_solver from integrator_experiment import setup_acados_integrator, IntegratorSetting import numpy as np -import scipy.linalg -from casadi import vertcat -def setup(x0, umax, dt_0, N_horizon, Tf, RTI=False, timeout_max_time=0, heuristic="ZERO"): - ocp = AcadosOcp() - - model = get_furuta_model() - ocp.model = model - - nx = model.x.rows() - nu = model.u.rows() - ny = nx + nu - ny_e = nx - - ocp.solver_options.N_horizon = N_horizon - - # set cost module - ocp.cost.cost_type = 'NONLINEAR_LS' - ocp.cost.cost_type_e = 'NONLINEAR_LS' - - Q_mat = np.diag([50., 500., 1., 1.]) - R_mat = np.diag([1e3]) - - ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) - ocp.cost.W_e = Q_mat - - ocp.model.cost_y_expr = vertcat(model.x, model.u) - ocp.model.cost_y_expr_e = model.x - ocp.cost.yref = np.zeros((ny, )) - ocp.cost.yref_e = np.zeros((ny_e, )) - - # set constraints - ocp.constraints.lbu = np.array([-umax]) - ocp.constraints.ubu = np.array([+umax]) - ocp.constraints.idxbu = np.array([0]) - - ocp.constraints.x0 = x0 - - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES - ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'ERK' - - # NOTE we use a nonuniform grid! - ocp.solver_options.time_steps = np.array([dt_0] + [(Tf-dt_0)/(N_horizon-1)]*(N_horizon-1)) - ocp.solver_options.sim_method_num_steps = np.array([1] + [2]*(N_horizon-1)) - ocp.solver_options.levenberg_marquardt = 1e-6 - ocp.solver_options.nlp_solver_max_iter = 20 - - ocp.solver_options.nlp_solver_type = 'SQP_RTI' if RTI else 'SQP' - ocp.solver_options.qp_solver_cond_N = N_horizon - - ocp.solver_options.tf = Tf - - # timeout - ocp.solver_options.timeout_max_time = timeout_max_time - ocp.solver_options.timeout_heuristic = heuristic - - solver_json = 'acados_ocp_' + model.name + '.json' - ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json) - - # setup plant simulator +def get_plant_integrator_settings(): integrator_settings = IntegratorSetting(integrator_type="IRK", num_stages=2, num_steps=2, newton_iter=20, newton_tol=1e-10, ) - integrator = setup_acados_integrator(model, dt_0, integrator_settings) - - return ocp_solver, integrator - + return integrator_settings def main(use_RTI=False, timeout_max_time=0., heuristic="ZERO"): @@ -115,7 +52,11 @@ def main(use_RTI=False, timeout_max_time=0., heuristic="ZERO"): N_horizon = 8 # number of shooting intervals dt_0 = 0.025 # sampling time = length of first shooting interval - ocp_solver, integrator = setup(x0, umax, dt_0, N_horizon, Tf, use_RTI, timeout_max_time, heuristic) + ocp_solver = setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, use_RTI, timeout_max_time, heuristic) + # setup plant simulator + integrator_settings = get_plant_integrator_settings() + model = get_furuta_model() + integrator = setup_acados_integrator(model, dt_0, integrator_settings) nx = ocp_solver.acados_ocp.dims.nx nu = ocp_solver.acados_ocp.dims.nu @@ -161,7 +102,6 @@ def main(use_RTI=False, timeout_max_time=0., heuristic="ZERO"): else: # solve ocp and get next control input simU[i,:] = ocp_solver.solve_for_x0(x0_bar = simX[i, :], fail_on_nonzero_status=False) - t[i] = ocp_solver.get_stats('time_tot') # simulate system @@ -190,10 +130,10 @@ def main(use_RTI=False, timeout_max_time=0., heuristic="ZERO"): if __name__ == '__main__': main(use_RTI=False, timeout_max_time=0.) - main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="ZERO") - main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="LAST") - main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="MAX_CALL") - main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="MAX_OVERALL") + # main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="ZERO") + # main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="LAST") + # main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="MAX_CALL") + # main(use_RTI=False, timeout_max_time=1*1e-3, heuristic="MAX_OVERALL") - main(use_RTI=True) # timeout not implemented for RTI + # main(use_RTI=True) # timeout not implemented for RTI diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index fd324c2a13..8bf61d4534 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -1105,6 +1105,16 @@ function make_consistent(self, is_mocp_phase) self.zoro_description.process(); end + % Anderson acceleration + if opts.with_anderson_acceleration + if strcmp(opts.nlp_solver_type, "DDP") + error('Anderson acceleration not supported for DDP solver.'); + end + if ~strcmp(opts.globalization, "FIXED_STEP") + error('Anderson acceleration only supported for FIXED_STEP globalization for now.'); + end + end + % check terminal stage fields = {'cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', ... 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', ... diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 4d2465bbbc..03e89f12ec 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -119,6 +119,7 @@ log_dual_step_norm store_iterates eval_residual_at_max_iter + with_anderson_acceleration timeout_max_time timeout_heuristic @@ -232,6 +233,7 @@ obj.log_dual_step_norm = 0; obj.store_iterates = false; obj.eval_residual_at_max_iter = []; + obj.with_anderson_acceleration = 0; obj.timeout_max_time = 0.; obj.timeout_heuristic = 'ZERO'; diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index b1ceb96989..cbaf54d92f 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -53,7 +53,7 @@ from .builders import ocp_get_default_cmake_builder, sim_get_default_cmake_builder -from .plot_utils import latexify_plot +from .plot_utils import latexify_plot, plot_convergence, plot_contraction_rates from .penalty_utils import symmetric_huber_penalty, one_sided_huber_penalty, huber_loss diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index bc507aa6d9..bfebf3acb7 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1094,6 +1094,13 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None if opts.nlp_solver_warm_start_first_qp_from_nlp and (opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon): raise NotImplementedError('nlp_solver_warm_start_first_qp_from_nlp only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.') + # Anderson acceleration + if opts.with_anderson_acceleration: + if opts.nlp_solver_type == "DDP": + raise NotImplementedError('Anderson acceleration not supported for DDP solver.') + if opts.globalization != "FIXED_STEP": + raise NotImplementedError('Anderson acceleration only supported for FIXED_STEP globalization for now.') + # check terminal stage for field in ('cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index e6a9981807..76eb8e09d4 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -145,6 +145,8 @@ def __init__(self): self.__custom_update_copy = True self.__num_threads_in_batch_solve: int = 1 self.__with_batch_functionality: bool = False + self.__with_anderson_acceleration: bool = False + @property def qp_solver(self): @@ -594,6 +596,17 @@ def as_rti_iter(self): """ return self.__as_rti_iter + @property + def with_anderson_acceleration(self): + """ + Determines if Anderson acceleration is performed. + Only supported for globalization == 'FIXED_STEP'. + + Type: bool + Default: False + """ + return self.__with_anderson_acceleration + @property def as_rti_level(self): @@ -1236,7 +1249,7 @@ def num_threads_in_batch_solve(self): Default: 1. """ return self.__num_threads_in_batch_solve - + @property def with_batch_functionality(self): """ @@ -1780,6 +1793,12 @@ def as_rti_iter(self, as_rti_iter): else: raise ValueError('Invalid as_rti_iter value. as_rti_iter must be a nonnegative int.') + @with_anderson_acceleration.setter + def with_anderson_acceleration(self, with_anderson_acceleration): + if not isinstance(with_anderson_acceleration, bool): + raise TypeError('Invalid with_anderson_acceleration value, must be bool.') + self.__with_anderson_acceleration = with_anderson_acceleration + @as_rti_level.setter def as_rti_level(self, as_rti_level): if as_rti_level in [0, 1, 2, 3, 4]: @@ -2013,7 +2032,7 @@ def with_batch_functionality(self, with_batch_functionality): if isinstance(with_batch_functionality, bool): self.__with_batch_functionality = with_batch_functionality else: - raise Exception('Invalid with_batch_functionality value. Expected bool.') + raise TypeError('Invalid with_batch_functionality value. Expected bool.') def set(self, attr, value): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index aea347fb1c..1957b2083a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1690,6 +1690,12 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: else: raise KeyError(f"res_comp_all is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") + elif field_ == 'res_all': + return np.concatenate((np.atleast_2d(self.get_stats('res_stat_all')), + np.atleast_2d(self.get_stats('res_eq_all')), + np.atleast_2d(self.get_stats('res_ineq_all')), + np.atleast_2d(self.get_stats('res_comp_all'))), axis=0).transpose() + else: raise ValueError(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' + f'\n Possible values are {fields}.') 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 4b2e68f927..d7923e0acd 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 @@ -2392,6 +2392,9 @@ ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "allow_direction_mode_switch_to_no ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_only_available_residuals", &rti_log_only_available_residuals); {%- endif %} + bool with_anderson_acceleration = {{ solver_options.with_anderson_acceleration }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); + int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); 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 f1ce6e7f33..e1d6f83f8c 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 @@ -2533,6 +2533,9 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_only_available_residuals", &rti_log_only_available_residuals); {%- endif %} + bool with_anderson_acceleration = {{ solver_options.with_anderson_acceleration }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); + int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); diff --git a/interfaces/acados_template/acados_template/plot_utils.py b/interfaces/acados_template/acados_template/plot_utils.py index b1b83c90bf..354ba62121 100644 --- a/interfaces/acados_template/acados_template/plot_utils.py +++ b/interfaces/acados_template/acados_template/plot_utils.py @@ -31,6 +31,10 @@ import shutil import matplotlib +import matplotlib.pyplot as plt +import numpy as np + +from typing import Optional def latexify_plot() -> None: text_usetex = True if shutil.which('latex') else False @@ -46,4 +50,53 @@ def latexify_plot() -> None: } matplotlib.rcParams.update(params) - return \ No newline at end of file + return + + +def plot_convergence(residuals: list, + list_labels: list, + xlim: Optional[float] = None, + ylim: tuple = None, + fig_filename: str = None): + latexify_plot() + + assert len(residuals) == len(list_labels), f"Lists of data and labels do not have the same length, got {len(residuals)} and {len(list_labels)}" + + plt.figure(figsize=(4.5, 3.0)) + for i in range(len(residuals)): + iters = np.arange(0, len(residuals[i])) + data = np.array(residuals[i]).squeeze() + plt.semilogy(iters, data, label=list_labels[i]) + plt.legend(loc='best') + plt.xlabel("iteration number") + plt.ylabel("KKT residual norm") + if ylim is not None: + plt.ylim(ylim) + if xlim is not None: + plt.xlim(0, xlim) + else: + plt.xlim(0, max([len(data) for data in residuals])) + plt.tight_layout() + plt.grid() + if fig_filename is not None: + plt.savefig(fig_filename, dpi=300, bbox_inches='tight', pad_inches=0.01) + plt.show() + +def plot_contraction_rates(rates_list: list, + labels: list, + fig_filename: str = None): + latexify_plot() + plt.figure(figsize=(4.5, 3.0)) + for rates, label in zip(rates_list, labels): + iters = np.arange(0, len(rates)) + plt.plot(iters, rates, label=label) + plt.legend(loc='best') + plt.xlabel("iteration number") + plt.ylabel("empirical contraction rate") + plt.xlim(0, max([len(data) for data in rates_list])) + plt.ylim(0, 1.1) + plt.tight_layout() + plt.grid() + if fig_filename is not None: + plt.savefig(fig_filename, dpi=300, bbox_inches='tight', pad_inches=0.01) + plt.show() From 2826ee6957828cd594ab3e34d5ea09ee07f6de58 Mon Sep 17 00:00:00 2001 From: JasperHoffmann <41264755+JasperHoffmann@users.noreply.github.com> Date: Thu, 12 Jun 2025 16:18:33 +0200 Subject: [PATCH 069/164] OCP sensitivity check (#1548) This PR moves the sensitivity check to the `AcadosOcp` class: 1. `AcadosOcp` gets a `ensure_solution_sensitivities_available` method. 2. `AcadosOcpOptions` gets a private method for checking whether the solver options are correctly set. 3. `AcadosModel` gets a private method to check whether the Hessian is custom. For `AcadosMultiPhaseOcp` the function `_ensure_solution_sensitivities_available` raises an error, as the sensitivity functionality is not interfaced yet. --- .../acados_template/acados_model.py | 6 +++ .../acados_template/acados_ocp.py | 15 ++++++++ .../acados_ocp_batch_solver.py | 2 +- .../acados_template/acados_ocp_options.py | 19 ++++++++++ .../acados_template/acados_ocp_solver.py | 38 ++++--------------- 5 files changed, 49 insertions(+), 31 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index e3aa7415f7..202998b592 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -960,3 +960,9 @@ def reformulate_with_polynomial_control(self, degree: int) -> None: self.name = self.name + f"_d{degree}" return evaluate_polynomial_u_fun + + + def _has_custom_hess(self) -> bool: + return not (is_empty(self.cost_expr_ext_cost_custom_hess_0) and + is_empty(self.cost_expr_ext_cost_custom_hess) and + is_empty(self.cost_expr_ext_cost_custom_hess_e)) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index bfebf3acb7..e635a5d008 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2131,6 +2131,21 @@ def detect_cost_type(self, model: AcadosModel, cost: AcadosOcpCost, dims: Acados print('--------------------------------------------------------------') + def ensure_solution_sensitivities_available(self, parametric=True) -> None: + """ + Check if the options are set correctly for calculating sensitivities. + + :param parametric: if True, check also if parametric sensitivities are available. + + :raises NotImplementedError: if the QP solver is not HPIPM. + :raises ValueError: if the Hessian approximation or regularization method is not set correctly for parametric sensitivities. + """ + has_custom_hess = self.model._has_custom_hess() + + self.solver_options._ensure_solution_sensitivities_available( + parametric, + has_custom_hess + ) def get_initial_cost_expression(self): model = self.model diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 6f9068ed58..80b5948e79 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -218,7 +218,7 @@ def eval_adjoint_solution_sensitivity(self, N_horizon = self.__ocp_solvers[0].acados_ocp.solver_options.N_horizon for n in range(n_batch): - self.__ocp_solvers[n]._sanity_check_solution_sensitivities() + self.__ocp_solvers[n]._ensure_solution_sensitivities_available() nx = self.__acados_lib.ocp_nlp_dims_get_from_attr( self.__ocp_solvers[0].nlp_config, self.__ocp_solvers[0].nlp_dims, self.__ocp_solvers[0].nlp_out, 0, "x".encode('utf-8')) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 76eb8e09d4..8d2962fa3f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -2037,3 +2037,22 @@ def with_batch_functionality(self, with_batch_functionality): def set(self, attr, value): setattr(self, attr, value) + + def _ensure_solution_sensitivities_available(self, parametric: bool = True, has_custom_hess: bool = False): + if not self.qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']: + raise NotImplementedError("Parametric sensitivities are only available with HPIPM as QP solver.") + + if not ( + self.hessian_approx == 'EXACT' and + self.regularize_method == 'NO_REGULARIZE' and + self.levenberg_marquardt == 0 and + self.exact_hess_constr == 1 and + self.exact_hess_cost == 1 and + self.exact_hess_dyn == 1 and + self.fixed_hess == 0 and + has_custom_hess is False + ): + raise ValueError("Parametric sensitivities are only correct if an exact Hessian is used!") + + if parametric and not self.with_solution_sens_wrt_params: + raise ValueError("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 1957b2083a..da71be1029 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -246,27 +246,10 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__has_x0 = acados_ocp_json['constraints']['has_x0'] self.__nsbu_0 = acados_ocp_json['dims']['nsbu'] self.__nbxe_0 = acados_ocp_json['dims']['nbxe_0'] - has_custom_hess = not (is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess_0']) and - is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess']) and - is_empty(acados_ocp_json['model']['cost_expr_ext_cost_custom_hess_e'])) elif self.__problem_class == "MOCP": self.__has_x0 = acados_ocp_json['constraints'][0]['has_x0'] self.__nsbu_0 = acados_ocp_json['phases_dims'][0]['nsbu'] self.__nbxe_0 = acados_ocp_json['phases_dims'][0]['nbxe_0'] - has_custom_hess = any([not (is_empty(model['cost_expr_ext_cost_custom_hess_0']) and - is_empty(model['cost_expr_ext_cost_custom_hess']) and - is_empty(model['cost_expr_ext_cost_custom_hess_e'])) for model in acados_ocp_json['model']]) - - self.__uses_exact_hessian = ( - self.__solver_options["hessian_approx"] == 'EXACT' and - self.__solver_options["regularize_method"] == 'NO_REGULARIZE' and - self.__solver_options["levenberg_marquardt"] == 0 and - self.__solver_options["exact_hess_constr"] == 1 and - self.__solver_options["exact_hess_cost"] == 1 and - self.__solver_options["exact_hess_dyn"] == 1 and - self.__solver_options["fixed_hess"] == 0 and - not has_custom_hess - ) acados_lib_path = acados_ocp_json['acados_lib_path'] code_export_directory = acados_ocp_json['code_export_directory'] @@ -694,16 +677,11 @@ def get_optimal_value_gradient(self, with_respect_to: str = "initial_state") -> return self.eval_and_get_optimal_value_gradient(with_respect_to) - def _sanity_check_solution_sensitivities(self, parametric=True) -> None: - if not (self.__solver_options["qp_solver"] == 'FULL_CONDENSING_HPIPM' or - self.__solver_options["qp_solver"] == 'PARTIAL_CONDENSING_HPIPM'): - raise NotImplementedError("Parametric sensitivities are only available with HPIPM as QP solver.") - - if not self.__uses_exact_hessian: - raise ValueError("Parametric sensitivities are only correct if an exact Hessian is used!") + def _ensure_solution_sensitivities_available(self, parametric=True) -> None: + if self.__problem_class == "MOCP": + raise ValueError("Solution sensitivities are not implemented for multiphase OCPs.") - if parametric and not self.__solver_options["with_solution_sens_wrt_params"]: - raise ValueError("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") + self.acados_ocp.ensure_solution_sensitivities_available(parametric=parametric) # type: ignore def eval_solution_sensitivity(self, @@ -773,14 +751,14 @@ def eval_solution_sensitivity(self, ngrad = nx field = "ex" if sanity_checks: - self._sanity_check_solution_sensitivities(parametric=False) + self._ensure_solution_sensitivities_available(parametric=False) elif with_respect_to == "p_global": np_global = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "p_global".encode('utf-8')) ngrad = np_global field = "p_global" if sanity_checks: - self._sanity_check_solution_sensitivities() + self._ensure_solution_sensitivities_available() # compute jacobians wrt params in all modules t0 = time.time() @@ -915,7 +893,7 @@ def eval_adjoint_solution_sensitivity(self, n_seeds = seed_u[0][1].shape[1] if sanity_checks: - self._sanity_check_solution_sensitivities() + self._ensure_solution_sensitivities_available() nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8')) nu = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "u".encode('utf-8')) @@ -981,7 +959,7 @@ def eval_param_sens(self, index: int, stage: int=0, field="ex"): print("WARNING: eval_param_sens() is deprecated. Please use eval_solution_sensitivity() instead!") - self._sanity_check_solution_sensitivities(False) + self._ensure_solution_sensitivities_available(False) field = field.encode('utf-8') From 1b5682d482f636c56fab61fa0143e309c9dabc79 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 16 Jun 2025 10:27:42 +0200 Subject: [PATCH 070/164] Actions windows matlab issue (#1549) --- .github/workflows/full_build_windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/full_build_windows.yml b/.github/workflows/full_build_windows.yml index 930b559353..eafc14f406 100644 --- a/.github/workflows/full_build_windows.yml +++ b/.github/workflows/full_build_windows.yml @@ -71,7 +71,7 @@ jobs: with: release: R2021a products: Simulink Simulink_Test - cache: true + cache: false - name: Setup MATLAB with MinGW Compiler uses: matlab-actions/run-command@v2 From d1c2737463c71101dd60be867c97b5f98f42a9fa Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Thu, 19 Jun 2025 11:19:29 +0200 Subject: [PATCH 071/164] MATLAB: implement `AcadosSimSolver.simulate()` (#1550) Analogous to the [Python version](https://github.com/acados/acados/blob/8ff3477761d5272b0e5735b97cf3fd821a1bd0b3/interfaces/acados_template/acados_template/acados_sim_solver.py#L241). I added a comment in the `getting_started` examples, for new users. --- .../minimal_example_closed_loop.m | 12 +------ .../getting_started/minimal_example_sim.m | 3 ++ .../acados_matlab_octave/AcadosSimSolver.m | 36 +++++++++++++++++++ 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m b/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m index 7b2e1d1f2b..60e7766f27 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m @@ -185,18 +185,8 @@ u0 = ocp_solver.get('u', 0); status = ocp_solver.get('status'); % 0 - success - % set initial state for the simulation - sim_solver.set('x', x0); - sim_solver.set('u', u0); - % simulate one step - sim_status = sim_solver.solve(); - if sim_status ~= 0 - disp(['acados integrator returned error status ', num2str(sim_status)]) - end - - % get simulated state - x_sim(:,i+1) = sim_solver.get('xn'); + x_sim(:,i+1) = sim_solver.simulate(x0, u0); u_sim(:,i) = u0; end diff --git a/examples/acados_matlab_octave/getting_started/minimal_example_sim.m b/examples/acados_matlab_octave/getting_started/minimal_example_sim.m index 418e2daf81..7262bc0803 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_sim.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_sim.m @@ -77,6 +77,9 @@ % get simulated state x_sim(:,ii+1) = sim_solver.get('xn'); + + % one can also use: + % x_sim(:,ii+1) = sim_solver.simulate(x_sim(:,ii), u0); end % forward sensitivities ( dxn_d[x0,u] ) diff --git a/interfaces/acados_matlab_octave/AcadosSimSolver.m b/interfaces/acados_matlab_octave/AcadosSimSolver.m index fabc4e411f..6cec697d80 100644 --- a/interfaces/acados_matlab_octave/AcadosSimSolver.m +++ b/interfaces/acados_matlab_octave/AcadosSimSolver.m @@ -144,6 +144,42 @@ function set(obj, field, value) end + function x_next = simulate(obj, x, u, z, xdot, p) + % Simulate the system forward for the given x, u, p and return x_next. + % The values xdot, z are used as initial guesses for implicit integrators, if provided. + % Wrapper around solve() taking care of setting/getting inputs/outputs. + % Fields which are set to an empty array or not provided will not be set. + + if nargin >= 2 && ~isempty(x) + obj.set('x', x); + end + if nargin >= 3 && ~isempty(u) + obj.set('u', u); + end + + if strcmp(obj.sim.solver_options.integrator_type, 'IRK') + if nargin >= 4 && ~isempty(z) + obj.set('z', z); + end + if nargin >= 5 && ~isempty(xdot) + obj.set('xdot', xdot); + end + end + + if nargin >= 6 && ~isempty(p) + obj.set('p', p); + end + + status = obj.solve(); + + if status ~= 0 + error('AcadosSimSolver for model %s returned status %d.', obj.name, status); + end + + x_next = obj.get('xn'); + end + + % function delete(obj) % Use default implementation. % MATLAB destroys the property values after the destruction of the object. From 8a3dc38863492705936d7f8aa3f412bd4b9fecd4 Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Thu, 19 Jun 2025 13:04:42 +0200 Subject: [PATCH 072/164] Fix Levenberg-Marquardt and add option `adaptive_levenberg_marquardt_obj_scalar` (#1552) - Breaking fix: multiply Levenberg-Marquardt term with `cost_scaling` instead of time step. Affects users that define `cost_scaling` factors explicitly, as they otherwise coincide. - An option `adaptive_levenberg_marquardt_obj_scalar` was added that makes the adaptive Levenberg-Marquardt option more versatile. In the previous Levenberg-Marquardt option, the LM factor was multiplied by `2.0*obj_value`. This was replaced by `adaptive_levenberg_marquardt_obj_scalar*obj_value`. --- acados/ocp_nlp/ocp_nlp_common.c | 25 +++++++++---------- acados/ocp_nlp/ocp_nlp_common.h | 1 + .../acados_matlab_octave/AcadosOcpOptions.m | 2 ++ .../acados_template/acados_ocp_options.py | 20 +++++++++++++++ .../c_templates_tera/acados_multi_solver.in.c | 3 +++ .../c_templates_tera/acados_solver.in.c | 3 +++ 6 files changed, 41 insertions(+), 13 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index b848a4d9d0..1ec7c686be 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1248,6 +1248,7 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) // adaptive Levenberg-Marquardt options opts->adaptive_levenberg_marquardt_mu_min = 1e-16; opts->adaptive_levenberg_marquardt_lam = 5.0; + opts->adaptive_levenberg_marquardt_obj_scalar = 2.0; opts->with_adaptive_levenberg_marquardt = false; opts->ext_qp_res = 0; @@ -1390,6 +1391,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, "adaptive_levenberg_marquardt_obj_scalar")) + { + double* adaptive_levenberg_marquardt_obj_scalar = (double *) value; + opts->adaptive_levenberg_marquardt_obj_scalar = *adaptive_levenberg_marquardt_obj_scalar; + } else if (!strcmp(field, "solution_sens_qp_t_lam_min")) { double* solution_sens_qp_t_lam_min = (double *) value; @@ -2775,10 +2781,11 @@ void ocp_nlp_add_levenberg_marquardt_term(ocp_nlp_config *config, ocp_nlp_dims * ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha, int iter, ocp_qp_in *qp_in) { + double scaling_factor; if (opts->with_adaptive_levenberg_marquardt) { adaptive_levenberg_marquardt_update_mu(iter, alpha, opts, mem); - double reg_param = 2*mem->cost_value*mem->adaptive_levenberg_marquardt_mu; + double reg_param = opts->adaptive_levenberg_marquardt_obj_scalar*mem->cost_value*mem->adaptive_levenberg_marquardt_mu; opts->levenberg_marquardt = reg_param; } // Only add the Levenberg-Marquardt term when it is bigger than zero @@ -2789,18 +2796,10 @@ void ocp_nlp_add_levenberg_marquardt_term(ocp_nlp_config *config, ocp_nlp_dims * int *nu = dims->nu; for (int i = 0; i <= N; i++) { - if (i < N) - { - // Levenberg Marquardt term: Ts[i] * levenberg_marquardt * eye() - blasfeo_ddiare(nu[i] + nx[i], in->Ts[i] * opts->levenberg_marquardt, - mem->qp_in->RSQrq+i, 0, 0); - } - else - { - // Levenberg Marquardt term: 1.0 * levenberg_marquardt * eye() - blasfeo_ddiare(nu[i] + nx[i], opts->levenberg_marquardt, - mem->qp_in->RSQrq+i, 0, 0); - } + config->cost[i]->model_get(config->cost[i], dims->cost[i], in->cost[i], "scaling", &scaling_factor); + // Levenberg Marquardt term: scaling_factor * levenberg_marquardt * eye() + blasfeo_ddiare(nu[i] + nx[i], scaling_factor * opts->levenberg_marquardt, + mem->qp_in->RSQrq+i, 0, 0); } } // else: do nothing } diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 9c3c8e8d28..f5d04baab1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -323,6 +323,7 @@ typedef struct ocp_nlp_opts double adaptive_levenberg_marquardt_lam; double adaptive_levenberg_marquardt_mu_min; double adaptive_levenberg_marquardt_mu0; + double adaptive_levenberg_marquardt_obj_scalar; int with_solution_sens_wrt_params; int with_value_sens_wrt_params; diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 03e89f12ec..5679229151 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -115,6 +115,7 @@ adaptive_levenberg_marquardt_lam adaptive_levenberg_marquardt_mu_min adaptive_levenberg_marquardt_mu0 + adaptive_levenberg_marquardt_obj_scalar log_primal_step_norm log_dual_step_norm store_iterates @@ -229,6 +230,7 @@ obj.adaptive_levenberg_marquardt_lam = 5.0; obj.adaptive_levenberg_marquardt_mu_min = 1e-16; obj.adaptive_levenberg_marquardt_mu0 = 1e-3; + obj.adaptive_levenberg_marquardt_obj_scalar = 2.0; obj.log_primal_step_norm = 0; obj.log_dual_step_norm = 0; obj.store_iterates = false; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 8d2962fa3f..454309fc3f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -124,6 +124,7 @@ 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.__adaptive_levenberg_marquardt_obj_scalar = 2.0 self.__log_primal_step_norm: bool = False self.__log_dual_step_norm: bool = False self.__store_iterates: bool = False @@ -664,6 +665,18 @@ def adaptive_levenberg_marquardt_mu0(self): """ return self.__adaptive_levenberg_marquardt_mu0 + @property + def adaptive_levenberg_marquardt_obj_scalar(self): + """ + So far: Only relevant for DDP + Flag defining the value of the scalar that is multiplied with the NLP + objective function in the adaptive levenberg_marquardt. + Must be > 0. + Default: 2.0 + type: float + """ + return self.__adaptive_levenberg_marquardt_obj_scalar + @property def log_primal_step_norm(self): """ @@ -1753,6 +1766,13 @@ def adaptive_levenberg_marquardt_mu0(self, adaptive_levenberg_marquardt_mu0): else: raise ValueError('Invalid adaptive_levenberg_marquardt_mu0 value. adaptive_levenberg_marquardt_mu0 must be a positive float.') + @adaptive_levenberg_marquardt_obj_scalar.setter + def adaptive_levenberg_marquardt_obj_scalar(self, adaptive_levenberg_marquardt_obj_scalar): + if isinstance(adaptive_levenberg_marquardt_obj_scalar, float) and adaptive_levenberg_marquardt_obj_scalar >= 0.0: + self.__adaptive_levenberg_marquardt_obj_scalar = adaptive_levenberg_marquardt_obj_scalar + else: + raise ValueError('Invalid adaptive_levenberg_marquardt_obj_scalar value. adaptive_levenberg_marquardt_obj_scalar must be a positive float.') + @log_primal_step_norm.setter def log_primal_step_norm(self, val): if not isinstance(val, bool): 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 d7923e0acd..f88104467c 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 @@ -2367,6 +2367,9 @@ ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "allow_direction_mode_switch_to_no double adaptive_levenberg_marquardt_mu0 = {{ solver_options.adaptive_levenberg_marquardt_mu0 }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); + double adaptive_levenberg_marquardt_obj_scalar = {{ solver_options.adaptive_levenberg_marquardt_obj_scalar }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); + bool eval_residual_at_max_iter = {{ solver_options.eval_residual_at_max_iter }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); 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 e1d6f83f8c..4527c029d8 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 @@ -2508,6 +2508,9 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps double adaptive_levenberg_marquardt_mu0 = {{ solver_options.adaptive_levenberg_marquardt_mu0 }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); + double adaptive_levenberg_marquardt_obj_scalar = {{ solver_options.adaptive_levenberg_marquardt_obj_scalar }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); + bool eval_residual_at_max_iter = {{ solver_options.eval_residual_at_max_iter }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); From e958bfc6450152ecb2f9356394e11d580dcd070c Mon Sep 17 00:00:00 2001 From: Jingtao <84231306+Pandatheon@users.noreply.github.com> Date: Thu, 19 Jun 2025 17:51:03 +0200 Subject: [PATCH 073/164] `AcadosCasadiOcpSolver`: Fix partial bounds for `set()` and `get()` (#1551) --- .../casadi_tests/test_casadi_get_set.py | 125 ++++++++++++++++++ interfaces/CMakeLists.txt | 4 + .../acados_casadi_ocp_solver.py | 63 +++++---- 3 files changed, 167 insertions(+), 25 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_get_set.py diff --git a/examples/acados_python/casadi_tests/test_casadi_get_set.py b/examples/acados_python/casadi_tests/test_casadi_get_set.py new file mode 100644 index 0000000000..c34e9fc5dd --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_get_set.py @@ -0,0 +1,125 @@ +import sys +sys.path.insert(0, '../getting_started') +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model + + +def get_x_u_traj(ocp_solver: Union[AcadosOcpSolver, AcadosCasadiOcpSolver], N_horizon: int): + ocp = ocp_solver.acados_ocp if isinstance(ocp_solver, AcadosOcpSolver) else ocp_solver.ocp + simX = np.zeros((N_horizon+1, ocp.dims.nx)) + simU = np.zeros((N_horizon, ocp.dims.nu)) + for i in range(N_horizon): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") + + return simX, simU + +def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + # add dummy control + model.u = ca.vertcat(model.u, ca.SX.sym('dummy_u')) + ocp.model = model + + # set h constraints + ocp.model.con_h_expr_0 = ca.norm_2(model.x) + ocp.constraints.lh_0 = np.array([0]) + ocp.constraints.uh_0 = np.array([3.16]) + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2, 1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set partial bounds for state + ocp.constraints.lbx = np.array([-10,-10]) + ocp.constraints.ubx = np.array([10,10]) + ocp.constraints.idxbx = np.array([0,3]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + N_horizon = 20 + Tf = 1.0 + ocp = formulate_ocp(Tf, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + # get solution + simX, simU = get_x_u_traj(ocp_solver, N_horizon) + result = ocp_solver.store_iterate_to_obj() + lam = ocp_solver.get_flat("lam") + pi = ocp_solver.get_flat("pi") + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp,solver="ipopt",verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + x_casadi_sol, u_casadi_sol = get_x_u_traj(casadi_ocp_solver, N_horizon) + lam_casadi = casadi_ocp_solver.get_flat("lam") + pi_casadi = casadi_ocp_solver.get_flat("pi") + + # evaluate difference + diff_x = np.linalg.norm(x_casadi_sol - simX) + print(f"Difference between casadi and acados solution in x: {diff_x}") + diff_u = np.linalg.norm(u_casadi_sol - simU) + print(f"Difference between casadi and acados solution in u: {diff_u}") + diff_lam = np.linalg.norm(lam_casadi - lam) + print(f"Difference between casadi and acados solution in lam: {diff_lam}") + diff_pi = np.linalg.norm(pi_casadi - pi) + print(f"Difference between casadi and acados solution in pi: {diff_pi}") + + test_tol = 1e-4 + if diff_x > test_tol or diff_u > test_tol or diff_lam > test_tol or diff_pi > test_tol: + raise ValueError(f"Test failed: difference between casadi and acados solution should be smaller than {test_tol}, but got {diff_x} and {diff_u} and {diff_lam} and {diff_pi}.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 60421f9449..006830e90c 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -352,6 +352,10 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_convex_ocp_with_onesided_constraints COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_ocp_with_onesided_constraints python main_convex_onesided.py) + + add_test(NAME python_casadi_get_set_example + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_get_set.py) # Sim add_test(NAME python_pendulum_ext_sim_example diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 18f26fd405..4366739692 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -55,6 +55,10 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # indices of variables within w 'x_in_w': [], 'u_in_w': [], + # indices of state bounds within lam_x(lam_w) in casadi formulation + 'lam_bx_in_lam_w':[], + # indices of control bounds within lam_x(lam_w) in casadi formulation + 'lam_bu_in_lam_w': [], # indices of dynamic constraints within g in casadi formulation 'pi_in_lam_g': [], # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation @@ -90,24 +94,33 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # parameters ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon)] - # setup state bounds + # setup state and control bounds lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 - ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 - offset = 0 - for i in range(1, N_horizon): - lb_xtraj_node[i][constraints.idxbx] = constraints.lbx - ub_xtraj_node[i][constraints.idxbx] = constraints.ubx - lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e - ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e - - # setup control bounds lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - for i in range(N_horizon): - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu + offset = 0 + for i in range(0, N_horizon+1): + if i == 0: + lb_xtraj_node[i][constraints.idxbx_0] = constraints.lbx_0 + ub_xtraj_node[i][constraints.idxbx_0] = constraints.ubx_0 + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_0)) + offset += dims.nx + elif i < N_horizon: + lb_xtraj_node[i][constraints.idxbx] = constraints.lbx + ub_xtraj_node[i][constraints.idxbx] = constraints.ubx + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx)) + offset += dims.nx + elif i == N_horizon: + lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e + ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_e)) + offset += dims.nx + if i < N_horizon: + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu + index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) + offset += dims.nu ### Concatenate primal variables and bounds # w = [x0, u0, x1, u1, ...] @@ -492,15 +505,15 @@ def get(self, stage: int, field: str): return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() elif field == 'lam': if stage == 0: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_0 else np.empty((0, 1)) - bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage < dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx else np.empty((0, 1)) - bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage == dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_e else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] bu_lam = np.empty((0, 1)) g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] @@ -673,17 +686,17 @@ def set(self, stage: int, field: str, value_: np.ndarray): n_ghphi = dims.ng_e + dims.nh_e + dims.nphi_e offset_u = (nbx+nbu+n_ghphi) - lbu_lam = value_[:nbu] if nbu else np.empty((dims.nu,)) - lbx_lam = value_[nbu:nbu+nbx] if nbx else np.empty((dims.nx,)) + lbu_lam = value_[:nbu] + lbx_lam = value_[nbu:nbu+nbx] lg_lam = value_[nbu+nbx:nbu+nbx+n_ghphi] - ubu_lam = value_[offset_u:offset_u+nbu] if nbu else np.empty((dims.nu,)) - ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] if nbx else np.empty((dims.nx,)) + ubu_lam = value_[offset_u:offset_u+nbu] + ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] ug_lam = value_[offset_u+nbu+nbx:offset_u+nbu+nbx+n_ghphi] if stage != dims.N: - self.lam_x0[self.index_map['x_in_w'][stage]+self.index_map['u_in_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam else: - self.lam_x0[self.index_map['x_in_w'][stage]] = ubx_lam-lbx_lam + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]] = ubx_lam-lbx_lam self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam elif field in ['sl', 'su']: # do nothing for now, only empty is supported From 7970df7f54f6341978a4ce13c23646ec9326cb44 Mon Sep 17 00:00:00 2001 From: JasperHoffmann <41264755+JasperHoffmann@users.noreply.github.com> Date: Mon, 23 Jun 2025 09:13:17 +0200 Subject: [PATCH 074/164] Python: fix `create_default_initial_iterate` wrt slack variables (#1555) The function didn't consider dual variables corresponding to the bounds of slack variables. --- interfaces/acados_template/acados_template/acados_ocp.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index e635a5d008..1a55ee0484 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2242,9 +2242,9 @@ def create_default_initial_iterate(self) -> AcadosOcpIterate: pi_traj = self.solver_options.N_horizon * [np.zeros(self.dims.nx)] - ni_0 = dims.nbu + dims.nbx_0 + dims.nh_0 + dims.nphi_0 + dims.ng - ni = dims.nbu + dims.nbx + dims.nh + dims.nphi + dims.ng - ni_e = dims.nbx_e + dims.nh_e + dims.nphi_e + dims.ng_e + ni_0 = dims.nbu + dims.nbx_0 + dims.nh_0 + dims.nphi_0 + dims.ng + dims.ns_0 + ni = dims.nbu + dims.nbx + dims.nh + dims.nphi + dims.ng + dims.ns + ni_e = dims.nbx_e + dims.nh_e + dims.nphi_e + dims.ng_e + dims.ns_e lam_traj = [np.zeros(2*ni_0)] + (self.solver_options.N_horizon-1) * [np.zeros(2*ni)] + [np.zeros(2*ni_e)] iterate = AcadosOcpIterate( From e7ff37e8b5436e1addb94356c4793cafb8c97c68 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 23 Jun 2025 09:34:31 +0200 Subject: [PATCH 075/164] Python: Fix `constraints_get` (#1556) Resolves https://github.com/acados/acados/issues/1554 --- interfaces/acados_c/ocp_nlp_interface.c | 10 ---------- .../acados_template/acados_ocp_solver.py | 2 +- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 5f380034b7..351112ef53 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1172,16 +1172,6 @@ void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, dims_out[0] = dims->nx[stage] + dims->nu[stage]; dims_out[1] = dims->nx[stage] + dims->nu[stage]; } - else if (!strcmp(field, "C")) - { - dims_out[0] = dims->ng[stage]; - dims_out[1] = dims->nx[stage]; - } - else if (!strcmp(field, "D")) - { - dims_out[0] = dims->ng[stage]; - dims_out[1] = dims->nu[stage]; - } else if (!strcmp(field, "scaling")) { dims_out[0] = 1; diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index da71be1029..d31adca371 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1959,7 +1959,7 @@ def constraints_get(self, stage_: int, field_: str) -> np.ndarray: dims_ = np.zeros((2,), dtype=np.intc, order="C") dims_data = cast(dims_.ctypes.data, POINTER(c_int)) - self.__acados_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ + self.__acados_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage_, field, dims_data) # check whether field is vector-valued From e5c9d8c078fd625473f315755b37f81c9cce139a Mon Sep 17 00:00:00 2001 From: Jingtao <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 25 Jun 2025 16:47:42 +0200 Subject: [PATCH 076/164] AcadosCasadiOcpSolver: add parameter dependency (#1557) - modify acados_casadi_ocp_solver.py for parameters - add a draft test for testing parameters (global and non-global) as physical constants in pendunlum task - modify draft test for stage-varying parameter - add a draft test for testing parameter in constraint and cost function --- .../test_casadi_p_in_constraint_and_cost.py | 144 +++++++++++++ .../casadi_tests/test_casadi_parametric.py | 194 ++++++++++++++++++ interfaces/CMakeLists.txt | 6 + .../acados_casadi_ocp_solver.py | 51 ++++- 4 files changed, 386 insertions(+), 9 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_p_in_constraint_and_cost.py create mode 100644 examples/acados_python/casadi_tests/test_casadi_parametric.py diff --git a/examples/acados_python/casadi_tests/test_casadi_p_in_constraint_and_cost.py b/examples/acados_python/casadi_tests/test_casadi_p_in_constraint_and_cost.py new file mode 100644 index 0000000000..9fd01864b9 --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_p_in_constraint_and_cost.py @@ -0,0 +1,144 @@ +# +# 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.; +# + +import sys +sys.path.insert(0, '../getting_started') +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model +import numpy as np +import scipy.linalg +from utils import plot_pendulum +from casadi import SX, vertcat + +PLOT = False + +def formulate_ocp(p_in_constraint=True, p_in_cost=True): + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + x = model.x + u = model.u + + Tf = 1.0 + nx = x.rows() + nu = u.rows() + N = 20 + + # set dimensions + ocp.solver_options.N_horizon = N + + # set cost + Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R = 2*np.diag([1e-2]) + cost_W = scipy.linalg.block_diag(Q, R) + + n_param = 6 + p = SX.sym('p', n_param) + constraint_quotient = p[0] + y_param = p[1:nx+nu+1] + ocp.model.p = p + + # define cost with parametric reference + ocp.cost.cost_type = 'EXTERNAL' + ocp.cost.cost_type_e = 'EXTERNAL' + + residual = y_param - vertcat(x, u) + ocp.model.cost_expr_ext_cost = residual.T @ cost_W @ residual + res_e = y_param[0:nx] - x + ocp.model.cost_expr_ext_cost_e = res_e.T @ Q @ res_e + + # set constraints + Fmax = 80 + ocp.constraints.lh = np.array([-Fmax]) + ocp.constraints.uh = np.array([+Fmax]) + ocp.model.con_h_expr = model.u / constraint_quotient + + ocp.constraints.lh_0 = np.array([-Fmax]) + ocp.constraints.uh_0 = np.array([+Fmax]) + ocp.model.con_h_expr_0 = model.u / constraint_quotient + + p_0 = np.zeros(n_param) + p_0[0] = 0.5 if p_in_constraint else 1 + p_0[1] = 1 if p_in_cost else 0 + ocp.parameter_values = p_0 + + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'EXACT' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.regularize_method = 'CONVEXIFY' # GAUSS_NEWTON, EXACT + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + # set prediction horizon + ocp.solver_options.tf = Tf + return ocp + + +def main(p_in_constraint=True, p_in_cost=True): + + print(f"\n\nRunning example with p_in_constraint={p_in_constraint}, p_in_cost={p_in_cost}") + ocp = formulate_ocp(p_in_constraint, p_in_cost) + + N = ocp.solver_options.N_horizon + Tf = ocp.solver_options.tf + + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + status = ocp_solver.solve() + + if status != 0: + raise Exception(f'acados returned status {status}.') + result = ocp_solver.store_iterate_to_obj() + + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + result_casadi = casadi_ocp_solver.store_iterate_to_obj() + + result.flatten().allclose(other=result_casadi.flatten()) + + Fmax = 80 + if PLOT: + acados_u = np.array([ocp_solver.get(i, "u") for i in range(N)]) + acados_x = np.array([ocp_solver.get(i, "x") for i in range(N+1)]) + casadi_u = np.array([casadi_ocp_solver.get(i, "u") for i in range(N)]) + casadi_x = np.array([casadi_ocp_solver.get(i, "x") for i in range(N+1)]) + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, acados_u, acados_x, latexify=False) + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, casadi_u, casadi_x, latexify=False) + +if __name__ == "__main__": + main(p_in_constraint=False, p_in_cost=False) + main(p_in_constraint=True, p_in_cost=False) + main(p_in_constraint=False, p_in_cost=True) + main(p_in_constraint=True, p_in_cost=True) \ No newline at end of file diff --git a/examples/acados_python/casadi_tests/test_casadi_parametric.py b/examples/acados_python/casadi_tests/test_casadi_parametric.py new file mode 100644 index 0000000000..0492165e89 --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_parametric.py @@ -0,0 +1,194 @@ +# +# 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.; +# +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, AcadosCasadiOcpSolver +import numpy as np +import sys +sys.path.insert(0, '../getting_started') +from utils import plot_pendulum + +from casadi import MX, vertcat, sin, cos +import casadi as ca +import time + +PLOT = False + +def export_pendulum_ode_model() -> AcadosModel: + # constants + m_cart = 1. # mass of the cart [kg] + + # parameters + g = MX.sym("g") + p = g + m = MX.sym("m") + l = MX.sym("l") + p_global = ca.vertcat(m,l) + + # set up states & controls + x1 = MX.sym('x1') + theta = MX.sym('theta') + v1 = MX.sym('v1') + dtheta = MX.sym('dtheta') + + x = vertcat(x1, theta, v1, dtheta) + + F = MX.sym('F') + u = vertcat(F) + + # xdot + nx = x.shape[0] + xdot = MX.sym('xdot', nx) + + # dynamics + cos_theta = cos(theta) + sin_theta = sin(theta) + denominator = m_cart + m - m*cos_theta**2 + f_expl = vertcat(v1, + dtheta, + (-m*l*sin_theta*dtheta**2 + m*g*cos_theta*sin_theta+F)/denominator, + (-m*l*cos_theta*sin_theta*dtheta**2 + F*cos_theta+(m_cart+m)*g*sin_theta)/(l*denominator) + ) + + f_impl = xdot - f_expl + + model = AcadosModel() + + model.f_impl_expr = f_impl + model.f_expl_expr = f_expl + model.x = x + model.xdot = xdot + model.u = u + model.p = p + model.p_global = p_global + model.name = 'pendulum_ode' + + # store meta information + model.x_labels = ['$x$ [m]', r'$\theta$ [rad]', '$v$ [m]', r'$\dot{\theta}$ [rad/s]'] + model.u_labels = ['$F$'] + model.t_label = '$t$ [s]' + + return model + + +def ocp_formulation() -> AcadosOcp: + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + # dimensions + nx = model.x.rows() + nu = model.u.rows() + + # set cost + Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q, R).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + ocp.parameter_values = np.array([9.81]) + ocp.p_global_values = np.array([0.1, 0.8]) + + return ocp + + +def main(stage_varying=True): + + print(f"\n\nRunning example with stage_varying_p={stage_varying}") + # create ocp + ocp = ocp_formulation() + + Tf = 1.0 + N_horizon = 20 + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + # set prediction horizon + ocp.solver_options.tf = Tf + ocp.solver_options.N_horizon = N_horizon + + # create acados solver + print(f"Creating ocp solver with p_global = {ocp.model.p_global}, p = {ocp.model.p}") + ocp_solver = AcadosOcpSolver(ocp, generate=True, build=True, verbose=False, save_p_global=True) + if stage_varying: + for i in range(0, N_horizon+1): + ocp_solver.set(stage_= i, field_= 'p', value_=np.array([9.81+i*0.3])) + status = ocp_solver.solve() + + if status != 0: + raise Exception(f'acados returned status {status}.') + result = ocp_solver.store_iterate_to_obj() + + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp, verbose=False) + if stage_varying: + for i in range(0, N_horizon+1): + casadi_ocp_solver.set(stage= i, field= 'p', value_=np.array([9.81+i*0.3])) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + result_casadi = casadi_ocp_solver.store_iterate_to_obj() + + result.flatten().allclose(other=result_casadi.flatten()) + + if PLOT: + acados_u = np.array([ocp_solver.get(i, "u") for i in range(N_horizon)]) + acados_x = np.array([ocp_solver.get(i, "x") for i in range(N_horizon+1)]) + casadi_u = np.array([casadi_ocp_solver.get(i, "u") for i in range(N_horizon)]) + casadi_x = np.array([casadi_ocp_solver.get(i, "x") for i in range(N_horizon+1)]) + plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu[0], acados_u, acados_x, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) + plot_pendulum(ocp.solver_options.shooting_nodes, ocp.constraints.ubu[0], casadi_u, casadi_x, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) + +if __name__ == "__main__": + main(stage_varying=False) + main(stage_varying=True) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 006830e90c..abfa4ed61b 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -356,6 +356,12 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_casadi_get_set_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_get_set.py) + add_test(NAME test_casadi_parametric + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_parametric.py) + add_test(NAME test_casadi_p_in_constraint_and_cost + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_p_in_constraint_and_cost.py) # Sim add_test(NAME python_pendulum_ext_sim_example diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 4366739692..f602eee167 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -55,6 +55,9 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # indices of variables within w 'x_in_w': [], 'u_in_w': [], + # indices of parameters within p_nlp + 'p_in_p_nlp': [], + 'p_global_in_p_nlp': [], # indices of state bounds within lam_x(lam_w) in casadi formulation 'lam_bx_in_lam_w':[], # indices of control bounds within lam_x(lam_w) in casadi formulation @@ -92,7 +95,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") # parameters - ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon)] + ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon+1)] # setup state and control bounds lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] @@ -128,7 +131,9 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): lbw_list = [] ubw_list = [] w0_list = [] + p_list = [] offset = 0 + offset_p = 0 x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) for i in range(N_horizon): # add x @@ -145,6 +150,10 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): w0_list.append(np.zeros((dims.nu,))) index_map['u_in_w'].append(list(range(offset, offset + dims.nu))) offset += dims.nu + # add parameters + p_list.append(ocp.parameter_values) + index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) + offset_p += dims.np ## terminal stage # add x w_sym_list.append(xtraj_node[-1]) @@ -153,6 +162,13 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): w0_list.append(x_guess) index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) offset += dims.nx + # add parameters + p_list.append(ocp.parameter_values) + index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) + offset_p += dims.np + p_list.append(ocp.p_global_values) + index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) + offset_p += dims.np_global nw = offset # number of primal variables @@ -167,8 +183,8 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) elif solver_options.integrator_type == "ERK": - ca_expl_ode = ca.Function('ca_expl_ode', [model.x, model.u], [model.f_expl_expr]) - # , model.p, model.p_global] + para = ca.vertcat(model.u, model.p, model.p_global) + ca_expl_ode = ca.Function('ca_expl_ode', [model.x, para], [model.f_expl_expr]) f_discr_fun = ca.simpleRK(ca_expl_ode, solver_options.sim_method_num_steps[0], solver_options.sim_method_num_stages[0]) else: raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") @@ -203,7 +219,8 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): if solver_options.integrator_type == "DISCRETE": dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) elif solver_options.integrator_type == "ERK": - dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], solver_options.time_steps[i]) + para = ca.vertcat(utraj_node[i], ptraj_node[i], model.p_global) + dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], para, solver_options.time_steps[i]) g.append(dyn_equality) lbg.append(np.zeros((dims.nx, 1))) ubg.append(np.zeros((dims.nx, 1))) @@ -352,10 +369,12 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} w0 = np.concatenate(w0_list) + p = np.concatenate(p_list) self.__nlp = nlp self.__bounds = bounds self.__w0 = w0 + self.__p = p self.__index_map = index_map self.__nlp_hess_l_custom = nlp_hess_l_custom self.__hess_approx_expr = hess_l @@ -374,6 +393,13 @@ def w0(self): """ return self.__w0 + @property + def p_nlp_values(self): + """ + Default parameter vector p_nlp in the form of [p_0,..., p_N, p_global] for given NLP. + """ + return self.__p + @property def bounds(self): """ @@ -425,6 +451,7 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, self.casadi_nlp = casadi_nlp_obj.nlp self.bounds = casadi_nlp_obj.bounds self.w0 = casadi_nlp_obj.w0 + self.p = casadi_nlp_obj.p_nlp_values self.index_map = casadi_nlp_obj.index_map self.nlp_hess_l_custom = casadi_nlp_obj.nlp_hess_l_custom @@ -457,7 +484,7 @@ def solve(self) -> int: :return: status of the solver """ - self.nlp_sol = self.casadi_solver(x0=self.w0, + self.nlp_sol = self.casadi_solver(x0=self.w0, p=self.p, lam_g0=self.lam_g0, lam_x0=self.lam_x0, lbx=self.bounds['lbx'], ubx=self.bounds['ubx'], lbg=self.bounds['lbg'], ubg=self.bounds['ubg'] @@ -489,7 +516,7 @@ def get(self, stage: int, field: str): Get the last solution of the solver. :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'pi', 'lam'] + :param field: string in ['x', 'u', 'pi', 'p', 'lam'] """ if not isinstance(stage, int): @@ -503,6 +530,8 @@ def get(self, stage: int, field: str): return self.nlp_sol_w[self.index_map['u_in_w'][stage]].flatten() elif field == 'pi': return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() + elif field == 'p': + return self.p[self.index_map['p_in_p_nlp'][stage]].flatten() elif field == 'lam': if stage == 0: bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] @@ -534,7 +563,7 @@ def get_flat(self, field_: str) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. - :param field: string in ['x', 'u', 'pi', 'lam', 'z', 'sl', 'su'] + :param field: string in ['x', 'u', 'pi', 'lam', 'p', 'p_global', 'z', 'sl', 'su'] .. note:: The parameter 'p_global' has no stage-wise structure and is processed in a memory saving manner by default. \n In order to read the 'p_global' parameter, the option 'save_p_global' must be set to 'True' upon instantiation. \n @@ -544,7 +573,7 @@ def get_flat(self, field_: str) -> np.ndarray: dims = self.ocp.dims result = [] - if field_ in ['x', 'lam', 'sl', 'su']: + if field_ in ['x', 'lam', 'sl', 'su', 'p']: for i in range(dims.N+1): result.append(self.get(i, field_)) return np.concatenate(result) @@ -552,6 +581,8 @@ def get_flat(self, field_: str) -> np.ndarray: for i in range(dims.N): result.append(self.get(i, field_)) return np.concatenate(result) + elif field_ == 'p_global': + return self.p[self.index_map['p_global_in_p_nlp']].flatten() # casadi variables. TODO: maybe remove this. elif field_ == 'lam_x': return self.nlp_sol_lam_x.flatten() @@ -653,7 +684,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: raise NotImplementedError() def get_cost(self) -> float: - raise NotImplementedError() + return self.nlp_sol['f'].full().item() def set(self, stage: int, field: str, value_: np.ndarray): """ @@ -671,6 +702,8 @@ def set(self, stage: int, field: str, value_: np.ndarray): self.w0[self.index_map['u_in_w'][stage]] = value_.flatten() elif field == 'pi': self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = -value_.flatten() + elif field == 'p': + self.p[self.index_map['p_in_p_nlp'][stage]] = value_.flatten() elif field == 'lam': if stage == 0: nbx = dims.nbx_0 From 8bba4ed127f9a4cdb2887efc4ed094e92701031d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 25 Jun 2025 17:09:53 +0200 Subject: [PATCH 077/164] add QP scaling for cost and constraints (#1463) This PR adds the option to scale the cost and constraints of QPs. In particular, the following options have been added: - `qpscaling_scale_constraints`: String in ["NO_OBJECTIVE_SCALING", "OBJECTIVE_GERSHGORIN"] Default: "NO_OBJECTIVE_SCALING". - `NO_OBJECTIVE_SCALING`: no scaling of the objective - `OBJECTIVE_GERSHGORIN`: estimate max. abs. eigenvalue using Gershgorin circles as `max_abs_eig`, then sets the objective scaling factor as `obj_factor = min(1.0, qpscaling_ub_max_abs_eig/max_abs_eig)` - `qpscaling_scale_constraints` String in ["NO_CONSTRAINT_SCALING", "INF_NORM"] Default: "NO_CONSTRAINT_SCALING". - `NO_CONSTRAINT_SCALING`: no scaling of the constraints - `INF_NORM`: scales each constraint except simple bounds by factor `1.0 / max(inf_norm_coeff, inf_norm_constraint_bound)`, such that the inf-norm of the constraint coefficients and bounds is <= 1.0. Slack penalties are adjusted accordingly to get an equivalent solution. First, the cost is scaled, then the constraints. ## Implementation details Minimal computational overhead is associated with the new scaling, in case it is not used. The implementation avoids copying values from the different data structures if they are the same and aliases pointers instead. ## Examples & Tests: - equivalence of primal and dual solutions is tested on different examples including slack variables and one-sided constraints - some tests were added, showcasing examples which cannot be solved without scaling the QP. ## Limitations - Real-time iterations, i.e. RTI and AS-RTI variants with split preparation and feedback phase are not compatible with QP scaling yet. Work on this has been started at https://github.com/FreyJo/acados/pull/76 but is not a priority right now. - Computation of solution sensitivities is not compatible with QP scaling yet. - Scaling tolerances of the QP solver is not done. Right now, the QP solver tolerances need to be set small enough, for the NLP solver to converge. A follow-up work is described in https://github.com/acados/acados/issues/1553 --------- Co-authored-by: d.kiessling --- .gitignore | 2 +- acados/ocp_nlp/ocp_nlp_common.c | 116 ++- acados/ocp_nlp/ocp_nlp_common.h | 14 +- acados/ocp_nlp/ocp_nlp_ddp.c | 4 +- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 6 +- ...ocp_nlp_globalization_merit_backtracking.c | 2 +- acados/ocp_nlp/ocp_nlp_qpscaling.c | 663 ++++++++++++++++++ acados/ocp_nlp/ocp_nlp_qpscaling.h | 108 +++ acados/ocp_nlp/ocp_nlp_sqp.c | 18 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 28 +- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 136 ++-- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 6 +- acados/ocp_qp/ocp_qp_common.c | 6 +- acados/utils/print.c | 5 +- acados/utils/print.h | 1 - acados/utils/types.h | 18 + .../convex_problem_globalization_necessary.m | 0 .../env.sh | 0 .../dense_nlp/test_qpscaling.m | 108 +++ .../test/run_matlab_examples_new_casadi.m | 1 + .../test/test_all_examples.m | 2 +- .../hock_schittkowsky/hs016_test.py | 15 +- .../hs074_constraint_scaling.py | 127 ++++ .../linear_mass_test_problem.py | 15 +- .../linear_mass_model/sqp_wfqp_test.py | 69 +- .../test_qpscaling_slacked.py | 285 ++++++++ .../non_ocp_nlp/qpscaling_test.py | 254 +++++++ .../pendulum_on_cart/common/pendulum_model.py | 54 ++ .../ocp/time_optimal_swing_up.py | 159 +++++ interfaces/CMakeLists.txt | 22 +- interfaces/acados_c/ocp_nlp_interface.c | 31 +- interfaces/acados_c/ocp_nlp_interface.h | 1 + interfaces/acados_matlab_octave/AcadosOcp.m | 15 + .../acados_matlab_octave/AcadosOcpOptions.m | 11 + .../acados_matlab_octave/AcadosOcpSolver.m | 10 + interfaces/acados_matlab_octave/ocp_get.c | 26 +- .../acados_template/acados_ocp.py | 5 +- .../acados_template/acados_ocp_iterate.py | 15 +- .../acados_template/acados_ocp_options.py | 78 +++ .../acados_template/acados_ocp_solver.py | 61 +- .../c_templates_tera/acados_multi_solver.in.c | 13 + .../c_templates_tera/acados_solver.in.c | 13 + .../matlab_templates/mex_solver.in.m | 1 - 43 files changed, 2354 insertions(+), 170 deletions(-) create mode 100644 acados/ocp_nlp/ocp_nlp_qpscaling.c create mode 100644 acados/ocp_nlp/ocp_nlp_qpscaling.h rename examples/acados_matlab_octave/{convex_problem_globalization_needed => dense_nlp}/convex_problem_globalization_necessary.m (100%) rename examples/acados_matlab_octave/{convex_problem_globalization_needed => dense_nlp}/env.sh (100%) create mode 100644 examples/acados_matlab_octave/dense_nlp/test_qpscaling.m create mode 100644 examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py create mode 100644 examples/acados_python/linear_mass_model/test_qpscaling_slacked.py create mode 100644 examples/acados_python/non_ocp_nlp/qpscaling_test.py create mode 100644 examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py diff --git a/.gitignore b/.gitignore index 62f1a779fd..8dcb492e32 100644 --- a/.gitignore +++ b/.gitignore @@ -93,7 +93,7 @@ env/ # Examples # c_generated_code -*_generated_code +*_generated_code* # Misc # ######## diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1ec7c686be..eeb747e5f2 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -182,6 +182,12 @@ static acados_size_t ocp_nlp_dims_calculate_size_self(int N) // regularization size += ocp_nlp_reg_dims_calculate_size(N); + // qpscaling + size += ocp_nlp_qpscaling_dims_calculate_size(N); + + // relaxed_qpscaling + size += ocp_nlp_qpscaling_dims_calculate_size(N); + size += sizeof(ocp_nlp_reg_dims); size += 8; // initial align @@ -277,16 +283,13 @@ static ocp_nlp_dims *ocp_nlp_dims_assign_self(int N, void *raw_memory) dims->regularize = ocp_nlp_reg_dims_assign(N, c_ptr); c_ptr += ocp_nlp_reg_dims_calculate_size(N); - /* initialize qp_solver dimensions */ -// dims->qp_solver->N = N; -// for (int i = 0; i <= N; i++) -// { - // TODO(dimitris): values below are needed for reformulation of QP when soft constraints - // are not supported. Make this a bit more transparent as it clushes with nbx/nbu above. -// dims->qp_solver->nsbx[i] = 0; -// dims->qp_solver->nsbu[i] = 0; -// dims->qp_solver->nsg[i] = 0; -// } + // qpscaling + dims->qpscaling = ocp_nlp_qpscaling_dims_assign(N, c_ptr); + c_ptr += ocp_nlp_qpscaling_dims_calculate_size(N); + + // relaxed_qpscaling + dims->relaxed_qpscaling = ocp_nlp_qpscaling_dims_assign(N, c_ptr); + c_ptr += ocp_nlp_qpscaling_dims_calculate_size(N); // N dims->N = N; @@ -1083,6 +1086,7 @@ acados_size_t ocp_nlp_opts_calculate_size(void *config_, void *dims_) size += qp_solver->opts_calculate_size(qp_solver, dims->qp_solver); size += config->regularize->opts_calculate_size(); + size += ocp_nlp_qpscaling_opts_calculate_size(); size += config->globalization->opts_calculate_size(config, dims); @@ -1149,6 +1153,9 @@ void *ocp_nlp_opts_assign(void *config_, void *dims_, void *raw_memory) opts->regularize = config->regularize->opts_assign(c_ptr); c_ptr += config->regularize->opts_calculate_size(); + opts->qpscaling = ocp_nlp_qpscaling_opts_assign(c_ptr); + c_ptr += ocp_nlp_qpscaling_opts_calculate_size(); + opts->globalization = config->globalization->opts_assign(config, dims, c_ptr); c_ptr += config->globalization->opts_calculate_size(config, dims); @@ -1224,6 +1231,9 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) // globalization globalization->opts_initialize_default(globalization, dims, opts->globalization); + // qpscaling + ocp_nlp_qpscaling_opts_initialize_default(dims->qpscaling, opts->qpscaling); + // dynamics for (int i = 0; i < N; i++) { @@ -1324,6 +1334,10 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value config->regularize->opts_set(config->regularize, opts->regularize, field+module_length+1, value); } + else if ( ptr_module!=NULL && (!strcmp(ptr_module, "qpscaling")) ) + { + ocp_nlp_qpscaling_opts_set(opts->qpscaling, field+module_length+1, value); + } else if ( ptr_module!=NULL && (!strcmp(ptr_module, "globalization")) ) { config->globalization->opts_set(config->globalization, opts->globalization, @@ -1598,6 +1612,9 @@ acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims // regularization size += config->regularize->memory_calculate_size(config->regularize, dims->regularize, opts->regularize); + // qpscaling + size += ocp_nlp_qpscaling_memory_calculate_size(dims->qpscaling, opts->qpscaling, dims->qp_solver->orig_dims); + // globalization size += config->globalization->memory_calculate_size(config->globalization, dims); @@ -1772,7 +1789,7 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims c_ptr += qp_solver->memory_calculate_size(qp_solver, dims->qp_solver, opts->qp_solver_opts); // regularization - mem->regularize = config->regularize->memory_assign(config->regularize, dims->regularize, + mem->regularize_mem = config->regularize->memory_assign(config->regularize, dims->regularize, opts->regularize, c_ptr); c_ptr += config->regularize->memory_calculate_size(config->regularize, dims->regularize, opts->regularize); @@ -1781,6 +1798,10 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims mem->globalization = config->globalization->memory_assign(config->globalization, dims, c_ptr); c_ptr += config->globalization->memory_calculate_size(config->globalization, dims); + // qpscaling + mem->qpscaling = ocp_nlp_qpscaling_memory_assign(dims->qpscaling, opts->qpscaling, dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_nlp_qpscaling_memory_calculate_size(dims->qpscaling, opts->qpscaling, dims->qp_solver->orig_dims); + int i; // dynamics for (i = 0; i < N; i++) @@ -2698,10 +2719,6 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di // set pointer to dmask in qp_in to dmask in nlp_in nlp_mem->qp_in->d_mask = nlp_in->dmask; - // alias to regularize memory - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); - // copy sampling times into dynamics model #if defined(ACADOS_WITH_OPENMP) #pragma omp for nowait @@ -3299,7 +3316,6 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl dims->nh_total += tmp; } - // precompute for (ii = 0; ii < N; ii++) { @@ -3330,6 +3346,15 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl config->cost[ii]->opts_set(config->cost[ii], opts->cost[ii], "compute_hess", &mem->compute_hess); } + ocp_nlp_qpscaling_precompute(dims->qpscaling, opts->qpscaling, mem->qpscaling, mem->qp_in, mem->qp_out); + + // alias from qp scaling memory (has to be after qpscaling precompute) + ocp_nlp_qpscaling_memory_get(dims->qpscaling, mem->qpscaling, "scaled_qp_in", 0, &mem->scaled_qp_in); + ocp_nlp_qpscaling_memory_get(dims->qpscaling, mem->qpscaling, "scaled_qp_out", 0, &mem->scaled_qp_out); + // alias to regularize memory + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, mem->regularize_mem, mem->scaled_qp_in); + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, mem->regularize_mem, mem->scaled_qp_out); + return status; } @@ -3706,7 +3731,7 @@ int ocp_nlp_common_setup_qp_matrices_and_factorize(ocp_nlp_config *config, ocp_n config->qp_solver->opts_set(config->qp_solver, nlp_opts->qp_solver_opts, "lam0_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); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, 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); @@ -3973,7 +3998,7 @@ void ocp_nlp_common_eval_lagr_grad_p(ocp_nlp_config *config, ocp_nlp_dims *dims, int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, - bool precondensed_lhs, ocp_qp_in *qp_in_, ocp_qp_out *qp_out_, + bool precondensed_lhs, ocp_qp_in *scaled_qp_in_, ocp_qp_in *qp_in_, ocp_qp_out *scaled_qp_out_, ocp_qp_out *qp_out_, ocp_qp_xcond_solver *xcond_solver) { acados_timer timer; @@ -4001,7 +4026,7 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims qp_work = xcond_solver->work; } - // qp_in_, qp_out_ are "optional", if NULL is given use nlp_mem->qp_in, nlp_mem->qp_out + // qp_in_, qp_out_, scaled_qp_out_ are "optional", if NULL is given use nlp_mem->scaled_qp_in, nlp_mem->qp_out, nlp_mem->scaled_qp_out ocp_qp_in *qp_in; if (qp_in_ == NULL) { @@ -4010,9 +4035,19 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims else { qp_in = qp_in_; - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, qp_in); } + ocp_qp_in *scaled_qp_in; + if (scaled_qp_in_ == NULL) + { + scaled_qp_in = nlp_mem->scaled_qp_in; + } + else + { + scaled_qp_in = scaled_qp_in_; + } + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, scaled_qp_in); + ocp_qp_out *qp_out = nlp_mem->qp_out; if (qp_out_ == NULL) { @@ -4021,9 +4056,19 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims else { qp_out = qp_out_; - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, qp_out); } + ocp_qp_out *scaled_qp_out; + if (scaled_qp_out_ == NULL) + { + scaled_qp_out = nlp_mem->scaled_qp_out; + } + else + { + scaled_qp_out = scaled_qp_out_; + } + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, scaled_qp_out); + ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings; double tmp_time; @@ -4034,12 +4079,12 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims if (precondensed_lhs) { qp_status = qp_solver->condense_rhs_and_solve(qp_solver, qp_dims, - qp_in, qp_out, qp_opts, qp_mem, qp_work); + scaled_qp_in, scaled_qp_out, qp_opts, qp_mem, qp_work); } else { qp_status = qp_solver->evaluate(qp_solver, qp_dims, - qp_in, qp_out, qp_opts, qp_mem, qp_work); + scaled_qp_in, scaled_qp_out, qp_opts, qp_mem, qp_work); } // add qp timings nlp_timings->time_qp_sol += acados_toc(&timer); @@ -4052,17 +4097,21 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims // compute correct dual solution in case of Hessian regularization acados_tic(&timer); config->regularize->correct_dual_sol(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize); + nlp_opts->regularize, nlp_mem->regularize_mem); nlp_timings->time_reg += acados_toc(&timer); - // reset regularize pointers if necessary - if (qp_in_ != NULL) + acados_tic(&timer); + ocp_nlp_qpscaling_rescale_solution(dims->qpscaling, nlp_opts->qpscaling, nlp_mem->qpscaling, qp_in, qp_out); + nlp_timings->time_qpscaling += acados_toc(&timer); + + // reset regularize pointers if necessary // TODO: check how to do this best with qpscaling + if (scaled_qp_in_ != NULL) { - ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); + ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->scaled_qp_in); } - if (qp_out_ != NULL) + if (scaled_qp_out_ != NULL) { - ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); + ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize_mem, nlp_mem->scaled_qp_out); } return qp_status; @@ -4165,6 +4214,10 @@ void ocp_nlp_timings_get(ocp_nlp_config *config, ocp_nlp_timings *timings, const { *value = timings->time_qp_solver_call; } + else if (!strcmp("time_qpscaling", field)) + { + *value = timings->time_qpscaling; + } else if (!strcmp("time_qp_xcond", field)) { *value = timings->time_qp_xcond; @@ -4276,6 +4329,10 @@ 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, "tau_iter", return_value_); } + else if (!strcmp("qpscaling_status", field)) + { + ocp_nlp_qpscaling_memory_get(NULL, nlp_mem->qpscaling, "status", 0, return_value_); + } else if (!strcmp("res_stat", field)) { double *value = return_value_; @@ -4346,6 +4403,7 @@ void ocp_nlp_timings_reset(ocp_nlp_timings *timings) timings->time_qp_sol = 0.0; timings->time_qp_solver_call = 0.0; timings->time_qp_xcond = 0.0; + timings->time_qpscaling = 0.0; timings->time_lin = 0.0; timings->time_reg = 0.0; timings->time_glob = 0.0; diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index f5d04baab1..0b3b66ce06 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -58,6 +58,7 @@ extern "C" { #include "acados/ocp_nlp/ocp_nlp_cost_common.h" #include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" #include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/ocp_nlp/ocp_nlp_qpscaling.h" #include "acados/ocp_nlp/ocp_nlp_globalization_common.h" #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/ocp_qp/ocp_qp_xcond_solver.h" @@ -146,6 +147,8 @@ typedef struct ocp_nlp_dims ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ?? ocp_qp_xcond_solver_dims *relaxed_qp_solver; // xcond solver instead ?? ocp_nlp_reg_dims *regularize; + ocp_nlp_qpscaling_dims *qpscaling; + ocp_nlp_qpscaling_dims *relaxed_qpscaling; int *nv; // number of primal variables (states+controls+slacks) int *nx; // number of differential states @@ -303,6 +306,7 @@ typedef struct ocp_nlp_opts { ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ??? void *regularize; + void *qpscaling; void *globalization; // globalization_opts void **dynamics; // dynamics_opts void **cost; // cost_opts @@ -387,6 +391,7 @@ typedef struct ocp_nlp_timings double time_qp_sol; double time_qp_solver_call; double time_qp_xcond; + double time_qpscaling; double time_lin; double time_reg; double time_tot; @@ -414,7 +419,8 @@ typedef struct ocp_nlp_memory { // void *qp_solver_mem; // xcond solver mem instead ??? ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ??? - void *regularize; + void *regularize_mem; + void *qpscaling; void *globalization; // globalization memory void **dynamics; // dynamics memory void **cost; // cost memory @@ -433,6 +439,10 @@ typedef struct ocp_nlp_memory ocp_qp_in *qp_in; ocp_qp_out *qp_out; + // scaled qp in & out (just pointers) + ocp_qp_in *scaled_qp_in; + ocp_qp_out *scaled_qp_out; + // for Anderson acceleration ocp_qp_out *prev_qp_out; ocp_qp_out *anderson_step; @@ -616,7 +626,7 @@ double ocp_nlp_get_l1_infeasibility(ocp_nlp_config *config, ocp_nlp_dims *dims, // int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, - bool precondensed_lhs, ocp_qp_in *qp_in_, ocp_qp_out *qp_out_, + bool precondensed_lhs, ocp_qp_in *scaled_qp_in_, ocp_qp_in *qp_in_, ocp_qp_out *scaled_qp_out_,ocp_qp_out *qp_out_, ocp_qp_xcond_solver *xcond_solver); // int ocp_nlp_solve_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts, diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index ba8fcec33f..1f0d4708f4 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -728,7 +728,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // NOTE: this is done before termination, such that we can get the QP at the stationary point that is actually solved, if we exit with success. acados_tic(&timer1); config->regularize->regularize(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize); + nlp_opts->regularize, nlp_mem->regularize_mem); nlp_timings->time_reg += acados_toc(&timer1); // Termination @@ -767,7 +767,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, print_ocp_qp_in(qp_in); } - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL, NULL, NULL); // restore default warm start if (ddp_iter==0) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index d763211cfa..ef81c83623 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -291,6 +291,7 @@ bool is_funnel_sufficient_decrease_satisfied(ocp_nlp_globalization_funnel_memory bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double pred_optimality, double step_size, double pred_infeasibility) { + // if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility) // if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility * pred_infeasibility) if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility) { @@ -581,7 +582,6 @@ int ocp_nlp_globalization_funnel_needs_objective_value() return 1; } -// QP objective value! Do not delete :D int ocp_nlp_globalization_funnel_needs_qp_objective_value() { return 1; @@ -590,8 +590,8 @@ int ocp_nlp_globalization_funnel_needs_qp_objective_value() // TODO(David): maybe rename to initialize void ocp_nlp_globalization_funnel_initialize_memory(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_) { - printf("Note: The funnel globalization is still under development.\n"); - printf("If you encouter problems or bugs, please report to the acados developers!\n"); + // printf("Note: The funnel globalization is still under development.\n"); + // printf("If you encouter problems or bugs, please report to the acados developers!\n"); ocp_nlp_config* config = config_; ocp_nlp_dims* dims = dims_; diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 97c60b066d..e4c1848e8c 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -693,7 +693,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // compute correct dual solution in case of Hessian regularization config->regularize->correct_dual_sol(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize); + nlp_opts->regularize, nlp_mem->regularize_mem); // ocp_qp_out_get(qp_out, "qp_info", &qp_info_); // int qp_iter = qp_info_->num_iter; diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c new file mode 100644 index 0000000000..903735a443 --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -0,0 +1,663 @@ +/* + * 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.; + */ + + +#include +#include +#include +#include +#include + +#include "acados/utils/math.h" +#include "acados/utils/mem.h" +#include "acados/utils/print.h" + +#include "acados/ocp_nlp/ocp_nlp_qpscaling.h" + +#include "blasfeo_d_aux.h" +#include "blasfeo_d_blas.h" +#include "blasfeo_d_aux_ext_dep.h" + + + +/************************************************ + * dims + ************************************************/ + +acados_size_t ocp_nlp_qpscaling_dims_calculate_size(int N) +{ + acados_size_t size = sizeof(ocp_nlp_qpscaling_dims); + // qp_dim + size += sizeof(ocp_qp_dims); + size += d_ocp_qp_dim_memsize(N); + size += 8; // align + make_int_multiple_of(8, &size); + + return size; +} + + + +ocp_nlp_qpscaling_dims *ocp_nlp_qpscaling_dims_assign(int N, void *raw_memory) +{ + char *c_ptr = (char *) raw_memory; + + // dims + ocp_nlp_qpscaling_dims *dims = (ocp_nlp_qpscaling_dims *) c_ptr; + c_ptr += sizeof(ocp_nlp_qpscaling_dims); + + // qp_dim + dims->qp_dim = (ocp_qp_dims *) c_ptr; + c_ptr += sizeof(ocp_qp_dims); + + align_char_to(8, &c_ptr); + + // qp_dim + d_ocp_qp_dim_create(N, dims->qp_dim, c_ptr); + c_ptr += d_ocp_qp_dim_memsize(N); + + assert((char *) raw_memory + ocp_nlp_qpscaling_dims_calculate_size(N) >= c_ptr); + + return dims; +} + + + +/************************************************ + * opts + ************************************************/ +acados_size_t ocp_nlp_qpscaling_opts_calculate_size(void) +{ + return sizeof(ocp_nlp_qpscaling_opts); +} + + + +void *ocp_nlp_qpscaling_opts_assign(void *raw_memory) +{ + return raw_memory; +} + + + +void ocp_nlp_qpscaling_opts_initialize_default(ocp_nlp_qpscaling_dims *dims, void *opts_) +{ + ocp_nlp_qpscaling_opts *opts = opts_; + + opts->lb_norm_inf_grad_obj = 1e-4; + opts->ub_max_abs_eig = 1e5; + opts->print_level = 0; + + opts->scale_qp_objective = NO_OBJECTIVE_SCALING; + opts->scale_qp_constraints = NO_CONSTRAINT_SCALING; + + return; +} + +void ocp_nlp_qpscaling_opts_set(void *opts_, const char *field, void* value) +{ + + ocp_nlp_qpscaling_opts *opts = opts_; + + if (!strcmp(field, "ub_max_abs_eig")) + { + double *d_ptr = value; + opts->ub_max_abs_eig = *d_ptr; + } + else if (!strcmp(field, "lb_norm_inf_grad_obj")) + { + double *d_ptr = value; + opts->lb_norm_inf_grad_obj = *d_ptr; + } + else if (!strcmp(field, "scale_objective")) + { + qpscaling_scale_objective_type *d_ptr = value; + opts->scale_qp_objective = *d_ptr; + } + else if (!strcmp(field, "scale_constraints")) + { + ocp_nlp_qpscaling_constraint_type *d_ptr = value; + opts->scale_qp_constraints = *d_ptr; + } + else + { + printf("\nerror: field %s not available in ocp_nlp_qpscaling_opts_set\n", field); + exit(1); + } + + return; + +} + + +/************************************************ + * memory + ************************************************/ + +acados_size_t ocp_nlp_qpscaling_memory_calculate_size(ocp_nlp_qpscaling_dims *dims, void *opts_, ocp_qp_dims *orig_qp_dim) +{ + int N = orig_qp_dim->N; + int i; + ocp_nlp_qpscaling_opts *opts = opts_; + + acados_size_t size = 0; + + size += sizeof(ocp_nlp_qpscaling_memory); + + if (opts->scale_qp_objective != NO_OBJECTIVE_SCALING || + opts->scale_qp_constraints != NO_CONSTRAINT_SCALING) + { + size += ocp_qp_in_calculate_size(orig_qp_dim); + size += ocp_qp_out_calculate_size(orig_qp_dim); + } + + // constraints_scaling_vec + if (opts->scale_qp_constraints) + { + size += (N + 1) * sizeof(struct blasfeo_dvec); // constraints_scaling_vec + for (i = 0; i <= N; i++) + { + size += blasfeo_memsize_dvec(orig_qp_dim->ng[i]); + } + } + + return size; +} + + + +void *ocp_nlp_qpscaling_memory_assign(ocp_nlp_qpscaling_dims *dims, void *opts_, ocp_qp_dims *orig_qp_dim, void *raw_memory) +{ + ocp_nlp_qpscaling_opts *opts = opts_; + char *c_ptr = (char *) raw_memory; + int N = orig_qp_dim->N; + + // setup qp dimensions + d_ocp_qp_dim_copy_all(orig_qp_dim, dims->qp_dim); + + ocp_nlp_qpscaling_memory *mem = (ocp_nlp_qpscaling_memory *) c_ptr; + c_ptr += sizeof(ocp_nlp_qpscaling_memory); + + mem->status = ACADOS_SUCCESS; + + if (opts->scale_qp_objective != NO_OBJECTIVE_SCALING || + opts->scale_qp_constraints != NO_CONSTRAINT_SCALING) + { + mem->scaled_qp_in = ocp_qp_in_assign(orig_qp_dim, c_ptr); + c_ptr += ocp_qp_in_calculate_size(orig_qp_dim); + mem->scaled_qp_out = ocp_qp_out_assign(orig_qp_dim, c_ptr); + c_ptr += ocp_qp_out_calculate_size(orig_qp_dim); + } + + if (opts->scale_qp_constraints) + { + assign_and_advance_blasfeo_dvec_structs(N + 1, &mem->constraints_scaling_vec, &c_ptr); + for (int i = 0; i <= N; ++i) + { + assign_and_advance_blasfeo_dvec_mem(orig_qp_dim->ng[i], mem->constraints_scaling_vec + i, &c_ptr); + blasfeo_dvecse(orig_qp_dim->ng[i], 1.0, mem->constraints_scaling_vec+i, 0); + } + } + assert((char *)mem + ocp_nlp_qpscaling_memory_calculate_size(dims, opts_, orig_qp_dim) >= c_ptr); + + return mem; +} + +/************************************************ + * getter functions + ************************************************/ +void *ocp_nlp_qpscaling_get_constraints_scaling_ptr(void *memory_, void* opts_) +{ + ocp_nlp_qpscaling_memory *mem = memory_; + ocp_nlp_qpscaling_opts *opts = opts_; + if (opts->scale_qp_constraints) + return mem->constraints_scaling_vec; + else + return NULL; +} + + +void ocp_nlp_qpscaling_memory_get(ocp_nlp_qpscaling_dims *dims, void *mem_, const char *field, int stage, void* value) +{ + ocp_nlp_qpscaling_memory *mem = mem_; + + if (!strcmp(field, "constr")) + { + double *ptr = value; + blasfeo_unpack_dvec(dims->qp_dim->ng[stage], mem->constraints_scaling_vec + stage, 0, ptr, 1); + } + else if (!strcmp(field, "obj")) + { + double *ptr = value; + *ptr = mem->obj_factor; + } + else if (!strcmp(field, "scaled_qp_in")) + { + ocp_qp_in **ptr = value; + *ptr = mem->scaled_qp_in; + } + else if (!strcmp(field, "scaled_qp_out")) + { + ocp_qp_out **ptr = value; + *ptr = mem->scaled_qp_out; + } + else if (!strcmp(field, "constraints_scaling_vec")) + { + struct blasfeo_dvec **ptr = value; + *ptr = mem->constraints_scaling_vec + stage; + } + else if (!strcmp(field, "status")) + { + int *ptr = value; + *ptr = mem->status; + } + else + { + printf("\nerror: ocp_nlp_qpscaling_memory_get: field %s not available\n", field); + exit(1); + } +} + + +/************************************************ + * helper functions + ************************************************/ +static double norm_inf_matrix_col(int col_idx, int col_length, struct blasfeo_dmat *At) +{ + double norm = 0.0; + for (int j = 0; j < col_length; ++j) + { + double tmp = BLASFEO_DMATEL(At, j, col_idx); + norm = fmax(norm, fabs(tmp)); + } + return norm; +} + + +static void rescale_solution_constraint_scaling(ocp_nlp_qpscaling_opts *opts, ocp_nlp_qpscaling_memory *mem, ocp_qp_in *qp_in, ocp_qp_out *qp_out) +{ + int *nb = qp_out->dim->nb; + int *ng = qp_out->dim->ng; + int *nx = qp_out->dim->nx; + int *nu = qp_out->dim->nu; + int *ns = qp_out->dim->ns; + int N = qp_out->dim->N; + double scaling_factor; + int s_idx; + + for (int i = 0; i <= N; i++) + { + // copy ux; + blasfeo_dveccp(nx[i]+nu[i]+2*ns[i], mem->scaled_qp_out->ux+i, 0, qp_out->ux+i, 0); + + if (opts->scale_qp_objective == NO_OBJECTIVE_SCALING) + { + // setup multipliers + blasfeo_dveccp(2*nb[i]+2*ng[i]+2*ns[i], mem->scaled_qp_out->lam+i, 0, qp_out->lam+i, 0); + if (i < N) + { + blasfeo_dveccp(nx[i+1], mem->scaled_qp_out->pi+i, 0, qp_out->pi+i, 0); + } + } + + // scale constraint multipliers + for (int j = 0; j < ng[i]; ++j) + { + scaling_factor = BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j); + + // scale lam of lower bound + BLASFEO_DVECEL(qp_out->lam+i, nb[i]+j) *= scaling_factor; + + // scale lam of upper bound + BLASFEO_DVECEL(qp_out->lam+i, 2*nb[i]+ng[i]+j) *= scaling_factor; + + s_idx = qp_in->idxs_rev[i][nb[i] + j]; // index of slack corresponding to this constraint + if (s_idx >= 0) + { + // scale slack bound multipliers + BLASFEO_DVECEL(qp_out->lam+i, 2*nb[i]+2*ng[i]+s_idx) *= scaling_factor; + BLASFEO_DVECEL(qp_out->lam+i, 2*nb[i]+2*ng[i]+ns[i]+s_idx) *= scaling_factor; + // scale slack variables + BLASFEO_DVECEL(qp_out->ux+i, nx[i]+nu[i]+s_idx) /= scaling_factor; + BLASFEO_DVECEL(qp_out->ux+i, nx[i]+nu[i]+ns[i]+s_idx) /= scaling_factor; + } + } + } +} + + +static void ocp_qp_scale_objective(ocp_nlp_qpscaling_memory *mem, ocp_qp_in *qp_in, double factor) +{ + int *nx = qp_in->dim->nx; + int *nu = qp_in->dim->nu; + int *ns = qp_in->dim->ns; + + for (int stage = 0; stage <= qp_in->dim->N; stage++) + { + // scale cost + blasfeo_dveccpsc(nx[stage]+nu[stage]+2*ns[stage], factor, qp_in->rqz+stage, 0, mem->scaled_qp_in->rqz+stage, 0); + blasfeo_dveccpsc(2*ns[stage], factor, qp_in->Z+stage, 0, mem->scaled_qp_in->Z+stage, 0); + blasfeo_dgecpsc(nx[stage]+nu[stage], nx[stage]+nu[stage], factor, qp_in->RSQrq+stage, 0, 0, mem->scaled_qp_in->RSQrq+stage, 0, 0); + } +} + + + +static void ocp_qp_out_scale_duals(ocp_nlp_qpscaling_memory *mem, ocp_qp_out *qp_out, double factor) +{ + struct d_ocp_qp_dim *qp_dim = qp_out->dim; + for (int i = 0; i <= qp_dim->N; i++) + { + blasfeo_dveccpsc(2*(qp_dim->nb[i]+qp_dim->ng[i]+qp_dim->ns[i]), factor, mem->scaled_qp_out->lam+i, 0, qp_out->lam+i, 0); + if (i < qp_dim->N) + { + blasfeo_dveccpsc(qp_dim->nx[i+1], factor, mem->scaled_qp_out->pi+i, 0, qp_out->pi+i, 0); + } + } +} + + + +/************************************************ + * functions + ************************************************/ + +void ocp_nlp_qpscaling_precompute(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in, ocp_qp_out *qp_out) +{ + ocp_nlp_qpscaling_opts *opts = opts_; + ocp_nlp_qpscaling_memory *mem = mem_; + + // alias stuff that is the same between scaled and unscaled versions + if (opts->scale_qp_constraints == NO_CONSTRAINT_SCALING && opts->scale_qp_objective == NO_OBJECTIVE_SCALING) + { + mem->scaled_qp_in = qp_in; + mem->scaled_qp_out = qp_out; + } + else if (opts->scale_qp_constraints == NO_CONSTRAINT_SCALING) + { + /* qp_in */ + // only cost scaling -> alias everything not touched + mem->scaled_qp_in->b = qp_in->b; + mem->scaled_qp_in->BAbt = qp_in->BAbt; + mem->scaled_qp_in->d = qp_in->d; + mem->scaled_qp_in->d_mask = qp_in->d_mask; + mem->scaled_qp_in->diag_H_flag = qp_in->diag_H_flag; + mem->scaled_qp_in->DCt = qp_in->DCt; + mem->scaled_qp_in->dim = qp_in->dim; + mem->scaled_qp_in->idxb = qp_in->idxb; + mem->scaled_qp_in->idxe = qp_in->idxe; + mem->scaled_qp_in->idxs_rev = qp_in->idxs_rev; + mem->scaled_qp_in->m = qp_in->m; + // NOT aliased: rqz, RSQrq, Z + /* qp_out */ + mem->scaled_qp_out->ux = qp_out->ux; + mem->scaled_qp_out->misc = qp_out->misc; + mem->scaled_qp_out->dim = qp_out->dim; + mem->scaled_qp_out->t = qp_out->t; + // NOT aliased: lam, pi + } + else + { + /* qp_in */ + // constraint scaling (& maybe cost scaling) -> alias everything not touched + mem->scaled_qp_in->b = qp_in->b; + mem->scaled_qp_in->BAbt = qp_in->BAbt; + mem->scaled_qp_in->d_mask = qp_in->d_mask; + mem->scaled_qp_in->diag_H_flag = qp_in->diag_H_flag; + mem->scaled_qp_in->dim = qp_in->dim; + mem->scaled_qp_in->idxb = qp_in->idxb; + mem->scaled_qp_in->idxe = qp_in->idxe; + mem->scaled_qp_in->idxs_rev = qp_in->idxs_rev; + mem->scaled_qp_in->m = qp_in->m; + // NOT aliased: rqz, Z, d, DCt + if (opts->scale_qp_objective == NO_OBJECTIVE_SCALING) + { + // alias RSQrq, otherwise it is set up in objective scaling + mem->scaled_qp_in->RSQrq = qp_in->RSQrq; + } + + /* qp_out */ + mem->scaled_qp_out->misc = qp_out->misc; + mem->scaled_qp_out->dim = qp_out->dim; + mem->scaled_qp_out->t = qp_out->t; + // NOT aliased: lam, pi, ux + } +} + +/** + * @brief Scales the objective function of an OCP QP using Gershgorin eigenvalue estimates. + * + * - estimate max. abs. eigenvalue using gershgorin circles as max_abs_eig + * - obj_factor = min(1.0, ub_max_abs_eig/max_abs_eig) + */ +void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in) +{ + double max_abs_eig = 0.0; + double tmp, max_upscale_factor, lb_grad_norm_factor; + + ocp_qp_dims *dim = dims->qp_dim; + ocp_nlp_qpscaling_memory *mem = mem_; + ocp_nlp_qpscaling_opts *opts = opts_; + + int *nx = dim->nx; + int *nu = dim->nu; + int *ns = dim->ns; + + struct blasfeo_dmat *RSQrq = qp_in->RSQrq; + double nrm_inf_grad_obj = 0.0; + for (int stage = 0; stage <= dim->N; stage++) + { + compute_gershgorin_max_abs_eig_estimate(nx[stage]+nu[stage], RSQrq+stage, &tmp); + max_abs_eig = fmax(max_abs_eig, tmp); + // take Z into account + blasfeo_dvecnrm_inf(2*ns[stage], qp_in->Z+stage, 0, &tmp); + max_abs_eig = fmax(max_abs_eig, tmp); + + // norm gradient + blasfeo_dvecnrm_inf(nx[stage]+nu[stage]+2*ns[stage], qp_in->rqz+stage, 0, &tmp); + nrm_inf_grad_obj = fmax(nrm_inf_grad_obj, fabs(tmp)); + } + + if (max_abs_eig < opts->ub_max_abs_eig) + { + mem->obj_factor = 1.0; + max_upscale_factor = opts->ub_max_abs_eig / max_abs_eig; + } + else + { + // scale objective down + mem->obj_factor = opts->ub_max_abs_eig / max_abs_eig; + max_upscale_factor = mem->obj_factor; + } + + if (mem->obj_factor*nrm_inf_grad_obj <= opts->lb_norm_inf_grad_obj) + { + // grad norm would become too small -> scale cost up + if (opts->print_level > 0) + { + printf("lb_norm_inf_grad_obj violated! %.2e\n", opts->lb_norm_inf_grad_obj); + printf("Gradient is very small! %.2e\n", mem->obj_factor*nrm_inf_grad_obj); + } + lb_grad_norm_factor = opts->lb_norm_inf_grad_obj / nrm_inf_grad_obj; + tmp = fmin(max_upscale_factor, lb_grad_norm_factor); + mem->obj_factor = fmax(mem->obj_factor, tmp); + mem->status = ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED; + } + if (opts->print_level > 0) + { + printf("Scaling factor objective: %.2e\n", mem->obj_factor); + } +} + + +// calculate scaling factors for all inequality constraints (except bounds) of the QP. +// The scaling factor is calculated as the maximum of the linear coefficients of the constraint and the maximum of the bounds. +void ocp_nlp_qpscaling_scale_constraints(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in) +{ + int *nx = qp_in->dim->nx; + int *nu = qp_in->dim->nu; + int *nb = qp_in->dim->nb; + int *ng = qp_in->dim->ng; + int *ns = qp_in->dim->ns; + int N = qp_in->dim->N; + int i, j, s_idx; + double mask_value_lower, mask_value_upper; + ocp_nlp_qpscaling_memory *mem = mem_; + ocp_nlp_qpscaling_opts *opts = opts_; + double coeff_norm, scaling_factor; + ocp_qp_in *scaled_qp_in = mem->scaled_qp_in; + + for (i = 0; i <= N; i++) + { + // setup all non-aliased stuff + blasfeo_dveccp(2*(nb[i]+ng[i]+ns[i]), qp_in->d+i, 0, scaled_qp_in->d+i, 0); + // copy cost, if not already done via cost scaling + if (opts->scale_qp_objective == NO_OBJECTIVE_SCALING) + { + blasfeo_dveccp(nx[i]+nu[i]+2*ns[i], qp_in->rqz+i, 0, scaled_qp_in->rqz+i, 0); + blasfeo_dveccp(2*ns[i], qp_in->Z+i, 0, scaled_qp_in->Z+i, 0); + } + // setup DCt, modify d in place + for (j = 0; j < ng[i]; j++) + { + coeff_norm = norm_inf_matrix_col(j, nu[i]+nx[i], qp_in->DCt+i); + mask_value_lower = BLASFEO_DVECEL(qp_in->d_mask+i, nb[i]+j); + mask_value_upper = BLASFEO_DVECEL(qp_in->d_mask+i, 2*nb[i]+ng[i]+j); + + // calculate scaling factor from row norm + double bound_max = fmax(fabs(mask_value_lower * BLASFEO_DVECEL(qp_in->d+i, nb[i]+j)), + fabs(mask_value_upper * BLASFEO_DVECEL(qp_in->d+i, 2*nb[i]+ng[i]+j))); + // only scale down. + scaling_factor = 1.0 / fmax(1.0, fmax(bound_max, coeff_norm)); + + // store scaling factor + BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j) = scaling_factor; + + // scale the constraint + blasfeo_dgecpsc(nu[i]+nx[i], 1, scaling_factor, qp_in->DCt+i, 0, j, mem->scaled_qp_in->DCt+i, 0, j); + + s_idx = qp_in->idxs_rev[i][nb[i] + j]; // index of slack corresponding to this constraint + if (s_idx != -1) + { + // printf("Scaling slack %d for constraint %d at stage %d with factor %.2e\n", s_idx, j, i, scaling_factor); + // scale associated slack cost + // lower + BLASFEO_DVECEL(mem->scaled_qp_in->rqz+i, nu[i]+nx[i]+s_idx) /= scaling_factor; + BLASFEO_DVECEL(mem->scaled_qp_in->Z+i, s_idx) /= (scaling_factor*scaling_factor); + // upper + BLASFEO_DVECEL(mem->scaled_qp_in->rqz+i, nu[i]+nx[i]+ns[i]+s_idx) /= scaling_factor; + BLASFEO_DVECEL(mem->scaled_qp_in->Z+i, ns[i]+s_idx) /= (scaling_factor*scaling_factor); + // scale slack bounds + BLASFEO_DVECEL(mem->scaled_qp_in->d+i, 2*(nb[i]+ng[i])+s_idx) *= scaling_factor; + BLASFEO_DVECEL(mem->scaled_qp_in->d+i, 2*(nb[i]+ng[i])+ns[i]+s_idx) *= scaling_factor; + } + + // scale lower bound + if (mask_value_lower == 1.0) + { + BLASFEO_DVECEL(mem->scaled_qp_in->d+i, nb[i]+j) *= scaling_factor; + } + + // scale upper bound + if (mask_value_upper == 1.0) + { + BLASFEO_DVECEL(mem->scaled_qp_in->d+i, 2*nb[i]+ng[i]+j) *= scaling_factor; + } + } + } +} + + +static void print_qp_scaling_factors_constr(ocp_nlp_qpscaling_dims *dims, ocp_nlp_qpscaling_opts *opts, ocp_nlp_qpscaling_memory *mem) +{ + if (opts->scale_qp_constraints) + { + printf("Scaling factors for constraints:\n"); + for (int i = 0; i <= dims->qp_dim->N; i++) + { + printf("Stage %d: ", i); + for (int j = 0; j < dims->qp_dim->ng[i]; j++) + { + printf("%.2e ", BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j)); + } + printf("\n"); + } + } +} + +void ocp_nlp_qpscaling_scale_qp(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in) +{ + ocp_nlp_qpscaling_opts *opts = opts_; + ocp_nlp_qpscaling_memory *mem = mem_; + + mem->status = ACADOS_SUCCESS; + if (opts->scale_qp_objective) + { + ocp_nlp_qpscaling_compute_obj_scaling_factor(dims, opts_, mem_, qp_in); + ocp_qp_scale_objective(mem, qp_in, mem->obj_factor); + } + else + { + // set the obj_factor to 1.0, for consinstency + mem->obj_factor = 1.0; + } + + if (opts->scale_qp_constraints) + { + ocp_nlp_qpscaling_scale_constraints(dims, opts_, mem_, qp_in); + } + if (opts->print_level > 0) + { + print_qp_scaling_factors_constr(dims, opts, mem); + } + // printf("qp_in AFTER SCALING\n"); + // print_ocp_qp_in(qp_in); +} + + +void ocp_nlp_qpscaling_rescale_solution(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in, ocp_qp_out *qp_out) +{ + ocp_nlp_qpscaling_memory *mem = mem_; + ocp_nlp_qpscaling_opts *opts = opts_; + + if (opts->scale_qp_objective) + { + ocp_qp_out_scale_duals(mem, qp_out, 1.0/mem->obj_factor); + } + if (opts->scale_qp_constraints) + { + rescale_solution_constraint_scaling(opts, mem, qp_in, qp_out); + qp_info *info = (qp_info *) qp_out->misc; + info->t_computed = 0; // t needs to be recomputed if needed. + } + // printf("qp_out AFTER RESCALING\n"); + // print_ocp_qp_out(qp_out); + return; +} diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.h b/acados/ocp_nlp/ocp_nlp_qpscaling.h new file mode 100644 index 0000000000..3cd98d0021 --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.h @@ -0,0 +1,108 @@ +/* + * 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.; + */ + + +#ifndef ACADOS_OCP_NLP_OCP_NLP_QPSCALING_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_QPSCALING_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" + + +/* dims */ + +// same as qp_dims +typedef struct +{ + ocp_qp_dims *qp_dim; +} ocp_nlp_qpscaling_dims; + +// +acados_size_t ocp_nlp_qpscaling_dims_calculate_size(int N); +// +ocp_nlp_qpscaling_dims *ocp_nlp_qpscaling_dims_assign(int N, void *raw_memory); + +/************************************************ + * options + ************************************************/ +typedef struct +{ + double ub_max_abs_eig; + double lb_norm_inf_grad_obj; + int print_level; + + qpscaling_scale_objective_type scale_qp_objective; + ocp_nlp_qpscaling_constraint_type scale_qp_constraints; +} ocp_nlp_qpscaling_opts; +// use all functions just through config pointers + + +typedef struct { + int status; + double obj_factor; + struct blasfeo_dvec *constraints_scaling_vec; + ocp_qp_in *scaled_qp_in; + ocp_qp_out *scaled_qp_out; +} ocp_nlp_qpscaling_memory; + +acados_size_t ocp_nlp_qpscaling_opts_calculate_size(void); +void ocp_nlp_qpscaling_opts_initialize_default(ocp_nlp_qpscaling_dims *dims, void *opts_); +void *ocp_nlp_qpscaling_opts_assign(void *raw_memory); +void ocp_nlp_qpscaling_opts_set(void *opts_, const char *field, void* value); + +// + +/************************************************ + * memory + ************************************************/ + +acados_size_t ocp_nlp_qpscaling_memory_calculate_size(ocp_nlp_qpscaling_dims *dims, void *opts_, ocp_qp_dims *orig_qp_dim); +void *ocp_nlp_qpscaling_memory_assign(ocp_nlp_qpscaling_dims *dims, void *opts_, ocp_qp_dims *orig_qp_dim, void *raw_memory); +void *ocp_nlp_qpscaling_get_constraints_scaling_ptr(void *memory_, void* opts_); +void ocp_nlp_qpscaling_memory_get(ocp_nlp_qpscaling_dims *dims, void *mem_, const char *field, int stage, void* value); + + +/************************************************ + * functionality + ************************************************/ +void ocp_nlp_qpscaling_precompute(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in, ocp_qp_out *qp_out); +// +void ocp_nlp_qpscaling_scale_qp(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in); + +void ocp_nlp_qpscaling_rescale_solution(ocp_nlp_qpscaling_dims *dims, void *opts_, void *mem_, ocp_qp_in *qp_in, ocp_qp_out *qp_out); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_QPSCALING_COMMON_H_ diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index cd82915811..b326c5b406 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -561,6 +561,9 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_qp_in *qp_in = nlp_mem->qp_in; ocp_qp_out *qp_out = nlp_mem->qp_out; + qp_info *qp_info_; + ocp_qp_out_get(qp_out, "qp_info", &qp_info_); + // zero timers ocp_nlp_timings_reset(nlp_timings); @@ -589,7 +592,6 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, nlp_mem->iter = 0; double prev_levenberg_marquardt = 0.0; int globalization_status; - qp_info *qp_info_; double timeout_previous_time_tot = 0.; double timeout_time_prev_iter = 0.; @@ -617,7 +619,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } - ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, nlp_mem->qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, qp_in); nlp_timings->time_lin += acados_toc(&timer1); // compute nlp residuals @@ -647,11 +649,16 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } prev_levenberg_marquardt = nlp_opts->levenberg_marquardt; + // QP scaling + acados_tic(&timer1); + ocp_nlp_qpscaling_scale_qp(dims->qpscaling, nlp_opts->qpscaling, nlp_mem->qpscaling, qp_in); + nlp_timings->time_qpscaling += acados_toc(&timer1); + // regularize Hessian // NOTE: this is done before termination, such that we can get the QP at the stationary point that is actually solved, if we exit with success. acados_tic(&timer1); config->regularize->regularize(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize); + nlp_opts->regularize, nlp_mem->regularize_mem); nlp_timings->time_reg += acados_toc(&timer1); // update timeout memory based on chosen heuristic @@ -733,7 +740,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL, NULL, NULL); // restore default warm start if (nlp_mem->iter==0) @@ -751,7 +758,6 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 0); #endif - ocp_qp_out_get(qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; // save statistics of last qp solver call @@ -764,7 +770,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // compute external QP residuals (for debugging) if (nlp_opts->ext_qp_res) { - ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); + ocp_qp_res_compute(nlp_mem->scaled_qp_in, nlp_mem->scaled_qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); if (nlp_mem->iter+1 < mem->stat_m) ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, mem->stat+(mem->stat_n*(nlp_mem->iter+1)+7)); } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 2c0d8793c1..7688a8005f 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -498,7 +498,7 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim // regularize Hessian acados_tic(&timer1); config->regularize->regularize_lhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); // condense lhs acados_tic(&timer1); @@ -549,13 +549,13 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * { // finish regularization config->regularize->regularize_rhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); } else if (opts->rti_phase == PREPARATION_AND_FEEDBACK) { // full regularization config->regularize->regularize(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); } else { @@ -591,7 +591,7 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * { precondensed_lhs = false; } - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, precondensed_lhs, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, precondensed_lhs, NULL, NULL, NULL, NULL, NULL); qp_info *qp_info_; ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); @@ -856,10 +856,10 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // regularization rhs acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); // solve QP - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL, NULL, NULL); // save statistics ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); @@ -908,11 +908,11 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // rhs regularization acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); // QP solve - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL, NULL, NULL); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -924,7 +924,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // compute correct dual solution in case of Hessian regularization acados_tic(&timer1); config->regularize->correct_dual_sol(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) { @@ -978,11 +978,11 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // rhs regularization acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); // QP solve - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, true, NULL, NULL, NULL, NULL, NULL); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -1050,11 +1050,11 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // full regularization acados_tic(&timer1); config->regularize->regularize(config->regularize, - dims->regularize, nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); // QP solve - qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL); + qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL, NULL, NULL); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -1098,7 +1098,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc // regularize Hessian acados_tic(&timer1); config->regularize->regularize_lhs(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize); + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); timings->time_reg += acados_toc(&timer1); // condense lhs qp_solver->condense_lhs(qp_solver, dims->qp_solver, diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 528c943bd9..d26c4a660d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -315,6 +315,8 @@ acados_size_t ocp_nlp_sqp_wfqp_memory_calculate_size(void *config_, void *dims_, size += ocp_qp_xcond_solver_workspace_calculate_size(config->relaxed_qp_solver, dims->relaxed_qp_solver, opts->nlp_opts->qp_solver_opts); size += ocp_qp_in_calculate_size(dims->relaxed_qp_solver->orig_dims); size += ocp_qp_out_calculate_size(dims->relaxed_qp_solver->orig_dims); + // relaxed qpscaling + size += ocp_nlp_qpscaling_memory_calculate_size(dims->qpscaling, nlp_opts->qpscaling, dims->relaxed_qp_solver->orig_dims); // stat int stat_m = opts->nlp_opts->max_iter+1; @@ -412,6 +414,10 @@ void *ocp_nlp_sqp_wfqp_memory_assign(void *config_, void *dims_, void *opts_, vo c_ptr += ocp_qp_in_calculate_size(dims->relaxed_qp_solver->orig_dims); mem->relaxed_qp_out = ocp_qp_out_assign(dims->relaxed_qp_solver->orig_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->relaxed_qp_solver->orig_dims); + // qpscaling + mem->relaxed_qpscaling_mem = ocp_nlp_qpscaling_memory_assign(dims->relaxed_qpscaling, opts->nlp_opts->qpscaling, dims->relaxed_qp_solver->orig_dims, c_ptr); + c_ptr += ocp_nlp_qpscaling_memory_calculate_size(dims->relaxed_qpscaling, opts->nlp_opts->qpscaling, dims->relaxed_qp_solver->orig_dims); + // nominal mem->relaxed_qp_solver.config = config->relaxed_qp_solver; mem->relaxed_qp_solver.dims = dims->relaxed_qp_solver; @@ -419,7 +425,7 @@ void *ocp_nlp_sqp_wfqp_memory_assign(void *config_, void *dims_, void *opts_, vo mem->relaxed_qp_solver.mem = mem->relaxed_qp_solver_mem; mem->relaxed_qp_solver.work = mem->relaxed_qp_solver_work; - set_relaxed_qp_in_matrix_pointers(mem, mem->nlp_mem->qp_in); + set_relaxed_qp_in_matrix_pointers(mem); // Z_cost_module assign_and_advance_blasfeo_dvec_structs(N + 1, &mem->Z_cost_module, &c_ptr); @@ -957,17 +963,19 @@ static void setup_hessian_matrices_for_qps(ocp_nlp_config *config, /* Solves the QP. Either solves feasibility QP or nominal QP */ -static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* opts, ocp_qp_in* qp_in, ocp_qp_out* qp_out, +static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* opts, + ocp_qp_in* scaled_qp_in, ocp_qp_in* qp_in, ocp_qp_out* scaled_qp_out, ocp_qp_out* qp_out, ocp_nlp_dims *dims, ocp_nlp_sqp_wfqp_memory* mem, ocp_nlp_in* nlp_in, ocp_nlp_out* nlp_out, ocp_nlp_memory* nlp_mem, ocp_nlp_workspace* nlp_work, bool solve_feasibility_qp, acados_timer timer_tot) { - acados_timer timer_qp; + acados_timer timer; ocp_nlp_opts* nlp_opts = opts->nlp_opts; ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings; int qp_status = ACADOS_SUCCESS; + // printf("\nprepare_and_solve_QP: solve_feasibility_qp %d\n", solve_feasibility_qp); // warm start of first QP if (nlp_mem->iter == 0) { @@ -993,10 +1001,10 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, qp_in); } + acados_tic(&timer); // regularize Hessian - acados_tic(&timer_qp); - config->regularize->regularize(config->regularize, dims->regularize, nlp_opts->regularize, nlp_mem->regularize); - nlp_timings->time_reg += acados_toc(&timer_qp); + config->regularize->regularize(config->regularize, dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + nlp_timings->time_reg += acados_toc(&timer); } // Show input to QP @@ -1004,33 +1012,35 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o { printf("\n\nSQP: ocp_qp_in at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_in->dim); - print_ocp_qp_in(qp_in); + print_ocp_qp_in(scaled_qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif - if (solve_feasibility_qp) { if (opts->use_constraint_hessian_in_feas_qp) { qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, - qp_in, qp_out, &mem->relaxed_qp_solver); + scaled_qp_in, qp_in, scaled_qp_out, qp_out, &mem->relaxed_qp_solver); } else { // dont regularize Hessian for feasibility QP qp_status = ocp_nlp_solve_qp(config, dims, nlp_opts, - nlp_mem, nlp_work, qp_in, qp_out, &mem->relaxed_qp_solver); + nlp_mem, nlp_work, scaled_qp_in, scaled_qp_out, &mem->relaxed_qp_solver); + acados_tic(&timer); + ocp_nlp_qpscaling_rescale_solution(dims->relaxed_qpscaling, nlp_opts->qpscaling, mem->relaxed_qpscaling_mem, qp_in, qp_out); + nlp_timings->time_qpscaling += acados_toc(&timer); } } else { qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, - NULL, NULL, NULL); + NULL, NULL, NULL, NULL, NULL); } mem->qps_solved_in_iter += 1; @@ -1044,7 +1054,7 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o { printf("\n\nSQP: ocp_qp_out at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_out->dim); - print_ocp_qp_out(qp_out); + print_ocp_qp_out(scaled_qp_out); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) @@ -1052,11 +1062,8 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o #endif // exit conditions on QP status - if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) + if (qp_status!=ACADOS_SUCCESS) { - // increment nlp_mem->iter to return full statistics and improve output below. - nlp_mem->iter++; - if (nlp_opts->print_level > 1) { printf("\n Failed to solve the following QP:\n"); @@ -1133,8 +1140,8 @@ static void setup_byrd_omojokun_bounds(ocp_nlp_dims *dims, ocp_nlp_memory *nlp_m int *ng = dims->ng; int *ni_nl = dims->ni_nl; - ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; - ocp_qp_out *relaxed_qp_out = mem->relaxed_qp_out; + ocp_qp_in *nominal_qp_in = nlp_mem->scaled_qp_in; + ocp_qp_out *relaxed_qp_out = mem->relaxed_scaled_qp_out; int i, j; double tmp_lower, tmp_upper; @@ -1181,8 +1188,12 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, ocp_nlp_workspace* nlp_work = work->nlp_work; ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; + ocp_qp_in *nominal_scaled_qp_in = nlp_mem->scaled_qp_in; ocp_qp_out *nominal_qp_out = nlp_mem->qp_out; + ocp_qp_out *nominal_scaled_qp_out = nlp_mem->scaled_qp_out; + ocp_qp_in *relaxed_qp_in = mem->relaxed_qp_in; + ocp_qp_in *relaxed_scaled_qp_in = mem->relaxed_scaled_qp_in; ocp_qp_out *relaxed_qp_out = mem->relaxed_qp_out; ocp_nlp_timings *nlp_timings = nlp_mem->nlp_timings; @@ -1190,11 +1201,10 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, int qp_status; int qp_iter = 0; - double l1_inf_QP_feasibility; /* Solve Feasibility QP: Objective: Only constraint Hessian/Identity AND only gradient of slack variables */ print_debug_output("Solve Feasibility QP!\n", nlp_opts->print_level, 2); - qp_status = prepare_and_solve_QP(config, opts, relaxed_qp_in, relaxed_qp_out, dims, mem, nlp_in, nlp_out, + qp_status = prepare_and_solve_QP(config, opts, relaxed_scaled_qp_in, relaxed_qp_in, mem->relaxed_scaled_qp_out, relaxed_qp_out, dims, mem, nlp_in, nlp_out, nlp_mem, nlp_work, true, timer_tot); ocp_qp_out_get(relaxed_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -1210,17 +1220,14 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, return nlp_mem->status; } - if (config->globalization->needs_objective_value() == 1) - { - l1_inf_QP_feasibility = calculate_qp_l1_infeasibility(dims, mem, work, opts, relaxed_qp_in, relaxed_qp_out); - mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); - } + // here was the calculation of pred_infeasibility earlier, moved outside /* Solve the nominal QP with updated bounds*/ print_debug_output("Solve Nominal QP!\n", nlp_opts->print_level, 2); setup_byrd_omojokun_bounds(dims, nlp_mem, mem, work, opts); // solve_feasibility_qp --> false in prepare_and_solve_QP - qp_status = prepare_and_solve_QP(config, opts, nominal_qp_in, nominal_qp_out, dims, mem, nlp_in, nlp_out, + + qp_status = prepare_and_solve_QP(config, opts, nominal_scaled_qp_in, nominal_qp_in, nominal_scaled_qp_out, nominal_qp_out, dims, mem, nlp_in, nlp_out, nlp_mem, nlp_work, false, timer_tot); ocp_qp_out_get(nominal_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -1289,17 +1296,33 @@ Feasibility QP and nominal QP share many entries. - Constraint bounds are different Where we point to the same memory for both QPs is given below. */ -void set_relaxed_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem, ocp_qp_in *qp_in) +void set_relaxed_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem) { + ocp_qp_in *qp_in = mem->nlp_mem->qp_in; + // dynamics mem->relaxed_qp_in->BAbt = qp_in->BAbt; // dynamics matrix & vector work space - mem->relaxed_qp_in->b = qp_in->b; // dynamics vector work space + mem->relaxed_qp_in->b = qp_in->b; // dynamics vector + // constraint defintitions mem->relaxed_qp_in->DCt = qp_in->DCt; // inequality constraints matrix mem->relaxed_qp_in->idxb = qp_in->idxb; mem->relaxed_qp_in->idxe = qp_in->idxe; } +static void set_relaxed_scaled_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem) +{ + ocp_qp_in *scaled_qp_in = mem->nlp_mem->scaled_qp_in; + + // dynamics + mem->relaxed_scaled_qp_in->BAbt = scaled_qp_in->BAbt; + mem->relaxed_scaled_qp_in->b = scaled_qp_in->b; + // constraint defintitions + mem->relaxed_scaled_qp_in->DCt = scaled_qp_in->DCt; + mem->relaxed_scaled_qp_in->idxb = scaled_qp_in->idxb; + mem->relaxed_scaled_qp_in->idxe = scaled_qp_in->idxe; +} + /* update QP rhs for feasibility QP (step prim var, abs dual var) - copy d parts from nominal QP and include bounds of QP slacks @@ -1307,11 +1330,21 @@ update QP rhs for feasibility QP (step prim var, abs dual var) */ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, - ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work) + ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work, bool scaled) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; - ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; - ocp_qp_in *relaxed_qp_in = mem->relaxed_qp_in; + ocp_qp_in *nominal_qp_in; + ocp_qp_in *relaxed_qp_in; + if (scaled) + { + nominal_qp_in = nlp_mem->scaled_qp_in; + relaxed_qp_in = mem->relaxed_scaled_qp_in; + } + else + { + nominal_qp_in = nlp_mem->qp_in; + relaxed_qp_in = mem->relaxed_qp_in; + } int N = dims->N; int *ns = dims->ns; @@ -1445,9 +1478,9 @@ static int calculate_search_direction(ocp_nlp_dims *dims, { // if the QP can be solved and the status is good, we return 0 // otherwise, we change the mode to Byrd-Omojokun and we continue. - search_direction_status = prepare_and_solve_QP(config, opts, nlp_mem->qp_in, nlp_mem->qp_out, - dims, mem, nlp_in, nlp_out, nlp_mem, work->nlp_work, - false, timer_tot); + search_direction_status = prepare_and_solve_QP(config, opts, nlp_mem->scaled_qp_in, + nlp_mem->qp_in, nlp_mem->scaled_qp_out, nlp_mem->qp_out, + dims, mem, nlp_in, nlp_out, nlp_mem, work->nlp_work, false, timer_tot); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; log_qp_stats(mem, false, search_direction_status, qp_iter); @@ -1487,8 +1520,14 @@ static int calculate_search_direction(ocp_nlp_dims *dims, mem->search_direction_type = "FN"; } search_direction_status = byrd_omojokun_direction_computation(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); - double l1_inf = calculate_qp_l1_infeasibility(dims, mem, work, opts, mem->relaxed_qp_in, mem->relaxed_qp_out); - if (l1_inf/(fmax(1.0, (double) mem->absolute_nns)) < opts->tol_ineq) + + double l1_inf_QP_feasibility = calculate_qp_l1_infeasibility(dims, mem, work, opts, mem->relaxed_qp_in, mem->relaxed_qp_out); + if (config->globalization->needs_objective_value() == 1) + { + mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); + } + + if (l1_inf_QP_feasibility/(fmax(1.0, (double) mem->absolute_nns)) < opts->tol_ineq) { mem->watchdog_zero_slacks_counter += 1; } @@ -1603,18 +1642,18 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); - // relaxed QP solver - // matrices for relaxed QP solver evaluated in nominal QP solver - ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work); - if (nlp_opts->with_adaptive_levenberg_marquardt || config->globalization->needs_objective_value() == 1) { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } + // setup relaxed QP + // matrices for relaxed QP solver evaluated in nominal QP solver + ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work, false); setup_hessian_matrices_for_qps(config, dims, nlp_in, nlp_out, opts, mem, nlp_work); // nlp_timings->time_lin += acados_toc(&timer1); + // compute nlp residuals ocp_nlp_res_compute(dims, nlp_opts, nlp_in, nlp_out, nlp_res, nlp_mem, nlp_work); ocp_nlp_res_get_inf_norm(nlp_res, &nlp_out->inf_norm_res); @@ -1660,6 +1699,17 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->l1_infeasibility = ocp_nlp_get_l1_infeasibility(config, dims, nlp_mem); } + /* Scale the QP */ + // scale the qp: includes constraints and objective + acados_tic(&timer1); + ocp_nlp_qpscaling_scale_qp(dims->qpscaling, nlp_opts->qpscaling, nlp_mem->qpscaling, nominal_qp_in); + nlp_timings->time_qpscaling += acados_toc(&timer1); + ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work, true); + + acados_tic(&timer1); + ocp_nlp_qpscaling_scale_qp(dims->relaxed_qpscaling, nlp_opts->qpscaling, mem->relaxed_qpscaling_mem, mem->relaxed_qp_in); // ensures feasibility constraint Hessian is scaled + nlp_timings->time_qpscaling += acados_toc(&timer1); + /* Search Direction Computation */ search_direction_status = calculate_search_direction(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); if (search_direction_status != ACADOS_SUCCESS) @@ -1868,6 +1918,12 @@ int ocp_nlp_sqp_wfqp_precompute(void *config_, void *dims_, void *nlp_in_, void ocp_nlp_precompute_common(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work); + ocp_nlp_qpscaling_precompute(dims->relaxed_qpscaling, opts->nlp_opts->qpscaling, mem->relaxed_qpscaling_mem, mem->relaxed_qp_in, mem->relaxed_qp_out); + ocp_nlp_qpscaling_memory_get(dims->relaxed_qpscaling, mem->relaxed_qpscaling_mem, "scaled_qp_in", 0, &mem->relaxed_scaled_qp_in); + ocp_nlp_qpscaling_memory_get(dims->relaxed_qpscaling, mem->relaxed_qpscaling_mem, "scaled_qp_out", 0, &mem->relaxed_scaled_qp_out); + + set_relaxed_scaled_qp_in_matrix_pointers(mem); + // overwrite output pointers normally set in ocp_nlp_alias_memory_to_submodules for (int stage = 0; stage <= dims->N; stage++) { diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index 39a9efbdf2..7af41d1d2a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -133,6 +133,10 @@ typedef struct // qp in & out ocp_qp_in *relaxed_qp_in; ocp_qp_out *relaxed_qp_out; + void *relaxed_qpscaling_mem; + // only pointers + ocp_qp_in *relaxed_scaled_qp_in; + ocp_qp_out *relaxed_scaled_qp_out; } ocp_nlp_sqp_wfqp_memory; @@ -144,7 +148,7 @@ void *ocp_nlp_sqp_wfqp_memory_assign(void *config, void *dims, void *opts_, void void ocp_nlp_sqp_wfqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); // -void set_relaxed_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem, ocp_qp_in *qp_in); +void set_relaxed_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem); /************************************************ * workspace diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 0d25a74898..7557083370 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -879,19 +879,19 @@ void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out) for (ii = 0; ii <= N; ii++) { - // compute slacks for bounds + // compute t slacks for bounds blasfeo_dvecex_sp(nb[ii], 1.0, idxb[ii], ux + ii, 0, t+ii, nb[ii] + ng[ii]); blasfeo_daxpby(nb[ii], 1.0, t + ii, nb[ii] + ng[ii], -1.0, d + ii, 0, t + ii, 0); blasfeo_daxpby(nb[ii], -1.0, t + ii, nb[ii] + ng[ii], -1.0, d + ii, nb[ii] + ng[ii], t + ii, nb[ii] + ng[ii]); - // compute slacks for general constraints + // compute t slacks for general constraints blasfeo_dgemv_t(nu[ii] + nx[ii], ng[ii], 1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, nb[ii], t + ii, nb[ii]); blasfeo_dgemv_t(nu[ii] + nx[ii], ng[ii], -1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, 2 * nb[ii] + ng[ii], t + ii, 2 * nb[ii] + ng[ii]); - // compute slacks for soft constraints + // compute t slacks for bounds on slack variables constraints for(int jj=0; jjidxs_rev[ii][jj]; diff --git a/acados/utils/print.c b/acados/utils/print.c index 4c4a227f25..0f3a9c0f55 100644 --- a/acados/utils/print.c +++ b/acados/utils/print.c @@ -393,11 +393,12 @@ void print_ocp_qp_out(ocp_qp_out *qp_out) blasfeo_print_tran_dvec(nu[ii] + nx[ii] + 2 * ns[ii], &qp_out->ux[ii], 0); printf("pi =\n"); - for (ii = 0; ii < N; ii++) blasfeo_print_tran_dvec(nx[ii + 1], &qp_out->pi[ii], 0); + for (ii = 0; ii < N; ii++) + blasfeo_print_tran_dvec(nx[ii + 1], &qp_out->pi[ii], 0); printf("lam =\n"); for (ii = 0; ii <= N; ii++) - blasfeo_print_tran_dvec(2 * nb[ii] + 2 * ng[ii] + 2 * ns[ii], &qp_out->lam[ii], 0); + blasfeo_print_exp_tran_dvec(2 * nb[ii] + 2 * ng[ii] + 2 * ns[ii], &qp_out->lam[ii], 0); printf("t =\n"); for (ii = 0; ii <= N; ii++) diff --git a/acados/utils/print.h b/acados/utils/print.h index c90af1c9ac..3fb5d74c00 100644 --- a/acados/utils/print.h +++ b/acados/utils/print.h @@ -66,7 +66,6 @@ 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); diff --git a/acados/utils/types.h b/acados/utils/types.h index 3d6f2f3487..802ad93e43 100644 --- a/acados/utils/types.h +++ b/acados/utils/types.h @@ -81,6 +81,7 @@ enum return_values ACADOS_READY = 5, ACADOS_UNBOUNDED = 6, ACADOS_TIMEOUT = 7, + ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED = 8, }; @@ -113,6 +114,23 @@ enum search_direction_mode FEASIBILITY_QP = 2, }; + +/// QP scaling types +typedef enum +{ + NO_OBJECTIVE_SCALING, + OBJECTIVE_GERSHGORIN, +} qpscaling_scale_objective_type; + +/// QP scaling types +typedef enum +{ + NO_CONSTRAINT_SCALING, + INF_NORM, +} ocp_nlp_qpscaling_constraint_type; + + + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m b/examples/acados_matlab_octave/dense_nlp/convex_problem_globalization_necessary.m similarity index 100% rename from examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m rename to examples/acados_matlab_octave/dense_nlp/convex_problem_globalization_necessary.m diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh b/examples/acados_matlab_octave/dense_nlp/env.sh similarity index 100% rename from examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh rename to examples/acados_matlab_octave/dense_nlp/env.sh diff --git a/examples/acados_matlab_octave/dense_nlp/test_qpscaling.m b/examples/acados_matlab_octave/dense_nlp/test_qpscaling.m new file mode 100644 index 0000000000..6b09891e07 --- /dev/null +++ b/examples/acados_matlab_octave/dense_nlp/test_qpscaling.m @@ -0,0 +1,108 @@ +function test_qpscaling() + import casadi.* + + % solver name + solver_name = 'test_qpscaling'; + + % create solver + nlp_solver_type = 'SQP'; + use_qp_scaling = true; + soft_h = true; + ocp_solver = create_solver(solver_name, nlp_solver_type, use_qp_scaling, soft_h); + + % solve + ocp_solver.solve(); + + if use_qp_scaling + constraints_scaling = ocp_solver.get_qp_scaling_constraints(0); + objective_scaling = ocp_solver.get_qp_scaling_objective(); + qpscaling_status = ocp_solver.get('qpscaling_status'); + fprintf('constraints scaling: %f\n', constraints_scaling); + fprintf('objective scaling: %f\n', objective_scaling); + fprintf('QP scaling status: %d\n', qpscaling_status); + end + + % print solution + ocp_solver.print('stat'); +end + + +function ocp_solver = create_solver(solver_name, nlp_solver_type, use_qp_scaling, soft_h) + import casadi.* + + ACADOS_INFTY = get_acados_infty(); + ocp = AcadosOcp(); + + nx = 2; + % set model + ocp.model.name = ['dense_nlp_' solver_name]; + ocp.model.x = SX.sym('x', nx, 1); + + ny = nx; + + % discretization + N = 0; + + ocp.cost.W_e = 1e3*2*diag([1e3, 1e3]); + + ocp.cost.cost_type = 'LINEAR_LS'; + ocp.cost.cost_type_e = 'LINEAR_LS'; + + ocp.cost.Vx_e = eye(nx); + ocp.cost.yref_e = ones(ny, 1); + + % set constraints + xmax = 2.0; + ocp.constraints.lbx_e = -xmax * ones(nx,1); + ocp.constraints.ubx_e = xmax * ones(nx,1); + ocp.constraints.idxbx_e = 0:(nx-1); + + % define soft nonlinear constraint + scale_h = 1.0; + radius = 1.0; + ocp.model.con_h_expr_e = scale_h * (ocp.model.x(1)^2 + ocp.model.x(2)^2); + ocp.constraints.lh_e = -1000 * ones(1,1); + ocp.constraints.lh_e = -ACADOS_INFTY * ones(1,1); + ocp.constraints.uh_e = scale_h * radius^2 * ones(1,1); + + % soften + if soft_h + ocp.constraints.idxsh_e = 0; + ocp.cost.zl_e = [ocp.cost.zl_e; 1.0]; + ocp.cost.zu_e = [ocp.cost.zu_e; 1.0]; + ocp.cost.Zl_e = [ocp.cost.Zl_e; 1.0]; + ocp.cost.Zu_e = [ocp.cost.Zu_e; 1.0]; + end + + % set options + solver_options = ocp.solver_options; + solver_options.N_horizon = N; + + solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + qp_tol = 5e-9; + solver_options.qp_solver_tol_stat = qp_tol; + solver_options.qp_solver_tol_eq = qp_tol; + solver_options.qp_solver_tol_ineq = qp_tol; + solver_options.qp_solver_tol_comp = qp_tol; + solver_options.qp_solver_ric_alg = 1; + solver_options.qp_solver_mu0 = 1e4; + solver_options.qp_solver_iter_max = 400; + solver_options.hessian_approx = 'GAUSS_NEWTON'; + solver_options.nlp_solver_type = nlp_solver_type; + solver_options.globalization = 'FUNNEL_L1PEN_LINESEARCH'; + solver_options.globalization_full_step_dual = true; + % solver_options.print_level = 1; + % solver_options.nlp_solver_max_iter = 2; + solver_options.nlp_solver_ext_qp_res = 0; + + if use_qp_scaling + ocp.solver_options.qpscaling_scale_constraints = 'INF_NORM'; + ocp.solver_options.qpscaling_scale_objective = 'OBJECTIVE_GERSHGORIN'; + end + + % create ocp solver + ocp_solver = AcadosOcpSolver(ocp); +end + + + diff --git a/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m b/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m index 522ec8b200..36cd6c7698 100644 --- a/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m +++ b/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m @@ -38,6 +38,7 @@ '../p_global_example/main.m'; '../p_global_example/simulink_test_p_global.m'; '../mocp_transition_example/main_parametric_mocp.m'; + '../dense_nlp/test_qpscaling.m'; }; diff --git a/examples/acados_matlab_octave/test/test_all_examples.m b/examples/acados_matlab_octave/test/test_all_examples.m index 8148730470..6554806461 100644 --- a/examples/acados_matlab_octave/test/test_all_examples.m +++ b/examples/acados_matlab_octave/test/test_all_examples.m @@ -57,7 +57,7 @@ '../mocp_transition_example/main_multiphase_ocp.m'; '../legacy_interface/getting_started/extensive_example_ocp.m'; '../legacy_interface/simple_dae_model/example_ocp.m'; - '../convex_problem_globalization_needed/convex_problem_globalization_necessary.m'; + '../dense_nlp/convex_problem_globalization_necessary.m'; }; diff --git a/examples/acados_python/hock_schittkowsky/hs016_test.py b/examples/acados_python/hock_schittkowsky/hs016_test.py index e5311f4b3b..58e044fab4 100644 --- a/examples/acados_python/hock_schittkowsky/hs016_test.py +++ b/examples/acados_python/hock_schittkowsky/hs016_test.py @@ -35,7 +35,8 @@ from itertools import product -def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM'): +def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM', scale_qp_constraints: bool = False): + print(f"Solving with {qp_solver} and scale_qp_constraints={scale_qp_constraints}") # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -83,9 +84,13 @@ def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM'): ocp.solver_options.globalization_full_step_dual = True ocp.solver_options.globalization_funnel_use_merit_fun_only = False + # Scaling + if scale_qp_constraints: + ocp.solver_options.qpscaling_scale_objective = 'OBJECTIVE_GERSHGORIN' + ocp.solver_options.qpscaling_scale_constraints = 'INF_NORM' ocp.code_export_directory = f'c_generated_code_{model.name}' - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json', verbose=False) # initialize solver xinit = np.array([-2, 1]) @@ -105,9 +110,9 @@ def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM'): def main(): sol_list = [] for qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']: - print(f"Solving with {qp_solver}...") - sol = solve_problem(qp_solver) - sol_list.append(sol) + for scaling in [False, True]: + sol = solve_problem(qp_solver, scaling) + sol_list.append(sol) ref_sol = sol_list[0] for i, sol in enumerate(sol_list[1:]): diff --git a/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py b/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py new file mode 100644 index 0000000000..ed88f63006 --- /dev/null +++ b/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py @@ -0,0 +1,127 @@ +# +# 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.; +# + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, ACADOS_INFTY +import numpy as np +import casadi as ca + + +def solve_problem_with_constraint_scaling(scale_constraints): + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = AcadosModel() + x = ca.SX.sym('x', 4) + + # dynamics: identity + model.disc_dyn_expr = x + model.x = x + model.name = f'hs_074' + ocp.model = model + + a = 0.55 + + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = 3*x[0] + 1.0e-6*x[0]**3 + 2*x[1] + 2.0e-6*x[1]**3/3 + + # constraints + g = ca.SX.zeros(4, 1) + g[0] = x[3] - x[2] + g[1] = x[0] - 1000*ca.sin(-x[2] - 0.25) - 1000*ca.sin(-x[3] - 0.25) + g[2] = x[1] - 1000*ca.sin(x[2] - 0.25) - 1000*ca.sin(x[2]-x[3] - 0.25) + g[3] = 1000*ca.sin(x[3] - 0.25) + 1000*ca.sin(x[3] - x[2] - 0.25) + + ocp.model.con_h_expr_e = g + ocp.constraints.lh_e = np.array([-a, 894.8, 894.8, -1294.8]) + ocp.constraints.uh_e = np.array([a, 894.8, 894.8, -1294.8]) + + # add bounds on x; + ocp.constraints.idxbx_e = np.arange(4) + ocp.constraints.ubx_e = np.array([1200, 1200, a, a]) + ocp.constraints.lbx_e = np.array([0.0, 0.0, -a, -a]) + + # set options + ocp.solver_options.N_horizon = 0 + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.qp_solver_mu0 = 1e3 + ocp.solver_options.hessian_approx = 'EXACT' + ocp.solver_options.regularize_method = 'MIRROR' + ocp.solver_options.print_level = 1 + ocp.solver_options.nlp_solver_max_iter = 1000 + ocp.solver_options.qp_solver_iter_max = 1000 + ocp.solver_options.nlp_solver_type = 'SQP_WITH_FEASIBLE_QP' + + # Globalization + ocp.solver_options.globalization = 'FUNNEL_L1PEN_LINESEARCH' + ocp.solver_options.globalization_full_step_dual = True + ocp.solver_options.globalization_funnel_use_merit_fun_only = False + + # Scaling + ocp.solver_options.qpscaling_scale_objective = 'OBJECTIVE_GERSHGORIN' + if scale_constraints: + ocp.solver_options.qpscaling_scale_constraints = 'INF_NORM' + + ocp.code_export_directory = f'c_generated_code_{model.name}' + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json', verbose = False) + + # initialize solver + xinit = np.zeros(4) + ocp_solver.set(0, "x", xinit) + + # solve + status = ocp_solver.solve() + + # checks + obj_scale = ocp_solver.get_qp_scaling_objective() + print(f"Objective scaling: {obj_scale:.4e}") + if scale_constraints: + constr_scale = ocp_solver.get_qp_scaling_constraints(stage=0) + print(f"Constraints scaling factors: {constr_scale}") + + if scale_constraints: + assert status == 0, "Scaling of the constraints was not succesful!" + else: + assert status == 4, "Problem should not be solvable without scaling!" + del ocp_solver + +def main(): + # run test cases + print("\nTest standard unscaled version, HPIPM should fail:") + solve_problem_with_constraint_scaling(scale_constraints=False) + print("\n\n----------------------------------------------") + print("\nTest constraint scaling version, HPIPM should fail:") + solve_problem_with_constraint_scaling(scale_constraints=True) + print("\n\n----------------------------------------------") + +if __name__ == '__main__': + main() diff --git a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py index 25b7363e2a..396b8cef81 100644 --- a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py +++ b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py @@ -154,10 +154,16 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): ocp.constraints.idxsh_e = np.array([0]) Zh = 1e6 * np.ones(1) zh = 1e4 * np.ones(1) - ocp.cost.zl = zh - ocp.cost.zu = zh - ocp.cost.Zl = Zh - ocp.cost.Zu = Zh + # initial: no obstacle constraint, no addtional slack + ocp.cost.zl_0 = ocp.cost.zl + ocp.cost.zu_0 = ocp.cost.zu + ocp.cost.Zl_0 = ocp.cost.Zl + ocp.cost.Zu_0 = ocp.cost.Zu + # path & terminal: slacked obstacle constraint + ocp.cost.zl = np.concatenate((ocp.cost.zl, zh)) + ocp.cost.zu = np.concatenate((ocp.cost.zu, zh)) + ocp.cost.Zl = np.concatenate((ocp.cost.Zl, Zh)) + ocp.cost.Zu = np.concatenate((ocp.cost.Zu, Zh)) ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh)) ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh)) ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh)) @@ -257,7 +263,6 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): plot_linear_mass_system_U(shooting_nodes, simU) # plot_linear_mass_system_X(shooting_nodes, simX) - # import pdb; pdb.set_trace() print(f"\n\n----------------------\n") if __name__ == '__main__': diff --git a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py index 7c60b3c45d..e0ca527d66 100644 --- a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py +++ b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py @@ -35,22 +35,6 @@ # an OCP to test the behavior of the SQP_WITH_FEASIBLE_QP functionalities -def main_test(): - - # SETTINGS: - soften_controls = True - soften_obstacle = False - soften_terminal = True - plot = True - ocp, ocp_solver1 = create_solver("1", soften_obstacle, soften_terminal, soften_controls) - standard_test(ocp, ocp_solver1, soften_obstacle, soften_terminal, soften_controls, plot) - - soften_controls = False - soften_obstacle = False - soften_terminal = False - plot = True - ocp, ocp_solver2 = create_solver("2", soften_obstacle, soften_terminal, soften_controls) - standard_test(ocp, ocp_solver2, soften_obstacle, soften_terminal, soften_controls, plot) def feasible_qp_dims_test(soften_obstacle, soften_terminal, soften_controls, N, ocp_solver: AcadosOcpSolver): """ @@ -125,10 +109,7 @@ def create_solver_opts(N=4, Tf=2, nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', allo solver_options.N_horizon = N solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' qp_tol = 5e-7 - solver_options.qp_solver_tol_stat = qp_tol - solver_options.qp_solver_tol_eq = qp_tol - solver_options.qp_solver_tol_ineq = qp_tol - solver_options.qp_solver_tol_comp = qp_tol + solver_options.qp_tol = qp_tol solver_options.qp_solver_ric_alg = 1 solver_options.qp_solver_mu0 = 1e4 solver_options.qp_solver_warm_start = 1 @@ -153,7 +134,7 @@ def create_solver_opts(N=4, Tf=2, nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', allo def create_solver(solver_name: str, soften_obstacle: bool, soften_terminal: bool, soften_controls: bool, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP', - allow_switches: bool = True): + allow_switching_modes: bool = True): # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -238,17 +219,24 @@ def create_solver(solver_name: str, soften_obstacle: bool, soften_terminal: bool ocp.constraints.idxsh_e = np.array([0]) Zh = 1e6 * np.ones(1) zh = 1e4 * np.ones(1) - ocp.cost.zl = zh - ocp.cost.zu = zh - ocp.cost.Zl = Zh - ocp.cost.Zu = Zh + # initial: no obstacle constraint, no addtional slack + ocp.cost.zl_0 = ocp.cost.zl + ocp.cost.zu_0 = ocp.cost.zu + ocp.cost.Zl_0 = ocp.cost.Zl + ocp.cost.Zu_0 = ocp.cost.Zu + # path & terminal: slacked obstacle constraint + ocp.cost.zl = np.concatenate((ocp.cost.zl, zh)) + ocp.cost.zu = np.concatenate((ocp.cost.zu, zh)) + ocp.cost.Zl = np.concatenate((ocp.cost.Zl, Zh)) + ocp.cost.Zu = np.concatenate((ocp.cost.Zu, Zh)) ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh)) ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh)) ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh)) ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh)) # load options - ocp.solver_options = create_solver_opts(N, Tf, nlp_solver_type, allow_switches) + ocp.solver_options = create_solver_opts(N, Tf, nlp_solver_type, allow_switching_modes) + # create ocp solver ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_{solver_name}_ocp.json', verbose=False) @@ -275,7 +263,6 @@ def standard_test(ocp: AcadosOcp, ocp_solver: AcadosOcpSolver, soften_obstacle: # get solution sol_X = np.array([ocp_solver.get(i,"x") for i in range(N+1)]) - # print summary print(f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations") print(f"solved sqp_wfqp problem with settings soften_obstacle = {soften_obstacle},soften_terminal = {soften_terminal}, SOFTEN_CONTROL = {soften_controls}") @@ -308,6 +295,14 @@ def test_same_behavior_sqp_and_sqp_wfqp(): res_solver2 = ocp_solver2.get_residuals() assert np.array_equal(res_solver1, res_solver2), "both solvers should have identical residual stats" + # check solutions + sol_1 = ocp_solver1.store_iterate_to_flat_obj() + sol_2 = ocp_solver2.store_iterate_to_flat_obj() + if sol_1.allclose(sol_2): + print("Both solvers have the same solution.") + else: + raise ValueError("Solutions of solvers differ!") + print(f"\n\n----------------------\n") def sqp_wfqp_test_same_matrices(): @@ -317,7 +312,7 @@ def sqp_wfqp_test_same_matrices(): soften_terminal = False # SQP solver - _, ocp_solver1 = create_solver("v1", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type='SQP_WITH_FEASIBLE_QP', allow_switches=False) + _, ocp_solver1 = create_solver("v1", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type='SQP_WITH_FEASIBLE_QP', allow_switching_modes=False) _ = ocp_solver1.solve() qp = ocp_solver1.get_last_qp() @@ -340,6 +335,24 @@ def sqp_wfqp_test_same_matrices(): print(f"\n\n----------------------\n") + +def main_test(): + # SETTINGS: + soften_controls = True + soften_obstacle = False + soften_terminal = True + plot = True + ocp, ocp_solver1 = create_solver("1", soften_obstacle, soften_terminal, soften_controls) + standard_test(ocp, ocp_solver1, soften_obstacle, soften_terminal, soften_controls, plot) + + soften_controls = False + soften_obstacle = False + soften_terminal = False + plot = True + ocp, ocp_solver2 = create_solver("2", soften_obstacle, soften_terminal, soften_controls) + standard_test(ocp, ocp_solver2, soften_obstacle, soften_terminal, soften_controls, plot) + + if __name__ == '__main__': main_test() test_same_behavior_sqp_and_sqp_wfqp() diff --git a/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py new file mode 100644 index 0000000000..c271ee843b --- /dev/null +++ b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py @@ -0,0 +1,285 @@ +# +# 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.; +# + +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY, AcadosOcpIterate +import numpy as np +import scipy.linalg +from linear_mass_model import export_linear_mass_model + + +def create_solver_opts(N=4, Tf=2, nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', allow_switching_modes=True, globalization= 'FUNNEL_L1PEN_LINESEARCH'): + + solver_options = AcadosOcp().solver_options + + # set options + solver_options.N_horizon = N + solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + solver_options.qp_tol = 1e-9 + solver_options.qp_solver_ric_alg = 1 + solver_options.qp_solver_mu0 = 1e4 + solver_options.qp_solver_warm_start = 1 + solver_options.qp_solver_iter_max = 400 + solver_options.hessian_approx = 'GAUSS_NEWTON' + solver_options.integrator_type = 'ERK' + solver_options.nlp_solver_type = nlp_solver_type + solver_options.globalization = globalization + solver_options.globalization_full_step_dual = True + solver_options.print_level = 1 + solver_options.nlp_solver_max_iter = 30 + solver_options.use_constraint_hessian_in_feas_qp = False + solver_options.nlp_solver_ext_qp_res = 0 + + if not allow_switching_modes: + solver_options.search_direction_mode = 'BYRD_OMOJOKUN' + solver_options.allow_direction_mode_switch_to_nominal = False + + # set prediction horizon + solver_options.tf = Tf + + return solver_options + +def create_solver(solver_name: str, soften_obstacle: bool, soften_terminal: bool, + soften_controls: bool, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP', + globalization= 'FUNNEL_L1PEN_LINESEARCH', + allow_switching_modes: bool = True, + use_qp_scaling: bool = False): + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_linear_mass_model() + ocp.model = model + ocp.model.name += solver_name + + nx = model.x.rows() + nu = model.u.rows() + ny = nu + + # discretization + Tf = 2 + N = 4 + + # set cost + Q = 2*np.diag([]) + R = 2*np.diag([1e1, 1e1]) + + ocp.cost.W_e = Q + ocp.cost.W = scipy.linalg.block_diag(Q, R) + + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + ocp.cost.Vx = np.zeros((ny, nx)) + + Vu = np.eye((nu)) + ocp.cost.Vu = Vu + ocp.cost.yref = np.zeros((ny, )) + + # set constraints + Fmax = 2 + ocp.constraints.lbu = -Fmax * np.ones((nu,)) + ocp.constraints.ubu = +Fmax * np.ones((nu,)) + ocp.constraints.idxbu = np.array(range(nu)) + + # Slack the controls + if soften_controls: + ocp.constraints.idxsbu = np.array(range(nu)) + + x0 = np.array([1e-1, 1.1, 0, 0]) + ocp.constraints.x0 = x0 + + # terminal constraint + x_goal = np.array([0, -1.1, 0, 0]) + ocp.constraints.idxbx_e = np.array(range(nx)) + ocp.constraints.lbx_e = x_goal + ocp.constraints.ubx_e = x_goal + + if soften_terminal: + ocp.constraints.idxsbx_e = np.array(range(nx)) + ocp.cost.zl_e = 42 * 1e3 * np.ones(nx) + ocp.cost.zu_e = 42 * 1e3 * np.ones(nx) + ocp.cost.Zl_e = 0 * np.ones(nx) + ocp.cost.Zu_e = 0 * np.ones(nx) + + # add cost for slacks + if soften_controls: + ocp.cost.zl = 1e1 * np.ones(nu) + ocp.cost.zu = 1e1 * np.ones(nu) + ocp.cost.Zl = 1e1 * np.ones(nu) + ocp.cost.Zu = 1e1 * np.ones(nu) + + # add obstacle + obs_rad = 1.0 + ocp.constraints.lh = -np.array([ACADOS_INFTY]) + ocp.constraints.uh = -np.array([obs_rad**2]) + x_square = model.x[0] ** 2 + model.x[1] ** 2 + ocp.model.con_h_expr = -x_square + # copy for terminal + ocp.constraints.uh_e = ocp.constraints.uh + ocp.constraints.lh_e = ocp.constraints.lh + ocp.model.con_h_expr_e = ocp.model.con_h_expr + + # # soften + if soften_obstacle: + ocp.constraints.idxsh = np.array([0]) + ocp.constraints.idxsh_e = np.array([0]) + Zh = 1e6 * np.ones(1) + zh = 1e4 * np.ones(1) + # initial: no obstacle constraint, no addtional slack + ocp.cost.zl_0 = ocp.cost.zl + ocp.cost.zu_0 = ocp.cost.zu + ocp.cost.Zl_0 = ocp.cost.Zl + ocp.cost.Zu_0 = ocp.cost.Zu + # path & terminal: slacked obstacle constraint + ocp.cost.zl = np.concatenate((ocp.cost.zl, zh)) + ocp.cost.zu = np.concatenate((ocp.cost.zu, zh)) + ocp.cost.Zl = np.concatenate((ocp.cost.Zl, Zh)) + ocp.cost.Zu = np.concatenate((ocp.cost.Zu, Zh)) + ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh)) + ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh)) + ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh)) + ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh)) + + # load options + ocp.solver_options = create_solver_opts(N, Tf, nlp_solver_type, allow_switching_modes, globalization) + if use_qp_scaling: + ocp.solver_options.qpscaling_scale_constraints = "INF_NORM" + ocp.solver_options.qpscaling_scale_objective = "OBJECTIVE_GERSHGORIN" + + # create ocp solver + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_{solver_name}_ocp.json', verbose=False) + + # # initialize + for i in range(N+1): + ocp_solver.set(i, "x", (N+1-i)/(N+1) * x0 + i/(N+1) * x_goal) + + return ocp, ocp_solver + + + +def check_qp_scaling(ocp_solver: AcadosOcpSolver): + if ocp_solver.acados_ocp.solver_options.qpscaling_scale_constraints == "NO_CONSTRAINT_SCALING": + try: + constraint_scaling = ocp_solver.get_qp_scaling_constraints(0) + except Exception as e: + print(f"constraint scaling not done as expected.") + return + + for i in range(ocp_solver.N+1): + constraint_scaling = ocp_solver.get_qp_scaling_constraints(i) + print(f"Constraint scaling at stage {i}: {constraint_scaling}") + + if ocp_solver.acados_ocp.solver_options.qpscaling_scale_objective != "NO_OBJECTIVE_SCALING": + objective_scaling = ocp_solver.get_qp_scaling_objective() + print(f"Objective scaling: {objective_scaling}") + + +def call_solver(ocp: AcadosOcp, ocp_solver: AcadosOcpSolver, soften_obstacle: bool, + soften_terminal: bool, soften_controls: bool, plot: bool) -> AcadosOcpIterate: + # solve + status = ocp_solver.solve() + ocp_solver.print_statistics() + + sqp_iter = ocp_solver.get_stats('sqp_iter') + if status != 0: + raise RuntimeError(f"acados returned status {status} after {sqp_iter} SQP iterations.") + # print(f'acados returned status {status}.') + + # print summary + print(f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations") + print(f"solved sqp_wfqp problem with settings soften_obstacle = {soften_obstacle},soften_terminal = {soften_terminal}, SOFTEN_CONTROL = {soften_controls}") + +def check_residual_solutions(stat1: np.ndarray, stat2: np.ndarray): + n_rows1 = len(stat1[0]) + n_rows2 = len(stat2[0]) + + assert n_rows1 == n_rows2, f"Both solvers should take the same number of iterations!, got {n_rows1} for solver 1, and {n_rows2} for solver 2" + + for jj in range(n_rows1): + # res_stat + assert np.allclose(stat1[1][jj], stat2[1][jj]), f"res_stat differs in iter {jj}" + # res_eq + assert np.allclose(stat1[2][jj], stat2[2][jj]), f"res_eq differs in iter {jj}" + # res_ineq + assert np.allclose(stat1[3][jj], stat2[3][jj]), f"res_ineq differs in iter {jj}" + # res_comp + assert np.allclose(stat1[4][jj], stat2[4][jj]), f"res_comp differs in iter {jj}" + +def test_qp_scaling(nlp_solver_type = 'SQP', globalization = 'FUNNEL_L1PEN_LINESEARCH'): + print(f"\n\nTesting solver={nlp_solver_type} with globalization={globalization}") + # SETTINGS: + soften_controls = True + soften_obstacle = True + soften_terminal = True + + # reference + ocp_1, ocp_solver_1 = create_solver("1", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type=nlp_solver_type, globalization=globalization, allow_switching_modes=False, use_qp_scaling=False) + sol_1 = call_solver(ocp_1, ocp_solver_1, soften_obstacle, soften_terminal, soften_controls, plot=False) + check_qp_scaling(ocp_solver_1) + sol_1 = ocp_solver_1.store_iterate_to_obj() + stats_1 = ocp_solver_1.get_stats("statistics") + + # test QP scaling + ocp_2, ocp_solver_2 = create_solver("2", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type=nlp_solver_type, allow_switching_modes=False, use_qp_scaling=True) + sol_2 = call_solver(ocp_2, ocp_solver_2, soften_obstacle, soften_terminal, soften_controls, plot=False) + check_qp_scaling(ocp_solver_2) + sol_2 = ocp_solver_2.store_iterate_to_obj() + ocp_solver_2.get_from_qp_in(1, "idxs_rev") + stats_2 = ocp_solver_2.get_stats("statistics") + + check_residual_solutions(stats_1, stats_2) + + # check solutions + for field in ["x_traj", "u_traj", "sl_traj", "su_traj", "lam_traj", "pi_traj"]: + v1 = getattr(sol_1, field) + v2 = getattr(sol_2, field) + for i in range(len(v1)): + if not np.allclose(v1[i], v2[i], atol=1e-6): + print(f"Field {field} differs at index {i}: max diff = {np.max(np.abs(v1[i] - v2[i]))}") + print(f"got difference {v1[i] - v2[i]}") + else: + pass + # print(f"Field {field} is the same at index {i}.") + + # equivalent check + if sol_1.allclose(sol_2, atol=1e-6): + print("Both solvers have the same solution.") + else: + raise ValueError("Solutions of solvers differ!") + + print("\n\n---------------------------------------------------------") + +if __name__ == '__main__': + test_qp_scaling(nlp_solver_type = 'SQP', globalization = 'FUNNEL_L1PEN_LINESEARCH') + test_qp_scaling(nlp_solver_type = 'SQP', globalization= 'MERIT_BACKTRACKING') + test_qp_scaling(nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', globalization = 'FUNNEL_L1PEN_LINESEARCH') + test_qp_scaling(nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', globalization= 'MERIT_BACKTRACKING') + diff --git a/examples/acados_python/non_ocp_nlp/qpscaling_test.py b/examples/acados_python/non_ocp_nlp/qpscaling_test.py new file mode 100644 index 0000000000..577bb7f542 --- /dev/null +++ b/examples/acados_python/non_ocp_nlp/qpscaling_test.py @@ -0,0 +1,254 @@ +# +# 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.; +# + +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY, AcadosOcpFlattenedIterate +import numpy as np +import casadi as ca + + +def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP', + allow_switching_modes: bool = True, + use_qp_scaling: bool = False, + soft_h: bool = True): + + ocp = AcadosOcp() + + nx = 2 + # set model + ocp.model.name = f"dense_nlp_{solver_name}" + ocp.model.x = ca.SX.sym('x', nx, 1) + + ny = nx + + # discretization + N = 0 + + ocp.cost.W_e = 2*np.diag([1e3, 1e3]) + + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + ocp.cost.Vx_e = np.eye((nx)) + ocp.cost.yref_e = np.ones((ny, )) + + # set constraints + xmax = 2.0 + ocp.constraints.lbx_e = -xmax * np.ones((nx,)) + ocp.constraints.ubx_e = +xmax * np.ones((nx,)) + ocp.constraints.idxbx_e = np.arange(nx) + # soften the bounds + # ocp.constraints.idxsbx_e = np.array([0, 1]) + # ocp.cost.zl_e = np.array([1.0, 1.0]) + # ocp.cost.zu_e = np.array([1e0, 1e0]) + # ocp.cost.Zl_e = np.array([1.0, 1.0]) + # ocp.cost.Zu_e = np.array([1e0, 1e0]) + + # define soft nonlinear constraint + scale_h = 1.0 + radius = 1.0 + ocp.model.con_h_expr_e = scale_h * (ocp.model.x[0]**2 + ocp.model.x[1]**2) + ocp.constraints.lh_e = -1000 * np.ones((1,)) + ocp.constraints.lh_e = -ACADOS_INFTY * np.ones((1,)) + ocp.constraints.uh_e = scale_h * radius**2 * np.ones((1,)) + + # soften + if soft_h: + ocp.constraints.idxsh_e = np.array([0]) + ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, np.array([1.0]))) + ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, np.array([1e0]))) + ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, np.array([1.0]))) + ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, np.array([1e0]))) + + # set options + solver_options = ocp.solver_options + solver_options.N_horizon = N + + solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + qp_tol = 5e-9 + solver_options.qp_tol = qp_tol + solver_options.qp_solver_ric_alg = 1 + solver_options.qp_solver_mu0 = 1e4 + solver_options.qp_solver_iter_max = 400 + solver_options.hessian_approx = 'GAUSS_NEWTON' + solver_options.nlp_solver_type = nlp_solver_type + solver_options.globalization = 'FUNNEL_L1PEN_LINESEARCH' + solver_options.globalization_full_step_dual = True + solver_options.print_level = 1 + solver_options.nlp_solver_max_iter = 6 + solver_options.use_constraint_hessian_in_feas_qp = False + solver_options.nlp_solver_ext_qp_res = 0 + + if not allow_switching_modes: + solver_options.search_direction_mode = 'BYRD_OMOJOKUN' + solver_options.allow_direction_mode_switch_to_nominal = False + + if use_qp_scaling: + ocp.solver_options.qpscaling_scale_constraints = "INF_NORM" + ocp.solver_options.qpscaling_scale_objective = "OBJECTIVE_GERSHGORIN" + + # create ocp solver + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + return ocp, ocp_solver + +def check_qp_scaling(ocp_solver: AcadosOcpSolver): + qpscaling_status = ocp_solver.get_stats("qpscaling_status") + if qpscaling_status == 0: + print("QP scaling reported no issues.") + else: + print(f"QP scaling reported issues with status {qpscaling_status}.") + + if ocp_solver.acados_ocp.solver_options.qpscaling_scale_constraints == "NO_CONSTRAINT_SCALING": + try: + constraint_scaling = ocp_solver.get_qp_scaling_constraints(0) + except Exception as e: + print(f"constraint scaling not done as expected.") + return + + for i in range(ocp_solver.N+1): + constraint_scaling = ocp_solver.get_qp_scaling_constraints(i) + print(f"Constraint scaling at stage {i}: {constraint_scaling}") + # if not np.all(constraint_scaling != 1.0): + # raise ValueError(f"Constraint scaling should have non-unit to actually test the functionality") + objective_scaling = ocp_solver.get_qp_scaling_objective() + print(f"Objective scaling: {objective_scaling}") + + +def call_solver(ocp_solver: AcadosOcpSolver) -> AcadosOcpFlattenedIterate: + # solve + status = ocp_solver.solve() + ocp_solver.print_statistics() + + sqp_iter = ocp_solver.get_stats('sqp_iter') + if status != 0: + # raise RuntimeError(f"acados returned status {status} after {sqp_iter} SQP iterations.") + print(f'acados returned status {status}.') + + print(f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations") + sol = ocp_solver.store_iterate_to_flat_obj() + return sol + +def check_solutions(sol_1: AcadosOcpFlattenedIterate, sol_2: AcadosOcpFlattenedIterate, soft_h: bool): + # check solutions + for field in ["x", "u", "sl", "su", "lam", "pi"]: + v1 = getattr(sol_1, field) + v2 = getattr(sol_2, field) + if not np.allclose(v1, v2, atol=1e-6): + print(f"Field {field} differs: max diff = {np.max(np.abs(v1 - v2))}") + print(f"got difference {v1 - v2}") + else: + print(f"Solutions match in field {field}.") + pass + print(f"{sol_1}") + + if soft_h: + if np.any(sol_1.su > 1e-1): + print("checked with active soft constraints.") + else: + raise ValueError("Soft constraints should be active, but are not.") + + if sol_1.allclose(sol_2): + print("Both solvers have the same solution.") + else: + raise ValueError("Solutions of solvers differ!") + +def check_residual_solutions(stat1: np.ndarray, stat2: np.ndarray): + n_rows1 = len(stat1[0]) + n_rows2 = len(stat2[0]) + + assert n_rows1 == n_rows2, f"Both solvers should take the same number of iterations!, got {n_rows1} for solver 1, and {n_rows2} for solver 2" + + for jj in range(n_rows1): + # res_stat + assert np.allclose(stat1[1][jj], stat2[1][jj]), f"res_stat differs in iter {jj}" + # res_eq + assert np.allclose(stat1[2][jj], stat2[2][jj]), f"res_eq differs in iter {jj}" + # res_ineq + assert np.allclose(stat1[3][jj], stat2[3][jj]), f"res_ineq differs in iter {jj}" + # res_comp + assert np.allclose(stat1[4][jj], stat2[4][jj]), f"res_comp differs in iter {jj}" + +def test_qp_scaling(soft_h: bool = True): + nlp_solver_type = "SQP" + # nlp_solver_type = "SQP_WITH_FEASIBLE_QP" + + # test without QP scaling + print("Reference ...") + _, ocp_solver_2 = create_solver("2", nlp_solver_type=nlp_solver_type, allow_switching_modes=True, use_qp_scaling=False, soft_h=soft_h) + sol_2 = call_solver(ocp_solver_2) + stats2 = ocp_solver_2.get_stats("statistics") + + check_qp_scaling(ocp_solver_2) + print(f"Reference solution: {sol_2}") + + # test QP scaling + print("Testing QP scaling with SQP solver...") + _, ocp_solver_1 = create_solver("1", nlp_solver_type=nlp_solver_type, allow_switching_modes=True, use_qp_scaling=True, soft_h=soft_h) + sol_1 = call_solver(ocp_solver_1) + check_qp_scaling(ocp_solver_1) + stats1 = ocp_solver_1.get_stats("statistics") + + check_residual_solutions(stats1, stats2) + check_solutions(sol_1, sol_2, soft_h) + +def test_sanity_check(soft_h: bool = True, use_qp_scaling: bool = True): + print("Sanity Check SQP and SQP_WITH_FEASIBLE_QP solver...") + + # test without QP scaling + print("Solving with SQP") + _, ocp_solver_2 = create_solver("2", nlp_solver_type="SQP", allow_switching_modes=True, use_qp_scaling=use_qp_scaling, soft_h=soft_h) + sol_2 = call_solver(ocp_solver_2) + check_qp_scaling(ocp_solver_2) + stats2 = ocp_solver_2.get_stats("statistics") + print(f"Reference solution: {sol_2}") + + # test QP scaling + print("Solving with SQP_WITH_FEASIBLE_QP") + _, ocp_solver_1 = create_solver("1", nlp_solver_type="SQP_WITH_FEASIBLE_QP", allow_switching_modes=True, use_qp_scaling=use_qp_scaling, soft_h=soft_h) + sol_1 = call_solver(ocp_solver_1) + stats1 = ocp_solver_1.get_stats("statistics") + check_qp_scaling(ocp_solver_1) + + check_residual_solutions(stats1, stats2) + check_solutions(sol_1, sol_2, soft_h) + print("\n") + + +if __name__ == '__main__': + # Sanity Checks + test_sanity_check(soft_h=False, use_qp_scaling=False) # overall solver sanity check + test_sanity_check(soft_h=True, use_qp_scaling=False) # overall solver sanity check + test_sanity_check(soft_h=False, use_qp_scaling=True) + test_sanity_check(soft_h=True, use_qp_scaling=True) + + test_qp_scaling(soft_h=False) + test_qp_scaling(soft_h=True) + + diff --git a/examples/acados_python/pendulum_on_cart/common/pendulum_model.py b/examples/acados_python/pendulum_on_cart/common/pendulum_model.py index 1331cc3c33..0ec234355a 100644 --- a/examples/acados_python/pendulum_on_cart/common/pendulum_model.py +++ b/examples/acados_python/pendulum_on_cart/common/pendulum_model.py @@ -165,3 +165,57 @@ def export_augmented_pendulum_model(): return model +def export_free_time_pendulum_ode_model() -> AcadosModel: + + model_name = 'free_time_pendulum' + + m_cart = 1.0 # mass of the cart [kg] + m = 0.1 # mass of the ball [kg] + g = 9.81 # gravity constant [m/s^2] + l = 0.8 # length of the rod [m] + + # set up states & controls + T = SX.sym('T') + x1 = SX.sym('x1') + theta = SX.sym('theta') + v1 = SX.sym('v1') + dtheta = SX.sym('dtheta') + + x = vertcat(T, x1, theta, v1, dtheta) + + F = SX.sym('F') + u = vertcat(F) + + # xdot + T_dot = SX.sym('T_dot') + x1_dot = SX.sym('x1_dot') + theta_dot = SX.sym('theta_dot') + v1_dot = SX.sym('v1_dot') + dtheta_dot = SX.sym('dtheta_dot') + + xdot = vertcat(T_dot, x1_dot, theta_dot, v1_dot, dtheta_dot) + + # dynamics + cos_theta = cos(theta) + sin_theta = sin(theta) + denominator = m_cart + m - m*cos_theta*cos_theta + f_expl = vertcat(0, + T*v1, + T*dtheta, + T*((-m*l*sin_theta*dtheta*dtheta + m*g*cos_theta*sin_theta+F)/denominator), + T*((-m*l*cos_theta*sin_theta*dtheta*dtheta + F*cos_theta+(m_cart+m)*g*sin_theta)/(l*denominator)) + ) + + f_impl = xdot - f_expl + + model = AcadosModel() + + model.f_impl_expr = f_impl + model.f_expl_expr = f_expl + model.x = x + model.xdot = xdot + model.u = u + model.name = model_name + + return model + diff --git a/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py b/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py new file mode 100644 index 0000000000..39832615d0 --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py @@ -0,0 +1,159 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +import sys +sys.path.insert(0, '../common') + +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY, AcadosOcpOptions +from pendulum_model import export_free_time_pendulum_ode_model +import numpy as np +from utils import plot_pendulum +import casadi as ca +from casadi.tools import entry, struct_symSX + +def formulate_ocp(opts: AcadosOcpOptions) -> AcadosOcp: + # create ocp object to formulate the OCP + N = 100 + nx = 5 + nu = 1 + Tf = 1.0 + + # Parameters + max_f = 5. + max_x1 = 1.0 + max_v = 2.0 + + x1_0 = 0.0 + theta_0 = np.pi + dx1_0 = 0.0 + dtheta_0 = 0.0 + + theta_f = 0.0 + dx1_f = 0.0 + dtheta_f = 0.0 + + ocp = AcadosOcp() + + model = export_free_time_pendulum_ode_model() + + if opts.qpscaling_scale_objective: + model.name += "scaled_objective" + else: + model.name += "no_scaling" + + # set model + ocp.model = model + + # Initial conditions + ocp.constraints.lbx_0 = np.array([0.0, x1_0, theta_0, dx1_0, dtheta_0]) + ocp.constraints.ubx_0 = np.array([ACADOS_INFTY, x1_0, theta_0, dx1_0, dtheta_0]) + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3, 4]) + + # Actuator constraints + ocp.constraints.lbu = np.array([-max_f]) + ocp.constraints.ubu = np.array([+max_f]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.lbx = np.array([0.0, -max_x1, -max_v]) + ocp.constraints.ubx = np.array([ACADOS_INFTY, max_x1, max_v]) + ocp.constraints.idxbx = np.array([0, 1, 3]) + + # Terminal constraints + ocp.constraints.lbx_e = np.array([0.0, theta_f, dx1_f, dtheta_f]) + ocp.constraints.ubx_e = np.array([ACADOS_INFTY, theta_f, dx1_f, dtheta_f]) + ocp.constraints.idxbx_e = np.array([0, 2, 3, 4]) + + # Define objective function + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = model.x[0] + + # set solver options + ocp.solver_options = opts + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + return ocp + + +def main(scale_objective: bool): + + opts = AcadosOcpOptions() + opts.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + opts.qp_solver_mu0 = 1e3 + opts.hessian_approx = 'EXACT' + opts.regularize_method = 'MIRROR' + opts.integrator_type = 'ERK' + opts.print_level = 1 + opts.nlp_solver_max_iter = 1000 + opts.qp_solver_iter_max = 1000 + + opts.nlp_solver_type = 'SQP_WITH_FEASIBLE_QP'#'SQP'# + + # Globalization + opts.globalization = 'FUNNEL_L1PEN_LINESEARCH'#'MERIT_BACKTRACKING'# + opts.globalization_full_step_dual = True + opts.globalization_funnel_use_merit_fun_only = False + + # Scaling + if scale_objective: + opts.qpscaling_scale_objective = "OBJECTIVE_GERSHGORIN" + else: + opts.qpscaling_scale_objective = "NO_OBJECTIVE_SCALING" + + ocp = formulate_ocp(opts) + + N = ocp.solver_options.N_horizon + + # create solver + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + # intialize + T0 = 1.0 + for i in range(N): + ocp_solver.set(i, "x", np.array([T0, 0.0, np.pi, 0.0, 0.0])) + ocp_solver.set(i, "u", np.array([0.0])) + ocp_solver.set(N, "x", np.array([T0, 0.0, np.pi, 0.0, 0.0])) + + # solve + status = ocp_solver.solve() + + if scale_objective: + if status != 0: + raise ValueError("Objective scaling should make the problem solvable!") + print(f"Objective scaling worked, status: {status}.") + else: + if status == 0: + raise ValueError("In Byrd-Omojokun equations, the multipliers should get too large which makes HPIPM fail!") + print("Problem not solvable without objective scaling, as expected.") + +if __name__ == "__main__": + main(scale_objective=False) + main(scale_objective=True) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index abfa4ed61b..820690c6b2 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -236,11 +236,14 @@ add_test(NAME py_sqp_wfqp_problem_hs015 COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky python hs015_test.py) - add_test(NAME py_sqp_wfqp_problem_hs016 COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky python hs016_test.py) +add_test(NAME py_sqp_wfqp_problem_hs074_constraint_scaling + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky + python hs074_constraint_scaling.py) + # CMake test add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp @@ -276,6 +279,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp python adaptive_eps_reg_test.py) + add_test(NAME py_qp_scaling_non_ocp + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp + python qpscaling_test.py) + # Convex test problem where full step SQP does not converge, but globalized SQP does add_test(NAME python_convex_test_problem_globalization COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_problem_globalization_needed @@ -290,6 +297,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/linear_mass_model python linear_mass_test_problem.py) + add_test(NAME py_qpscaling_slacked + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/linear_mass_model + python test_qpscaling_slacked.py) + # Armijo test problem add_test(NAME python_armijo_test COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests @@ -390,6 +401,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=GNSF) + add_test(NAME py_qp_scaling_time_opt_swingup + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp + python time_optimal_swing_up.py) + # CMake and solver=DISCRETE test add_test(NAME python_example_ocp_dynamics_formulations_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp @@ -439,6 +454,7 @@ add_test(NAME python_pendulum_ocp_example_cmake # Directory non_ocp_nlp set_tests_properties(python_maratos_test_problem_globalization PROPERTIES DEPENDS python_test_adaptive_reg) + set_tests_properties(python_test_adaptive_reg PROPERTIES DEPENDS py_qp_scaling_non_ocp) # Directory acados_python/tests set_tests_properties(python_test_cython_vs_ctypes PROPERTIES DEPENDS python_test_reset) @@ -478,6 +494,9 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_time_varying_irk PROPERTIES DEPENDS test_polynomial_controls_and_penalties) set_tests_properties(test_polynomial_controls_and_penalties PROPERTIES DEPENDS py_mocp_qp_test) + # Directory linear_mass_model + set_tests_properties(python_OCP_maratos_test_problem_globalization PROPERTIES DEPENDS py_qpscaling_slacked) + # Directory pendulum_on_cart/ocp set_tests_properties(python_pendulum_ocp_example_reuse_code PROPERTIES DEPENDS python_nonuniform_discretization_ocp_example) set_tests_properties(python_nonuniform_discretization_ocp_example PROPERTIES DEPENDS python_rti_loop_ocp_example) @@ -488,6 +507,7 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_pendulum_ocp_IRK PROPERTIES DEPENDS python_pendulum_ocp_ERK) set_tests_properties(python_pendulum_ocp_ERK PROPERTIES DEPENDS python_pendulum_ocp_GNSF) set_tests_properties(python_pendulum_ocp_GNSF PROPERTIES DEPENDS python_example_ocp_dynamics_formulations_cmake) + set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS py_qp_scaling_time_opt_swingup) if(ACADOS_WITH_QPDUNES) set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS python_qpDUNES_test) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 351112ef53..49a4c871c0 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -53,6 +53,7 @@ #include "acados/ocp_nlp/ocp_nlp_reg_project.h" #include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h" #include "acados/ocp_nlp/ocp_nlp_reg_noreg.h" +#include "acados/ocp_nlp/ocp_nlp_qpscaling.h" #include "acados/ocp_nlp/ocp_nlp_globalization_fixed_step.h" #include "acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h" #include "acados/ocp_nlp/ocp_nlp_globalization_funnel.h" @@ -894,6 +895,13 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n "nh", &dims_value); return dims_value; } + else if (!strcmp(field, "qpscaling_constr")) + { + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "ng", &dims_value); + dims_value += dims->ni_nl[stage]; + return dims_value; + } // ocp_nlp_cost_dims else if (!strcmp(field, "y_ref") || !strcmp(field, "yref") || !strcmp(field, "ny")) { @@ -1019,6 +1027,11 @@ void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, o dims_out[0] = 1; dims_out[1] = dims->nx[stage]; } + else if (!strcmp(field, "idxs_rev") || !strcmp(field, "relaxed_idxs_rev")) + { + dims_out[0] = 1; + dims_out[1] = dims->nb[stage] + dims->ng[stage] + dims->ni_nl[stage]; + } else if (!strcmp(field, "zl") || !strcmp(field, "zu") || !strcmp(field, "Zl") || !strcmp(field, "Zu") || !strcmp(field, "idxs")) { config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "ns", &dims_out[0]); @@ -1495,6 +1508,11 @@ static void get_from_qp_in(ocp_qp_in *qp_in, int stage, const char *field, void int *int_values = value; d_ocp_qp_get_idxs(stage, qp_in, int_values); } + else if (!strcmp(field, "idxs_rev")) + { + int *int_values = value; + d_ocp_qp_get_idxs_rev(stage, qp_in, int_values); + } else if (!strcmp(field, "idxb")) { int *int_values = value; @@ -1571,17 +1589,24 @@ void ocp_nlp_get_at_stage(ocp_nlp_solver *solver, int stage, const char *field, char module[MAX_STR_LEN]; extract_module_name(field, module, &module_length, &ptr_module); ocp_qp_in *qp_in; - const char *qp_field_name = field; + const char *field_name_getter = field; if ( ptr_module!=NULL && (!strcmp(ptr_module, "relaxed")) ) { ocp_nlp_get(solver, "relaxed_qp_in", &qp_in); - qp_field_name = field+module_length+1; + field_name_getter = field+module_length+1; + get_from_qp_in(qp_in, stage, field_name_getter, value); + } + else if ( ptr_module!=NULL && (!strcmp(ptr_module, "qpscaling")) ) + { + field_name_getter = field+module_length+1; + ocp_nlp_qpscaling_memory_get(dims->qpscaling, nlp_mem->qpscaling, + field_name_getter, stage, value); } else { qp_in = nlp_mem->qp_in; + get_from_qp_in(qp_in, stage, field_name_getter, value); } - get_from_qp_in(qp_in, stage, qp_field_name, value); } } diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 361c6a1ff1..066e0d4325 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -96,6 +96,7 @@ typedef enum INVALID_REGULARIZE, } ocp_nlp_reg_t; + /// Globalization types typedef enum { diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 8bf61d4534..21f7692e4d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -819,6 +819,15 @@ function make_consistent(self, is_mocp_phase) error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); end + qpscaling_scale_constraints_types = {'INF_NORM', 'NO_CONSTRAINT_SCALING'}; + if ~ismember(opts.qpscaling_scale_constraints, qpscaling_scale_constraints_types) + error(['Invalid qpscaling_scale_constraints: ', opts.qpscaling_scale_constraints, '. Available options are: ', strjoin(qpscaling_scale_constraints_types, ', ')]); + end + + qpscaling_scale_objective_types = {'OBJECTIVE_GERSHGORIN', 'NO_OBJECTIVE_SCALING'}; + if ~ismember(opts.qpscaling_scale_objective, qpscaling_scale_objective_types) + error(['Invalid qpscaling_scale_objective: ', opts.qpscaling_scale_objective, '. Available options are: ', strjoin(qpscaling_scale_objective_types, ', ')]); + end % OCP name self.name = model.name; @@ -989,6 +998,12 @@ function make_consistent(self, is_mocp_phase) error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); end + if ~strcmp(opts.qpscaling_scale_constraints, "NO_CONSTRAINT_SCALING") || ~strcmp(opts.qpscaling_scale_objective, "NO_OBJECTIVE_SCALING") + if strcmp(opts.nlp_solver_type, "SQP_RTI") + error('qpscaling_scale_constraints and qpscaling_scale_objective not supported for SQP_RTI solver.'); + end + end + % Set default parameters for globalization ddp_with_merit_or_funnel = strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') || (strcmp(opts.globalization, 'MERIT_BACKTRACKING') && strcmp(opts.nlp_solver_type, 'DDP')); diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 5679229151..9e5081da03 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -83,6 +83,10 @@ reg_max_cond_block reg_min_epsilon reg_adaptive_eps + qpscaling_ub_max_abs_eig + qpscaling_lb_norm_inf_grad_obj + qpscaling_scale_objective + qpscaling_scale_constraints exact_hess_cost exact_hess_dyn exact_hess_constr @@ -102,6 +106,8 @@ globalization_funnel_fraction_switching_condition globalization_funnel_initial_penalty_parameter globalization_funnel_use_merit_fun_only + + search_direction_mode use_constraint_hessian_in_feas_qp allow_direction_mode_switch_to_nominal @@ -188,6 +194,10 @@ obj.print_level = 0; obj.cost_discretization = 'EULER'; obj.regularize_method = 'NO_REGULARIZE'; + obj.qpscaling_ub_max_abs_eig = 1e5; + obj.qpscaling_lb_norm_inf_grad_obj = 1e-4; + obj.qpscaling_scale_objective = 'NO_OBJECTIVE_SCALING'; + obj.qpscaling_scale_constraints = 'NO_CONSTRAINT_SCALING'; obj.reg_epsilon = 1e-4; obj.reg_adaptive_eps = false; obj.reg_max_cond_block = 1e7; @@ -206,6 +216,7 @@ obj.globalization_full_step_dual = []; obj.globalization_eps_sufficient_descent = []; + % funnel options obj.globalization_funnel_init_increase_factor = 15; obj.globalization_funnel_init_upper_bound = 1.0; diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 18a821f4d2..9a30e59d0e 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -179,6 +179,16 @@ function set(obj, field, value, varargin) status = obj.t_ocp.custom_update(data); end + function value = get_qp_scaling_constraints(obj, stage) + % returns the qp scaling constraints for the given stage + value = obj.t_ocp.get('qpscaling_constr', stage); + end + + function value = get_qp_scaling_objective(obj) + % returns the qp scaling objective + value = obj.t_ocp.get('qpscaling_obj'); + end + function value = get(obj, field, varargin) if strcmp('hess_block', field) diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index 977ffad633..7dacf39ca3 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -109,6 +109,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) strcmp(field, "qp_zl") && strcmp(field, "qp_zu") && strcmp(field, "qp_Zl") && + strcmp(field, "qpscaling_obj") && + strcmp(field, "qpscaling_constr") && strcmp(field, "qp_Zu")) { sprintf(buffer, "\nocp_get: invalid stage index, got stage = %d = N, field = %s, field not available at final shooting node\n", stage, field); @@ -473,15 +475,15 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) ocp_nlp_get(solver, "status", &status); *mat_ptr = (double) status; } - else if (!strcmp(field, "sqp_iter") || !strcmp(field, "nlp_iter")) + else if (!strcmp(field, "sqp_iter") || !strcmp(field, "nlp_iter") || !strcmp(field, "qpscaling_status")) // int fields { plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); double *mat_ptr = mxGetPr( plhs[0] ); - int nlp_iter; - ocp_nlp_get(solver, "nlp_iter", &nlp_iter); - *mat_ptr = (double) nlp_iter; + int value; + ocp_nlp_get(solver, field, &value); + *mat_ptr = (double) value; } - 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")) + 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") || !strcmp(field, "time_qp_scaling")) { plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); double *mat_ptr = mxGetPr( plhs[0] ); @@ -522,6 +524,20 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *mat_ptr = mxGetPr( plhs[0] ); ocp_nlp_get(solver, field, mat_ptr); } + else if (!strcmp(field, "qpscaling_constr")) + { + int size = ocp_nlp_dims_get_from_attr(config, dims, out, stage, "qpscaling_constr"); + plhs[0] = mxCreateNumericMatrix(size, 1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + ocp_nlp_get_at_stage(solver, stage, field, mat_ptr); + } + else if (!strcmp(field, "qpscaling_obj")) + { + int size = 1; + plhs[0] = mxCreateNumericMatrix(size, 1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + ocp_nlp_get_at_stage(solver, stage, field, mat_ptr); + } else if (!strcmp(field, "residuals")) { if (plan->nlp_solver == SQP_RTI) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 1a55ee0484..fc937a02c2 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1033,9 +1033,12 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None if any([dims.ng_e, dims.nphi_e, dims.nh_e]): raise ValueError('DDP only supports initial state constraints, got terminal constraints.') - ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') + if opts.qpscaling_scale_constraints != "NO_CONSTRAINT_SCALING" or opts.qpscaling_scale_objective != "NO_OBJECTIVE_SCALING": + if opts.nlp_solver_type == "SQP_RTI": + raise NotImplementedError('qpscaling_scale_constraints and qpscaling_scale_objective not supported for SQP_RTI solver.') # Set default parameters for globalization + ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') if opts.globalization_alpha_min is None: if ddp_with_merit_or_funnel: opts.globalization_alpha_min = 1e-17 diff --git a/interfaces/acados_template/acados_template/acados_ocp_iterate.py b/interfaces/acados_template/acados_template/acados_ocp_iterate.py index e13675569e..1a2a4be88b 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_iterate.py +++ b/interfaces/acados_template/acados_template/acados_ocp_iterate.py @@ -48,7 +48,7 @@ class AcadosOcpFlattenedIterate: pi: np.ndarray lam: np.ndarray - def allclose(self, other, rtol=1e-5, atol=1e-8) -> bool: + def allclose(self, other, rtol=1e-5, atol=1e-6) -> bool: if not isinstance(other, AcadosOcpFlattenedIterate): raise TypeError(f"Expected AcadosOcpFlattenedIterate, got {type(other)}") return ( @@ -95,14 +95,21 @@ class AcadosOcpIterate: def flatten(self) -> AcadosOcpFlattenedIterate: return AcadosOcpFlattenedIterate( x=np.concatenate(self.x_traj), - u=np.concatenate(self.u_traj), - z=np.concatenate(self.z_traj), + u=np.concatenate(self.u_traj) if len(self.u_traj) > 0 else np.array([]), + z=np.concatenate(self.z_traj) if len(self.z_traj) > 0 else np.array([]), sl=np.concatenate(self.sl_traj), su=np.concatenate(self.su_traj), - pi=np.concatenate(self.pi_traj), + pi=np.concatenate(self.pi_traj) if len(self.pi_traj) > 0 else np.array([]), lam=np.concatenate(self.lam_traj), ) + def allclose(self, other, rtol=1e-5, atol=1e-8) -> bool: + if not isinstance(other, AcadosOcpIterate): + raise TypeError(f"Expected AcadosOcpIterate, got {type(other)}") + s = self.flatten() + o = other.flatten() + return s.allclose(o, rtol=rtol, atol=atol) + @dataclass class AcadosOcpIterates: """ diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 454309fc3f..4ced0c5819 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -108,6 +108,10 @@ def __init__(self): self.__globalization_funnel_initial_penalty_parameter = 1.0 self.__globalization_funnel_use_merit_fun_only = False self.__globalization_fixed_step_length = 1.0 + self.__qpscaling_ub_max_abs_eig = 1e5 + self.__qpscaling_lb_norm_inf_grad_obj = 1e-4 + self.__qpscaling_scale_objective = "NO_OBJECTIVE_SCALING" + self.__qpscaling_scale_constraints = "NO_CONSTRAINT_SCALING" self.__ext_cost_num_hess = 0 self.__globalization_use_SOC = 0 self.__globalization_alpha_min = None @@ -354,6 +358,49 @@ def globalization_fixed_step_length(self): """ return self.__globalization_fixed_step_length + @property + def qpscaling_ub_max_abs_eig(self): + """ + Maximum upper bound for eigenvalues of Hessian after QP scaling. + Type: float >= 0. + Default: 1e5. + """ + return self.__qpscaling_ub_max_abs_eig + + @property + def qpscaling_lb_norm_inf_grad_obj(self): + """ + Minimum allowed lower bound for inf norm of qp gradient in QP scaling. + Is attempted to be respected, respecting qpscaling_ub_max_abs_eig is prioritized. + Type: float >= 0. + Default: 1e-4. + """ + return self.__qpscaling_lb_norm_inf_grad_obj + + @property + def qpscaling_scale_objective(self): + """ + String in ["NO_OBJECTIVE_SCALING", "OBJECTIVE_GERSHGORIN"] + Default: "NO_OBJECTIVE_SCALING". + + - NO_OBJECTIVE_SCALING: no scaling of the objective + - OBJECTIVE_GERSHGORIN: estimate max. abs. eigenvalue using Gershgorin circles as `max_abs_eig`, then sets the objective scaling factor as `obj_factor = min(1.0, qpscaling_ub_max_abs_eig/max_abs_eig)` + """ + return self.__qpscaling_scale_objective + + @property + def qpscaling_scale_constraints(self): + """ + String in ["NO_CONSTRAINT_SCALING", "INF_NORM"] + Default: "NO_CONSTRAINT_SCALING". + + - NO_CONSTRAINT_SCALING: no scaling of the constraints + - INF_NORM: scales each constraint except simple bounds by factor `1.0 / max(inf_norm_coeff, inf_norm_constraint_bound)`, such that the inf-norm of the constraint coefficients and bounds is <= 1.0. + Slack penalties are adjusted accordingly to get an equivalent solution. + First, the cost is scaled, then the constraints. + """ + return self.__qpscaling_scale_constraints + @property def nlp_solver_step_length(self): """ @@ -1678,6 +1725,34 @@ def globalization_fixed_step_length(self, globalization_fixed_step_length): else: raise ValueError('Invalid globalization_fixed_step_length value. globalization_fixed_step_length must be a positive float.') + @qpscaling_ub_max_abs_eig.setter + def qpscaling_ub_max_abs_eig(self, qpscaling_ub_max_abs_eig): + if isinstance(qpscaling_ub_max_abs_eig, float) and qpscaling_ub_max_abs_eig >= 0.: + self.__qpscaling_ub_max_abs_eig = qpscaling_ub_max_abs_eig + else: + raise ValueError('Invalid qpscaling_ub_max_abs_eig value. qpscaling_ub_max_abs_eig must be a positive float.') + + @qpscaling_lb_norm_inf_grad_obj.setter + def qpscaling_lb_norm_inf_grad_obj(self, qpscaling_lb_norm_inf_grad_obj): + if isinstance(qpscaling_lb_norm_inf_grad_obj, float) and qpscaling_lb_norm_inf_grad_obj >= 0.: + self.__qpscaling_lb_norm_inf_grad_obj = qpscaling_lb_norm_inf_grad_obj + else: + raise ValueError('Invalid qpscaling_lb_norm_inf_grad_obj value. qpscaling_lb_norm_inf_grad_obj must be a positive float.') + + @qpscaling_scale_objective.setter + def qpscaling_scale_objective(self, qpscaling_scale_objective): + qpscaling_scale_objective_types = ["NO_OBJECTIVE_SCALING", "OBJECTIVE_GERSHGORIN"] + if not qpscaling_scale_objective in qpscaling_scale_objective_types: + raise ValueError(f'Invalid qpscaling_scale_objective value. Must be in {qpscaling_scale_objective_types}, got {qpscaling_scale_objective}.') + self.__qpscaling_scale_objective = qpscaling_scale_objective + + @qpscaling_scale_constraints.setter + def qpscaling_scale_constraints(self, qpscaling_scale_constraints): + qpscaling_scale_constraints_types = ["NO_CONSTRAINT_SCALING", "INF_NORM"] + if not qpscaling_scale_constraints in qpscaling_scale_constraints_types: + raise ValueError(f'Invalid qpscaling_scale_constraints value. Must be in {qpscaling_scale_constraints_types}, got {qpscaling_scale_constraints}.') + self.__qpscaling_scale_constraints = qpscaling_scale_constraints + @nlp_solver_step_length.setter def nlp_solver_step_length(self, nlp_solver_step_length): print("The option nlp_solver_step_length is deprecated and has new name: globalization_fixed_step_length") @@ -2076,3 +2151,6 @@ def _ensure_solution_sensitivities_available(self, parametric: bool = True, has_ if parametric and not self.with_solution_sens_wrt_params: raise ValueError("Parametric sensitivities are only available if with_solution_sens_wrt_params is set to True.") + + if self.qpscaling_scale_constraints != "NO_CONSTRAINT_SCALING" or self.qpscaling_scale_objective != "NO_OBJECTIVE_SCALING": + raise ValueError("Parametric sensitivities are only available if no scaling is applied to the QP.") diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index d31adca371..05607bd412 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -303,7 +303,7 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__qp_dynamics_fields = {'A', 'B', 'b'} self.__qp_cost_fields = {'Q', 'R', 'S', 'q', 'r', 'zl', 'zu', 'Zl', 'Zu'} self.__qp_constraint_fields = {'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'} - self.__qp_constraint_int_fields = {'idxs', 'idxb'} + self.__qp_constraint_int_fields = {'idxs', 'idxb', 'idxs_rev'} self.__qp_pc_hpipm_fields = {'P', 'K', 'Lr', 'p'} self.__qp_pc_fields = {'pcond_Q', 'pcond_R', 'pcond_S'} self.__all_qp_fields = self.__qp_dynamics_fields | self.__qp_cost_fields | self.__qp_constraint_fields | self.__qp_constraint_int_fields | self.__qp_pc_hpipm_fields | self.__qp_pc_fields @@ -1495,6 +1495,7 @@ def get_status(self) -> int: - 5: Solver created (ACADOS_READY) - 6: Problem unbounded (ACADOS_UNBOUNDED) - 7: Solver timeout (ACADOS_TIMEOUT) + - 8: QP scaling could not satisfy bounds (ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED); NOTE: this status is typically not returned by the solver, but can be checked via `get_stats('qpscaling_status')` See `return_values` in https://github.com/acados/acados/blob/main/acados/utils/types.h """ @@ -1504,7 +1505,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', 'nlp_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', 'time_qpscaling', 'nlp_iter', 'sqp_iter', 'residuals', 'qp_iter', 'alpha'] Available fields: - time_tot: total CPU time previous call @@ -1515,6 +1516,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: - time_qp: CPU time qp solution - time_qp_solver_call: CPU time inside qp solver (without converting the QP) - time_qp_xcond: time_glob: CPU time globalization + - time_qpscaling: CPU time for QP scaling - time_solution_sensitivities: CPU time for previous call to eval_param_sens - time_solution_sens_lin: CPU time for linearization in eval_param_sens - time_solution_sens_solve: CPU time for solving in eval_solution_sensitivity @@ -1523,8 +1525,9 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: - time_feedback: CPU time for last feedback phase, relevant for (AS-)RTI, otherwise returns total compuation time. - sqp_iter: number of SQP iterations - nlp_iter: number of NLP solver iterations (DDP or SQP) - - qp_stat: status of QP solver - - qp_iter: vector of QP iterations for last SQP call + - qp_stat: vector of QP solver status for last NLP solver call + - qp_iter: vector of QP iterations for last NLP solver call + - qpscaling_status: status of last call to qpscaling module - statistics: table with info about last iteration - stat_m: number of rows in statistics matrix - stat_n: number of columns in statistics matrix @@ -1545,6 +1548,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: 'time_qp', 'time_qp_solver_call', 'time_qp_xcond', + 'time_qpscaling', 'time_glob', 'time_solution_sensitivities', 'time_reg', @@ -1552,15 +1556,11 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: 'time_feedback', 'qp_tau_iter', ] - fields = double_fields + [ - 'sqp_iter', - 'ddp_iter', - 'nlp_iter', + int_fields = ['ddp_iter', 'sqp_iter', 'nlp_iter', 'stat_m', 'stat_n', 'qpscaling_status'] + fields = double_fields + int_fields + [ 'qp_stat', 'qp_iter', 'statistics', - 'stat_m', - 'stat_n', 'residuals', 'alpha', 'res_eq_all', @@ -1569,7 +1569,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: field = field_.encode('utf-8') - if field_ in ['ddp_iter', 'sqp_iter', 'nlp_iter', 'stat_m', 'stat_n']: + if field_ in int_fields: out = c_int(0) self.__acados_lib.ocp_nlp_get(self.nlp_solver, field, byref(out)) return out.value @@ -2117,6 +2117,45 @@ def get_from_qp_in(self, stage_: int, field_: str): return out + def get_qp_scaling_constraints(self, stage: int) -> np.ndarray: + """ + If the solver performs QP scaling, this function returns the scaling factors for the constraints. + Bounds are not scaled, so the dimension is ng + nh + nphi. + Only available if qpscaling_scale_constraints != NO_CONSTRAINT_SCALING. + """ + if self.__solver_options["qpscaling_scale_constraints"] == "NO_CONSTRAINT_SCALING": + raise ValueError(f"get_qp_scaling_constraints: only works if qpscaling_scale_constraints != NO_CONSTRAINT_SCALING.") + + # call getter + field_ = "qpscaling_constr" + field = field_.encode('utf-8') + dims = self.dims_get(field_, stage) + out = np.zeros((dims,), dtype=np.float64, order="C") + out_data = cast(out.ctypes.data, POINTER(c_double)) + out_data_p = cast((out_data), c_void_p) + self.__acados_lib.ocp_nlp_get_at_stage(self.nlp_solver, stage, field, out_data_p) + + return out + + + def get_qp_scaling_objective(self) -> float: + """ + Returns the cost scaling value corresponding to the previous QP solution. + Only available if qpscaling_scale_objective != NO_OBJECTIVE_SCALING. + """ + if self.__solver_options["qpscaling_scale_objective"] == "NO_OBJECTIVE_SCALING": + raise ValueError("get_qp_scaling_objective: only works for QP solvers with qpscaling_scale_objective != NO_OBJECTIVE_SCALING.") + + # create output array + out = np.zeros((1,), dtype=np.float64, order="C") + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getter + field = "qpscaling_obj".encode('utf-8') + self.__acados_lib.ocp_nlp_get_at_stage(self.nlp_solver, 0, field, out_data) + + return out[0] + def __ocp_nlp_get_from_iterate(self, iteration_, stage_, field_): stage = c_int(stage_) field = field_.encode('utf-8') 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 f88104467c..118b9379ac 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 @@ -2373,6 +2373,19 @@ ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "allow_direction_mode_switch_to_no bool eval_residual_at_max_iter = {{ solver_options.eval_residual_at_max_iter }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); + // QP scaling + double qpscaling_ub_max_abs_eig = {{ solver_options.qpscaling_ub_max_abs_eig }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); + + double qpscaling_lb_norm_inf_grad_obj = {{ solver_options.qpscaling_lb_norm_inf_grad_obj }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); + + qpscaling_scale_objective_type qpscaling_scale_objective = {{ solver_options.qpscaling_scale_objective }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); + + ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = {{ solver_options.qpscaling_scale_constraints }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + {%- if solver_options.nlp_solver_type == "SQP" and solver_options.timeout_max_time > 0 %} double timeout_max_time = {{ solver_options.timeout_max_time }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "timeout_max_time", &timeout_max_time); 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 4527c029d8..8d9e72be8c 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 @@ -2514,6 +2514,19 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps bool eval_residual_at_max_iter = {{ solver_options.eval_residual_at_max_iter }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); + // QP scaling + double qpscaling_ub_max_abs_eig = {{ solver_options.qpscaling_ub_max_abs_eig }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); + + double qpscaling_lb_norm_inf_grad_obj = {{ solver_options.qpscaling_lb_norm_inf_grad_obj }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); + + qpscaling_scale_objective_type qpscaling_scale_objective = {{ solver_options.qpscaling_scale_objective }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); + + ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = {{ solver_options.qpscaling_scale_constraints }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + {%- if solver_options.nlp_solver_type == "SQP" and solver_options.timeout_max_time > 0 %} double timeout_max_time = {{ solver_options.timeout_max_time }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "timeout_max_time", &timeout_max_time); 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 3145928dc7..77a51fee54 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 @@ -121,7 +121,6 @@ function eval_param_sens(obj, field, stage, index) ocp_eval_param_sens(obj.C_ocp, field, stage, index); end - % get -- borrowed from MEX interface function value = get(varargin) % usage: % obj.get(field, value, [stage]) From a66778d78b1aab194d9b788fa550ff06fce17118 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 26 Jun 2025 08:39:39 +0200 Subject: [PATCH 078/164] Fix parallel execution of casadi_tests (#1563) --- interfaces/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 820690c6b2..d12408b5b0 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -363,7 +363,7 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_convex_ocp_with_onesided_constraints COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_ocp_with_onesided_constraints python main_convex_onesided.py) - + add_test(NAME python_casadi_get_set_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_get_set.py) @@ -448,6 +448,10 @@ add_test(NAME python_pendulum_ocp_example_cmake # Directory zoRO_example set_tests_properties(python_fast_zoro_example PROPERTIES DEPENDS python_zoro_diff_drive_example) + # casadi_tests + set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS test_casadi_p_in_constraint_and_cost) + set_tests_properties(test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS test_casadi_parametric) + # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) set_tests_properties(python_pendulum_closed_loop_example PROPERTIES DEPENDS python_pendulum_sim_example) From c4965e7d585e1aeba4ac876a70740bbd8b4e2163 Mon Sep 17 00:00:00 2001 From: Bao Date: Thu, 26 Jun 2025 14:01:02 +0700 Subject: [PATCH 079/164] Update tera_renderer automatic downloading (#1560) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Continued from: https://github.com/acados/tera_renderer/pull/13 Associated with: https://github.com/acados/tera_renderer/pull/14 --------- Co-authored-by: Ngô Việt Hoài Bảo Co-authored-by: Jonathan Frey --- .github/linux/install_tera.sh | 4 +- docs/python_interface/index.md | 5 ++- .../acados_matlab_octave/set_up_t_renderer.m | 14 ++++-- .../acados_template/acados_template/utils.py | 44 +++++++++++++++---- 4 files changed, 50 insertions(+), 17 deletions(-) diff --git a/.github/linux/install_tera.sh b/.github/linux/install_tera.sh index e6168bcf42..0398e89c74 100755 --- a/.github/linux/install_tera.sh +++ b/.github/linux/install_tera.sh @@ -30,9 +30,9 @@ # # install tera -TERA_RENDERER_VERSION='0.0.34'; +TERA_RENDERER_VERSION='0.2.0'; _TERA_RENDERER_GITHUB_RELEASES="https://github.com/acados/tera_renderer/releases/download/v${TERA_RENDERER_VERSION}/"; -TERA_RENDERER_URL="${_TERA_RENDERER_GITHUB_RELEASES}/t_renderer-v${TERA_RENDERER_VERSION}-linux"; +TERA_RENDERER_URL="${_TERA_RENDERER_GITHUB_RELEASES}/t_renderer-v${TERA_RENDERER_VERSION}-linux-amd64"; mkdir -p bin; pushd bin; diff --git a/docs/python_interface/index.md b/docs/python_interface/index.md index cc3aa58bd6..5fc383eea5 100644 --- a/docs/python_interface/index.md +++ b/docs/python_interface/index.md @@ -57,8 +57,9 @@ Currently, Python >= 3.8 is tested. 5. Optional: Can be done automatically through the interface: In order to be able to successfully render C code templates, you need to download the `t_renderer` binaries for your platform from and place them in `/bin` - Please strip the version and platform from the binaries (e.g.`t_renderer-v0.0.34 -> t_renderer`). - Notice that you might need to make `t_renderer` executable. + Please strip the version, platform and architecture from the binaries (e.g.`t_renderer-V-P-A -> t_renderer`). + On Linux/MacOS, you might need to make `t_renderer` executable. + On Windows, make sure you append `.exe` extension to the end (`t_renderer.exe`). Run `export ACADOS_SOURCE_DIR=` such that the location of acados will be known to the Python package at run time. 6. Optional: Set `acados_lib_path`, `acados_include_path`. diff --git a/interfaces/acados_matlab_octave/set_up_t_renderer.m b/interfaces/acados_matlab_octave/set_up_t_renderer.m index 66d8531675..b4d18ca869 100644 --- a/interfaces/acados_matlab_octave/set_up_t_renderer.m +++ b/interfaces/acados_matlab_octave/set_up_t_renderer.m @@ -26,13 +26,19 @@ function set_up_t_renderer(t_renderer_location, varargin) end end - t_renderer_version = 'v0.0.34'; + t_renderer_version = 'v0.2.0'; if ismac() - suffix = '-osx'; + [~,result] = system('uname -v'); + if any(strfind(result,'ARM64')) + suffix = '-osx-arm64'; + else + % failsafe, newer mac users can always use rosetta with this + suffix = '-osx-amd64'; + end elseif isunix() - suffix = '-linux'; + suffix = '-linux-amd64'; elseif ispc() - suffix = '-windows'; + suffix = '-windows-amd64.exe'; end acados_root_dir = getenv('ACADOS_INSTALL_DIR'); diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 0acc0e13c8..afca4d2db8 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -34,6 +34,7 @@ import os import shutil import sys +import platform import urllib.request from subprocess import DEVNULL, STDOUT, call if os.name == 'nt': @@ -47,7 +48,7 @@ from contextlib import contextmanager -TERA_VERSION = "0.0.34" +TERA_VERSION = "0.2.0" PLATFORM2TERA = { "linux": "linux", @@ -97,9 +98,7 @@ def get_python_interface_path(): def get_tera_exec_path(): TERA_PATH = os.environ.get('TERA_PATH') if not TERA_PATH: - TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') - if os.name == 'nt': - TERA_PATH += '.exe' + TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') + get_binary_ext() return TERA_PATH @@ -232,6 +231,25 @@ def get_shared_lib_prefix(): else: return 'lib' +def get_binary_ext(): + if os.name == 'nt': + return '.exe' + else: + return '' + +def get_architecture_amd64_arm64(): + # common uname -m results + # https://en.wikipedia.org/wiki/Uname + current_arch = platform.machine() + amd64_compatible = ["i3", "i6", "amd", "x86"] + arm64_compatible = ["arm", "aarch"] + if any([current_arch.lower().startswith(arch) for arch in amd64_compatible]): + return "amd64" + elif any([current_arch.lower().startswith(arch) for arch in arm64_compatible]): + return "arm64" + else: + raise RuntimeError(f"Your detected architecture {current_arch} may not be compatible with amd64 or arm64.") + def get_tera() -> str: tera_path = get_tera_exec_path() acados_path = get_acados_path() @@ -240,15 +258,23 @@ def get_tera() -> str: if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): return tera_path + try: + arch = get_architecture_amd64_arm64() + except RuntimeError as e: + print(e) + print("Try building tera_renderer from source at https://github.com/acados/tera_renderer") + sys.exit(1) + + binary_ext = get_binary_ext() repo_url = "https://github.com/acados/tera_renderer/releases" - url = "{}/download/v{}/t_renderer-v{}-{}".format( - repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform]) + url = "{}/download/v{}/t_renderer-v{}-{}-{}{}".format( + repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform], arch, binary_ext) manual_install = 'For manual installation follow these instructions:\n' manual_install += '1 Download binaries from {}\n'.format(url) manual_install += '2 Copy them in {}/bin\n'.format(acados_path) - manual_install += '3 Strip the version and platform from the binaries: ' - manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n' + manual_install += '3 Strip the version and platform and architecture from the binaries: ' + manual_install += f'as t_renderer-v{TERA_VERSION}-P-A{binary_ext} -> t_renderer{binary_ext})\n' manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' manual_install += '"chmod +x {}"\n\n'.format(tera_path) @@ -276,7 +302,7 @@ def get_tera() -> str: os.makedirs(tera_dir) # Download tera - print(f"Dowloading {url}") + print(f"Downloading {url}") with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: shutil.copyfileobj(response, out_file) print("Successfully downloaded t_renderer.") From be0f1ace641094d4076cd49303cae2b528e0ab9f Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Thu, 26 Jun 2025 12:18:21 +0200 Subject: [PATCH 080/164] Move options from solvers to `ocp_nlp_common` (#1564) We moved the solution tolerances, `warm_start_qp`-opts, and `eval_residual_at_max_iter` to `ocp_nlp_common`. This removes redundancy in the code and makes tolerances available in cases where only `ocp_nlp_opts` is available. --- acados/ocp_nlp/ocp_nlp_common.c | 67 ++++++++++ acados/ocp_nlp/ocp_nlp_common.h | 11 ++ acados/ocp_nlp/ocp_nlp_ddp.c | 93 +++---------- acados/ocp_nlp/ocp_nlp_ddp.h | 7 - acados/ocp_nlp/ocp_nlp_sqp.c | 108 +++------------- acados/ocp_nlp/ocp_nlp_sqp.h | 9 -- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 19 +-- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 2 - acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 122 +++++------------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 10 -- 10 files changed, 148 insertions(+), 300 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index eeb747e5f2..1b9e9dc8a3 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1265,6 +1265,25 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) opts->qp_warm_start = 0; opts->store_iterates = false; + opts->warm_start_first_qp = false; + opts->warm_start_first_qp_from_nlp = false; + opts->eval_residual_at_max_iter = false; + + // tolerances + opts->tol_stat = 1e-8; + opts->tol_eq = 1e-8; + opts->tol_ineq = 1e-8; + opts->tol_comp = 1e-8; + opts->tol_unbounded = -1e10; + opts->tol_min_step_norm = 1e-12; + + // overwrite default submodules opts + // qp tolerance + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_stat", &opts->tol_stat); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_eq", &opts->tol_eq); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_ineq", &opts->tol_ineq); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_comp", &opts->tol_comp); + return; } @@ -1517,6 +1536,54 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value bool* with_anderson_acceleration = (bool *) value; opts->with_anderson_acceleration = *with_anderson_acceleration; } + else if (!strcmp(field, "tol_stat")) + { + double* tol_stat = (double *) value; + opts->tol_stat = *tol_stat; + // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_stat", value); + } + else if (!strcmp(field, "tol_eq")) + { + double* tol_eq = (double *) value; + opts->tol_eq = *tol_eq; + // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_eq", value); + } + else if (!strcmp(field, "tol_ineq")) + { + double* tol_ineq = (double *) value; + opts->tol_ineq = *tol_ineq; + // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_ineq", value); + } + else if (!strcmp(field, "tol_comp")) + { + double* tol_comp = (double *) value; + opts->tol_comp = *tol_comp; + // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_comp", value); + } + else if (!strcmp(field, "tol_min_step_norm")) + { + double* tol_min_step_norm = (double *) value; + opts->tol_min_step_norm = *tol_min_step_norm; + } + else if (!strcmp(field, "warm_start_first_qp")) + { + bool* warm_start_first_qp = (bool *) value; + opts->warm_start_first_qp = *warm_start_first_qp; + } + else if (!strcmp(field, "warm_start_first_qp_from_nlp")) + { + bool* warm_start_first_qp_from_nlp = (bool *) value; + opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; + } + else if (!strcmp(field, "eval_residual_at_max_iter")) + { + bool* eval_residual_at_max_iter = (bool *) value; + opts->eval_residual_at_max_iter = *eval_residual_at_max_iter; + } else { printf("\nerror: ocp_nlp_opts_set: wrong field: %s\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 0b3b66ce06..1f02190ef9 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -336,10 +336,21 @@ typedef struct ocp_nlp_opts int ext_qp_res; int qp_warm_start; + 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 bool store_iterates; // flag indicating whether intermediate iterates should be stored bool with_anderson_acceleration; + // termination tolerances + double tol_stat; // exit tolerance on stationarity condition + double tol_eq; // exit tolerance on equality constraints + double tol_ineq; // exit tolerance on inequality constraints + double tol_comp; // exit tolerance on complementarity condition + double tol_unbounded; // exit threshold when objective function seems to be unbounded + double tol_min_step_norm; // exit tolerance for small step + } ocp_nlp_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index 1f0d4708f4..8b1fa76645 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -111,22 +111,6 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_ // DDP opts opts->nlp_opts->max_iter = 20; - opts->tol_stat = 1e-8; - opts->tol_eq = 1e-8; - opts->tol_ineq = 1e-8; - opts->tol_comp = 1e-8; - opts->tol_zero_res = 1e-12; - - opts->warm_start_first_qp = false; - opts->warm_start_first_qp_from_nlp = false; - opts->eval_residual_at_max_iter = false; - - // overwrite default submodules opts - // qp tolerance - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", &opts->tol_stat); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", &opts->tol_eq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", &opts->tol_ineq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", &opts->tol_comp); return; } @@ -165,53 +149,7 @@ void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* v } else // nlp opts { - if (!strcmp(field, "tol_stat")) - { - double* tol_stat = (double *) value; - opts->tol_stat = *tol_stat; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", value); - } - else if (!strcmp(field, "tol_eq")) - { - double* tol_eq = (double *) value; - opts->tol_eq = *tol_eq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", value); - } - else if (!strcmp(field, "tol_ineq")) - { - double* tol_ineq = (double *) value; - opts->tol_ineq = *tol_ineq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", value); - } - else if (!strcmp(field, "tol_comp")) - { - double* tol_comp = (double *) value; - opts->tol_comp = *tol_comp; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", value); - } - else if (!strcmp(field, "warm_start_first_qp")) - { - bool* warm_start_first_qp = (bool *) value; - opts->warm_start_first_qp = *warm_start_first_qp; - } - else if (!strcmp(field, "warm_start_first_qp_from_nlp")) - { - bool* warm_start_first_qp_from_nlp = (bool *) value; - opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; - } - else if (!strcmp(field, "eval_residual_at_max_iter")) - { - bool* eval_residual_at_max_iter = (bool *) value; - opts->eval_residual_at_max_iter = *eval_residual_at_max_iter; - } - else - { - ocp_nlp_opts_set(config, nlp_opts, field, value); - } + ocp_nlp_opts_set(config, nlp_opts, field, value); } return; @@ -499,6 +437,7 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_memory *mem, ocp_nlp_ddp_opts *opts) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; + ocp_nlp_opts *nlp_opts = opts->nlp_opts; // check for nans if (isnan(nlp_res->inf_norm_res_stat) || isnan(nlp_res->inf_norm_res_eq) || isnan(nlp_res->inf_norm_res_ineq)) @@ -513,12 +452,12 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me // We do not need to check for the complementarity condition and for the // inequalities since we have an unconstrainted OCP - if (nlp_res->inf_norm_res_eq < opts->tol_eq) + if (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq) { // Check that iterate must be dynamically feasible - if (nlp_res->inf_norm_res_stat < opts->tol_stat) + if (nlp_res->inf_norm_res_stat < nlp_opts->tol_stat) {// Check Stationarity nlp_mem->status = ACADOS_SUCCESS; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Optimal Solution found! Converged to KKT point.\n"); } @@ -526,10 +465,10 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me } // Check for zero-residual solution of a least-squares problem - if (opts->nlp_opts->with_adaptive_levenberg_marquardt && (nlp_mem->cost_value < opts->tol_zero_res)) + if (nlp_opts->with_adaptive_levenberg_marquardt && (nlp_mem->cost_value < opts->tol_zero_res)) { nlp_mem->status = ACADOS_SUCCESS; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Optimal Solution found! Converged To Zero Residual Solution.\n"); } @@ -538,11 +477,11 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me } // Check for small step - if ((ddp_iter > 0) && (mem->step_norm < opts->tol_eq)) + if ((ddp_iter > 0) && (mem->step_norm < nlp_opts->tol_eq)) { - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { - if (nlp_res->inf_norm_res_eq < opts->tol_eq) + if (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq) { printf("Stopped: Converged To Feasible Point. Step size is < tol_eq.\n"); } @@ -556,10 +495,10 @@ static bool check_termination(int ddp_iter, ocp_nlp_res *nlp_res, ocp_nlp_ddp_me } // Check for maximum iterations - if (ddp_iter >= opts->nlp_opts->max_iter) + if (ddp_iter >= nlp_opts->max_iter) { nlp_mem->status = ACADOS_MAXITER; - if (opts->nlp_opts->print_level > 0){ + if (nlp_opts->print_level > 0){ printf("Stopped: Maximum Iterations Reached.\n"); } return true; @@ -672,7 +611,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // 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->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (ddp_iter != opts->nlp_opts->max_iter || nlp_opts->eval_residual_at_max_iter) { /* Prepare the QP data */ // linearize NLP, update QP matrices, and add Levenberg-Marquardt term @@ -705,7 +644,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Check if initial guess was infeasible - if ((infeasible_initial_guess == true) && (nlp_res->inf_norm_res_eq > opts->tol_eq)) + if ((infeasible_initial_guess == true) && (nlp_res->inf_norm_res_eq > nlp_opts->tol_eq)) { if (nlp_opts->print_level > 0) { @@ -747,13 +686,13 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // warm start of first QP if (ddp_iter == 0) { - if (!opts->warm_start_first_qp) + if (!nlp_opts->warm_start_first_qp) { // (typically) no warm start at first iteration int tmp_int = 0; qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int); } - else if (opts->warm_start_first_qp_from_nlp) + else if (nlp_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); diff --git a/acados/ocp_nlp/ocp_nlp_ddp.h b/acados/ocp_nlp/ocp_nlp_ddp.h index a8ddafa584..332428c9f8 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.h +++ b/acados/ocp_nlp/ocp_nlp_ddp.h @@ -56,14 +56,7 @@ extern "C" { typedef struct { ocp_nlp_opts *nlp_opts; - double tol_stat; // exit tolerance on stationarity condition - double tol_eq; // exit tolerance on equality constraints - double tol_ineq; // exit tolerance on inequality constraints - double tol_comp; // exit tolerance on complementarity condition double tol_zero_res; // exit tolerance if objective function is 0 for least-squares problem - 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 } ocp_nlp_ddp_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index b326c5b406..69ca2ac1f2 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -107,33 +107,14 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ ocp_nlp_sqp_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // this first !!! ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP opts opts->nlp_opts->max_iter = 20; - opts->tol_stat = 1e-8; - opts->tol_eq = 1e-8; - opts->tol_ineq = 1e-8; - opts->tol_comp = 1e-8; - opts->tol_unbounded = -1e10; - opts->tol_min_step_norm = 1e-12; - - opts->warm_start_first_qp = false; - opts->eval_residual_at_max_iter = false; - opts->timeout_heuristic = ZERO; opts->timeout_max_time = 0; // corresponds to no timeout - // overwrite default submodules opts - // qp tolerance - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", &opts->tol_stat); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", &opts->tol_eq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", &opts->tol_ineq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", &opts->tol_comp); - return; } @@ -171,55 +152,7 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v } else // nlp opts { - if (!strcmp(field, "tol_stat")) - { - double* tol_stat = (double *) value; - opts->tol_stat = *tol_stat; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", value); - } - else if (!strcmp(field, "tol_eq")) - { - double* tol_eq = (double *) value; - opts->tol_eq = *tol_eq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", value); - } - else if (!strcmp(field, "tol_ineq")) - { - double* tol_ineq = (double *) value; - opts->tol_ineq = *tol_ineq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", value); - } - else if (!strcmp(field, "tol_comp")) - { - double* tol_comp = (double *) value; - opts->tol_comp = *tol_comp; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", value); - } - else if (!strcmp(field, "tol_min_step_norm")) - { - double* tol_min_step_norm = (double *) value; - opts->tol_min_step_norm = *tol_min_step_norm; - } - else if (!strcmp(field, "warm_start_first_qp")) - { - bool* warm_start_first_qp = (bool *) value; - opts->warm_start_first_qp = *warm_start_first_qp; - } - else if (!strcmp(field, "warm_start_first_qp_from_nlp")) - { - bool* warm_start_first_qp_from_nlp = (bool *) value; - opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; - } - else if (!strcmp(field, "eval_residual_at_max_iter")) - { - bool* eval_residual_at_max_iter = (bool *) value; - opts->eval_residual_at_max_iter = *eval_residual_at_max_iter; - } - else if (!strcmp(field, "timeout_max_time")) + if (!strcmp(field, "timeout_max_time")) { double* timeout_max_time = (double *) value; opts->timeout_max_time = *timeout_max_time; @@ -418,13 +351,14 @@ void ocp_nlp_sqp_work_get(void *config_, void *dims_, void *work_, static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_res, ocp_nlp_sqp_memory *mem, ocp_nlp_sqp_opts *opts) { // ocp_nlp_memory *nlp_mem = mem->nlp_mem; + ocp_nlp_opts *nlp_opts = opts->nlp_opts; // check for nans if (isnan(nlp_res->inf_norm_res_stat) || isnan(nlp_res->inf_norm_res_eq) || isnan(nlp_res->inf_norm_res_ineq) || isnan(nlp_res->inf_norm_res_comp)) { mem->nlp_mem->status = ACADOS_NAN_DETECTED; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: NaN detected in iterate.\n"); } @@ -432,10 +366,10 @@ 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->nlp_opts->max_iter) + if (!nlp_opts->eval_residual_at_max_iter && n_iter >= nlp_opts->max_iter) { mem->nlp_mem->status = ACADOS_MAXITER; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Maximum iterations reached.\n"); } @@ -443,13 +377,13 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check if solved to tolerance - if ((nlp_res->inf_norm_res_stat < opts->tol_stat) && - (nlp_res->inf_norm_res_eq < opts->tol_eq) && - (nlp_res->inf_norm_res_ineq < opts->tol_ineq) && - (nlp_res->inf_norm_res_comp < opts->tol_comp)) + if ((nlp_res->inf_norm_res_stat < nlp_opts->tol_stat) && + (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq) && + (nlp_res->inf_norm_res_ineq < nlp_opts->tol_ineq) && + (nlp_res->inf_norm_res_comp < nlp_opts->tol_comp)) { mem->nlp_mem->status = ACADOS_SUCCESS; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Optimal solution found! Converged to KKT point.\n"); } @@ -457,11 +391,11 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for small step - if (opts->tol_min_step_norm > 0.0 && (n_iter > 0) && (mem->step_norm < opts->tol_min_step_norm)) + if (nlp_opts->tol_min_step_norm > 0.0 && (n_iter > 0) && (mem->step_norm < nlp_opts->tol_min_step_norm)) { - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { - if (nlp_res->inf_norm_res_eq < opts->tol_eq && nlp_res->inf_norm_res_ineq < opts->tol_ineq) + if (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq && nlp_res->inf_norm_res_ineq < nlp_opts->tol_ineq) { printf("Stopped: Converged to feasible point. Step size is < tol_eq.\n"); } @@ -475,10 +409,10 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for unbounded problem - if (mem->nlp_mem->cost_value <= opts->tol_unbounded) + if (mem->nlp_mem->cost_value <= nlp_opts->tol_unbounded) { mem->nlp_mem->status = ACADOS_UNBOUNDED; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Problem seems to be unbounded.\n"); } @@ -486,10 +420,10 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for maximum iterations - if (n_iter >= opts->nlp_opts->max_iter) + if (n_iter >= nlp_opts->max_iter) { mem->nlp_mem->status = ACADOS_MAXITER; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Maximum iterations reached.\n"); } @@ -601,7 +535,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // 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 (nlp_mem->iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->iter != opts->nlp_opts->max_iter || nlp_opts->eval_residual_at_max_iter) { // store current iterate if (nlp_opts->store_iterates) @@ -717,13 +651,13 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // warm start of first QP if (nlp_mem->iter == 0) { - if (!opts->warm_start_first_qp) + if (!nlp_opts->warm_start_first_qp) { // (typically) no warm start at first iteration int tmp_int = 0; qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int); } - else if (opts->warm_start_first_qp_from_nlp) + else if (nlp_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); @@ -818,7 +752,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Compute the step norm - if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm || nlp_opts->print_level > 0) + if (nlp_opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm || nlp_opts->print_level > 0) { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(qp_out); if (nlp_opts->log_primal_step_norm) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index 07ca3d5564..6a47f3a0a2 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -56,16 +56,7 @@ extern "C" { typedef struct { ocp_nlp_opts *nlp_opts; - double tol_stat; // exit tolerance on stationarity condition - double tol_eq; // exit tolerance on equality constraints - double tol_ineq; // exit tolerance on inequality constraints - double tol_comp; // exit tolerance on complementarity condition - double tol_unbounded; // exit threshold when objective function seems to be unbounded - double tol_min_step_norm; // exit tolerance for small step int log_primal_step_norm; // compute and log the max norm of the primal steps - 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 double timeout_max_time; // maximum time the solve may require before timeout is triggered. No timeout if 0. ocp_nlp_timeout_heuristic_t timeout_heuristic; // type of heuristic used to predict solve time of next QP } ocp_nlp_sqp_opts; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 7688a8005f..2d110a898d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -113,8 +113,7 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP RTI opts - opts->warm_start_first_qp = false; - opts->warm_start_first_qp_from_nlp = true; + opts->nlp_opts->warm_start_first_qp_from_nlp = true; opts->rti_phase = 0; opts->as_rti_level = STANDARD_RTI; opts->as_rti_advancement_strategy = SIMULATE_ADVANCE; @@ -160,12 +159,7 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, } else // nlp opts { - if (!strcmp(field, "warm_start_first_qp")) - { - bool* warm_start_first_qp = (bool *) value; - opts->warm_start_first_qp = *warm_start_first_qp; - } - else if (!strcmp(field, "rti_phase")) + if (!strcmp(field, "rti_phase")) { int* rti_phase = (int *) value; if (*rti_phase < 0 || *rti_phase > 2) @@ -191,11 +185,6 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, int* rti_log_only_available_residuals = (int *) value; opts->rti_log_only_available_residuals = *rti_log_only_available_residuals; } - else if (!strcmp(field, "warm_start_first_qp_from_nlp")) - { - bool* warm_start_first_qp_from_nlp = (bool *) value; - opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; - } else if (!strcmp(field, "as_rti_level")) { int* as_rti_level = (int *) value; @@ -571,13 +560,13 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * // set QP warm start if (mem->is_first_call) { - if (!opts->warm_start_first_qp) + if (!nlp_opts->warm_start_first_qp) { int tmp_int = 0; config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "warm_start", &tmp_int); } - else if (opts->warm_start_first_qp_from_nlp) + else if (nlp_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); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 40ddaa0bdf..1d9d100d8d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -80,8 +80,6 @@ typedef struct { ocp_nlp_opts *nlp_opts; int compute_dual_sol; - 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; as_rti_level_t as_rti_level; as_rti_advancement_strategy_t as_rti_advancement_strategy; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index d26c4a660d..1220e90a6b 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -103,22 +103,13 @@ void ocp_nlp_sqp_wfqp_opts_initialize_default(void *config_, void *dims_, void * ocp_nlp_sqp_wfqp_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // this first !!! ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP opts opts->nlp_opts->max_iter = 20; - opts->tol_stat = 1e-8; - opts->tol_eq = 1e-8; - opts->tol_ineq = 1e-8; - opts->tol_comp = 1e-8; - opts->tol_unbounded = -1e10; - opts->tol_min_step_norm = 1e-12; - - opts->warm_start_first_qp = false; - opts->eval_residual_at_max_iter = true; + + opts->nlp_opts->eval_residual_at_max_iter = true; opts->use_QP_l1_inf_from_slacks = false; // if manual calculation used, results seem more accurate and solver performs better! opts->use_constraint_hessian_in_feas_qp = false; @@ -129,13 +120,6 @@ void ocp_nlp_sqp_wfqp_opts_initialize_default(void *config_, void *dims_, void * opts->log_pi_norm_inf = true; opts->log_lam_norm_inf = true; - // overwrite default submodules opts - // qp tolerance - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", &opts->tol_stat); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", &opts->tol_eq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", &opts->tol_ineq); - qp_solver->opts_set(qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", &opts->tol_comp); - return; } @@ -185,55 +169,7 @@ void ocp_nlp_sqp_wfqp_opts_set(void *config_, void *opts_, const char *field, vo } else // nlp opts { - if (!strcmp(field, "tol_stat")) - { - double* tol_stat = (double *) value; - opts->tol_stat = *tol_stat; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_stat", value); - } - else if (!strcmp(field, "tol_eq")) - { - double* tol_eq = (double *) value; - opts->tol_eq = *tol_eq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_eq", value); - } - else if (!strcmp(field, "tol_ineq")) - { - double* tol_ineq = (double *) value; - opts->tol_ineq = *tol_ineq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_ineq", value); - } - else if (!strcmp(field, "tol_comp")) - { - double* tol_comp = (double *) value; - opts->tol_comp = *tol_comp; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. - config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", value); - } - else if (!strcmp(field, "tol_min_step_norm")) - { - double* tol_min_step_norm = (double *) value; - opts->tol_min_step_norm = *tol_min_step_norm; - } - else if (!strcmp(field, "warm_start_first_qp")) - { - bool* warm_start_first_qp = (bool *) value; - opts->warm_start_first_qp = *warm_start_first_qp; - } - else if (!strcmp(field, "warm_start_first_qp_from_nlp")) - { - bool* warm_start_first_qp_from_nlp = (bool *) value; - opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; - } - else if (!strcmp(field, "eval_residual_at_max_iter")) - { - bool* eval_residual_at_max_iter = (bool *) value; - opts->eval_residual_at_max_iter = *eval_residual_at_max_iter; - } - else if (!strcmp(field, "use_constraint_hessian_in_feas_qp")) + if (!strcmp(field, "use_constraint_hessian_in_feas_qp")) { bool* use_constraint_hessian_in_feas_qp = (bool *) value; opts->use_constraint_hessian_in_feas_qp = *use_constraint_hessian_in_feas_qp; @@ -570,12 +506,14 @@ void ocp_nlp_sqp_wfqp_work_get(void *config_, void *dims_, void *work_, static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_res, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_opts *opts) { + ocp_nlp_opts *nlp_opts = opts->nlp_opts; + // check for nans if (isnan(nlp_res->inf_norm_res_stat) || isnan(nlp_res->inf_norm_res_eq) || isnan(nlp_res->inf_norm_res_ineq) || isnan(nlp_res->inf_norm_res_comp)) { mem->nlp_mem->status = ACADOS_NAN_DETECTED; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: NaN detected in iterate.\n"); } @@ -583,10 +521,10 @@ 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->nlp_opts->max_iter) + if (!nlp_opts->eval_residual_at_max_iter && n_iter >= nlp_opts->max_iter) { mem->nlp_mem->status = ACADOS_MAXITER; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Maximum iterations Reached.\n"); } @@ -594,13 +532,13 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check if solved to tolerance - if ((nlp_res->inf_norm_res_stat < opts->tol_stat) && - (nlp_res->inf_norm_res_eq < opts->tol_eq) && - (nlp_res->inf_norm_res_ineq < opts->tol_ineq) && - (nlp_res->inf_norm_res_comp < opts->tol_comp)) + if ((nlp_res->inf_norm_res_stat < nlp_opts->tol_stat) && + (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq) && + (nlp_res->inf_norm_res_ineq < nlp_opts->tol_ineq) && + (nlp_res->inf_norm_res_comp < nlp_opts->tol_comp)) { mem->nlp_mem->status = ACADOS_SUCCESS; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Optimal solution found! Converged to KKT point.\n"); } @@ -608,11 +546,11 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for small step - if (opts->tol_min_step_norm > 0.0 && (n_iter > 0) && (mem->step_norm < opts->tol_min_step_norm)) + if (nlp_opts->tol_min_step_norm > 0.0 && (n_iter > 0) && (mem->step_norm < nlp_opts->tol_min_step_norm)) { - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { - if (nlp_res->inf_norm_res_eq < opts->tol_eq && nlp_res->inf_norm_res_ineq < opts->tol_ineq) + if (nlp_res->inf_norm_res_eq < nlp_opts->tol_eq && nlp_res->inf_norm_res_ineq < nlp_opts->tol_ineq) { printf("Stopped: Converged to feasible point. Step size is < tol_eq.\n"); } @@ -626,10 +564,10 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for unbounded problem - if (mem->nlp_mem->cost_value <= opts->tol_unbounded) + if (mem->nlp_mem->cost_value <= nlp_opts->tol_unbounded) { mem->nlp_mem->status = ACADOS_UNBOUNDED; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Problem seems to be unbounded.\n"); } @@ -637,10 +575,10 @@ static bool check_termination(int n_iter, ocp_nlp_dims *dims, ocp_nlp_res *nlp_r } // check for maximum iterations - if (n_iter >= opts->nlp_opts->max_iter) + if (n_iter >= nlp_opts->max_iter) { mem->nlp_mem->status = ACADOS_MAXITER; - if (opts->nlp_opts->print_level > 0) + if (nlp_opts->print_level > 0) { printf("Stopped: Maximum iterations reached.\n"); } @@ -718,7 +656,7 @@ static double calculate_pred_l1_inf(ocp_nlp_sqp_wfqp_opts* opts, ocp_nlp_sqp_wfq } else { - if (mem->l1_infeasibility < fmin(opts->tol_ineq, opts->tol_eq)) + if (mem->l1_infeasibility < fmin(opts->nlp_opts->tol_ineq, opts->nlp_opts->tol_eq)) { return 0.0; } @@ -729,8 +667,6 @@ static double calculate_pred_l1_inf(ocp_nlp_sqp_wfqp_opts* opts, ocp_nlp_sqp_wfq } } - - /* Calculates the QP l1 infeasibility by summing up the additional slack variables included in the QP. @@ -760,7 +696,7 @@ static double calculate_qp_l1_infeasibility_from_slacks(ocp_nlp_dims *dims, ocp_ l1_inf += fmax(0.0, tmp2); } } - assert(l1_inf > -opts->tol_ineq); + assert(l1_inf > -opts->nlp_opts->tol_ineq); return l1_inf; } @@ -846,7 +782,7 @@ static double calculate_qp_l1_infeasibility_manually(ocp_nlp_dims *dims, ocp_nlp } } } - assert(l1_inf > -opts->tol_ineq); + assert(l1_inf > -opts->nlp_opts->tol_ineq); return l1_inf; } @@ -860,7 +796,7 @@ static double calculate_qp_l1_infeasibility(ocp_nlp_dims *dims, ocp_nlp_sqp_wfqp // this is only possible if directly after a QP was solved if (opts->use_QP_l1_inf_from_slacks) { - // seems to be inaccurate. Results are worse! + // Inaccurate if QP solver tolerance is low! l1_inf = calculate_qp_l1_infeasibility_from_slacks(dims, mem, opts, qp_out); } else @@ -979,13 +915,13 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o // warm start of first QP if (nlp_mem->iter == 0) { - if (!opts->warm_start_first_qp) + if (!nlp_opts->warm_start_first_qp) { // (typically) no warm start at first iteration int tmp_int = 0; qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &tmp_int); } - else if (opts->warm_start_first_qp_from_nlp) + else if (nlp_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); @@ -1527,7 +1463,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); } - if (l1_inf_QP_feasibility/(fmax(1.0, (double) mem->absolute_nns)) < opts->tol_ineq) + if (l1_inf_QP_feasibility/(fmax(1.0, (double) mem->absolute_nns)) < nlp_opts->tol_ineq) { mem->watchdog_zero_slacks_counter += 1; } @@ -1627,7 +1563,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // 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 (nlp_mem->iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->iter != opts->nlp_opts->max_iter || nlp_opts->eval_residual_at_max_iter) { // store current iterate if (nlp_opts->store_iterates) @@ -1722,7 +1658,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Compute the step norm - if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) + if (nlp_opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nominal_qp_out); if (nlp_opts->log_primal_step_norm) diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index 7af41d1d2a..e036fab389 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -54,18 +54,8 @@ extern "C" { typedef struct { ocp_nlp_opts *nlp_opts; - double tol_stat; - double tol_eq; - double tol_ineq; - double tol_comp; - double tol_unbounded; // exit threshold when objective function seems to be unbounded - double tol_min_step_norm; // exit tolerance for small step bool log_pi_norm_inf; // compute and log the max norm of the pi multipliers bool log_lam_norm_inf; // compute and log the max norm of the lam multipliers - bool warm_start_first_qp; - 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 - bool use_constraint_hessian_in_feas_qp; // Either use exact Hessian or identity matrix in feasibility QP bool use_QP_l1_inf_from_slacks; // True: sums up the slack variable values from qp_out; False: compute manually; Should give the same result. int search_direction_mode; // determines how the QPs should be solved From bd253c6bb7eb7e4472fad36d12cb7044353a08ad Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 26 Jun 2025 14:06:28 +0200 Subject: [PATCH 081/164] Developer checks (#1562) Introduced new CMake compiler flag `ACADOS_DEVELOPER_DEBUG_CHECKS`. Enables the introduction of sanity checks without the use of asserts. As a one, we added a check for nonnegativity of slack variables after every step update. --------- Co-authored-by: d.kiessling --- CMakeLists.txt | 7 ++++ acados/ocp_nlp/ocp_nlp_common.c | 38 ++++++++++++++++--- acados/ocp_nlp/ocp_nlp_ddp.c | 2 - acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 16 +++++++- docs/installation/index.md | 1 + 5 files changed, 55 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d752c3642a..9cc0969016 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ endif() option(ACADOS_WITH_OPENMP "OpenMP Parallelization" OFF) option(ACADOS_SILENT "No console status output" OFF) option(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE "Print QP inputs and outputs to file in SQP" OFF) +option(ACADOS_DEVELOPER_DEBUG_CHECKS "Enable developer debug sanity checks. Avoids asserts" OFF) # Additional targets option(ACADOS_UNIT_TESTS "Compile Unit tests" OFF) @@ -192,6 +193,12 @@ if(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE") endif() +if(ACADOS_DEVELOPER_DEBUG_CHECKS) + message(STATUS "ACADOS_DEVELOPER_DEBUG_CHECKS is ON") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DACADOS_DEVELOPER_DEBUG_CHECKS") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DACADOS_DEVELOPER_DEBUG_CHECKS") +endif() + # uninstall if(NOT TARGET uninstall) # Configure Uninstall diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1b9e9dc8a3..f2c0827a4f 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -3156,6 +3156,26 @@ void ocp_nlp_level_c_update(ocp_nlp_config *config, // - adjoint call for inequalities as for dynamics } +#if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) +static void sanity_check_nlp_slack_nonnegativity(ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_out *out) +{ + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + for (int i = 0; i <= N; i++) + { + for (int jj = 0; jj < 2*ns[i]; jj++) + { + if (BLASFEO_DVECEL(out->ux+i, nx[i]+nu[i]+jj) < -opts->tol_ineq) + { + printf("found slack value %e < 0 at i=%d j=%d\n", BLASFEO_DVECEL(out->ux+i, nx[i]+nu[i]+jj), i, jj); + exit(1); + } + } + } +} +#endif /* calculates new iterate or trial iterate in 'out_destination' with step 'mem->qp_out', @@ -3179,9 +3199,9 @@ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, int *ni = dims->ni; int *nz = dims->nz; -#if defined(ACADOS_WITH_OPENMP) + #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for -#endif + #endif for (int i = 0; i <= N; i++) { // step in primal variables @@ -3215,9 +3235,14 @@ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, { // out->z = mem->z_alg + alpha * dzdux * qp_out->ux blasfeo_dgemv_t(nu[i]+nx[i], nz[i], alpha, mem->dzduxt+i, 0, 0, - mem->qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); + mem->qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); + } } - } +#if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) + ocp_nlp_opts *opts = opts_; + sanity_check_nlp_slack_nonnegativity(dims, opts, out_destination); +#endif + } void ocp_nlp_initialize_qp_from_nlp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_qp_in *qp_in, @@ -3284,7 +3309,6 @@ void ocp_nlp_update_variables_sqp_delta_primal_dual(ocp_nlp_config *config, ocp_ int *ni = dims->ni; int *nz = dims->nz; - #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for #endif @@ -3304,6 +3328,10 @@ void ocp_nlp_update_variables_sqp_delta_primal_dual(ocp_nlp_config *config, ocp_ step->ux+i, 0, 1.0, mem->z_alg+i, 0, out->z+i, 0); } } +#if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) + sanity_check_nlp_slack_nonnegativity(dims, opts, out); +#endif + } diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index 8b1fa76645..1cd68cb1c0 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -104,8 +104,6 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_ ocp_nlp_ddp_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // this first !!! ocp_nlp_opts_initialize_default(config, dims, nlp_opts); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 1220e90a6b..f5483c2f20 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -696,7 +696,13 @@ static double calculate_qp_l1_infeasibility_from_slacks(ocp_nlp_dims *dims, ocp_ l1_inf += fmax(0.0, tmp2); } } - assert(l1_inf > -opts->nlp_opts->tol_ineq); +#if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) + if (l1_inf < -opts->nlp_opts->tol_ineq) + { + printf("calculate_qp_l1_infeasibility_from_slacks: got negative l1 infeasibility!\n"); + exit(1); + } +#endif return l1_inf; } @@ -782,7 +788,13 @@ static double calculate_qp_l1_infeasibility_manually(ocp_nlp_dims *dims, ocp_nlp } } } - assert(l1_inf > -opts->nlp_opts->tol_ineq); +#if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) + if (l1_inf < -opts->nlp_opts->tol_ineq) + { + printf("calculate_qp_l1_infeasibility_manually: got negative l1 infeasibility!\n"); + exit(1); + } +#endif return l1_inf; } diff --git a/docs/installation/index.md b/docs/installation/index.md index 35b051bf03..497842dd9c 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -50,6 +50,7 @@ Adjust these options based on your requirements. | `ACADOS_NUM_THREADS` | Number of threads for OpenMP parallelization within one NLP solver. If not set, `omp_get_max_threads` will be used to determine the number of threads. If multiple solves should be parallelized, e.g. with an `AcadosOcpBatchSolver` or `AcadosSimBatchSolver`, set this to 1. | Not set | | `ACADOS_SILENT` | No console status output | `OFF` | | `ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE` | Print QP inputs and outputs to file in SQP | `OFF` | +| `ACADOS_DEVELOPER_DEBUG_CHECKS` | Enable developer debug checks | `OFF` | | `CMAKE_BUILD_TYPE` | Build type (e.g., Release, Debug, etc.) | `Release` | | `ACADOS_UNIT_TESTS` | Compile unit tests | `OFF` | | `ACADOS_EXAMPLES` | Compile C examples | `OFF` | From f0fc14da5d062c934a5d71389dd395fcd142db35 Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Fri, 27 Jun 2025 09:48:58 +0200 Subject: [PATCH 082/164] Bug fix in `ocp_nlp_compute_dual_lam_norm_inf` (#1566) Bug fix in calculation of inf norm of lam. The for loop should go until N, i.e., `<=N`. --- acados/ocp_nlp/ocp_nlp_common.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index f2c0827a4f..f65f9bc66d 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -2599,7 +2599,7 @@ double ocp_nlp_compute_dual_lam_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_ou double norm_lam = 0.0; // compute inf norm of lam - for (i = 0; i < N; i++) + for (i = 0; i <= N; i++) { for (j=0; j<2*dims->ni[i]; j++) { From 3acc2d13f5231d42b61dd64c2826e871b924e7d8 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Sun, 29 Jun 2025 16:50:12 +0200 Subject: [PATCH 083/164] MATLAB: add functionalty to assess constraint violation and residuals in detail. (#1558) - add `AcadosOcpSolver.load_iterate_from_obj()` - add getter fields `res_eq_all`, `res_stat_all` - add function `AcadosOcpSolver.evaluate_constraints_and_get_violation()`: returns the constraint violations for all stages in a cell array. Values > 0 indicate a violation of the constraints. - add function `AcadosOcpSolver.get_constraint_indices_with_violation(tol)`: computes the indices of the constraints that are violated with respect to the given tolerance, in the form `[stage_index_0, constraint_index_0, stage_index_1, constraint_index_1, ....]` all indices are zero -based. --- .github/workflows/full_build_windows.yml | 2 +- acados/ocp_nlp/ocp_nlp_common.c | 48 ++- acados/ocp_nlp/ocp_nlp_common.h | 5 + .../acados_matlab_octave/generic_nlp/main.m | 10 +- .../getting_started/extensive_example_ocp.m | 2 +- .../example_ocp_custom_hess.m | 4 +- .../nonlinear_constraint_test.m | 352 ++++++++++++++++++ .../test/run_matlab_examples_new_casadi.m | 1 + .../convex_problem_globalization_necessary.py | 2 +- .../linear_mass_test_problem.py | 9 +- interfaces/acados_c/ocp_nlp_interface.c | 29 ++ interfaces/acados_c/ocp_nlp_interface.h | 3 + .../acados_matlab_octave/AcadosOcpSolver.m | 45 +++ .../ocp_compile_interface.m | 1 - interfaces/acados_matlab_octave/ocp_get.c | 77 +++- .../acados_matlab_octave/ocp_get_cost.c | 67 ---- .../matlab_templates/mex_solver.in.m | 6 +- 17 files changed, 574 insertions(+), 89 deletions(-) create mode 100644 examples/acados_matlab_octave/pendulum_on_cart_model/nonlinear_constraint_test.m delete mode 100644 interfaces/acados_matlab_octave/ocp_get_cost.c diff --git a/.github/workflows/full_build_windows.yml b/.github/workflows/full_build_windows.yml index eafc14f406..2f67b2e66d 100644 --- a/.github/workflows/full_build_windows.yml +++ b/.github/workflows/full_build_windows.yml @@ -14,7 +14,7 @@ env: ACADOS_ON_CI: ON jobs: - core_build: + test_matlab_windows: runs-on: windows-latest steps: diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index f65f9bc66d..4030ad389d 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -3021,7 +3021,7 @@ void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, config->constraints[i]->update_qp_vectors(config->constraints[i], dims->constraints[i], in->constraints[i], opts->constraints[i], mem->constraints[i], work->constraints[i]); - // copy ineq function value into nlp mem, then into QP + // copy ineq function value into mem, then into QP struct blasfeo_dvec *ineq_fun = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); blasfeo_dveccp(2 * ni[i], ineq_fun, 0, mem->ineq_fun + i, 0); @@ -3774,6 +3774,22 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in } +void ocp_nlp_eval_constraints_common(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 N = dims->N; + + for (int i = 0; i <= N; i++) + { + config->constraints[i]->compute_fun(config->constraints[i], dims->constraints[i], + in->constraints[i], opts->constraints[i], + mem->constraints[i], work->constraints[i]); + // copy ineq function value into mem + struct blasfeo_dvec *ineq_fun = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); + blasfeo_dveccp(2 * dims->ni[i], ineq_fun, 0, mem->ineq_fun + i, 0); + } +} + 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) @@ -4493,6 +4509,36 @@ void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const c } +void ocp_nlp_memory_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_memory *nlp_mem, int stage, const char *field, void *return_value_) +{ + // int *nb = dims->nb; + // int *ng = dims->ng; + int *ni = dims->ni; + int *nv = dims->nv; + int *nx = dims->nx; + // int *ni_nl = dims->ni_nl; + if (!strcmp("ineq_fun", field)) + { + double *value = return_value_; + blasfeo_unpack_dvec(2 * ni[stage], nlp_mem->ineq_fun + stage, 0, value, 1); + } + else if (!strcmp("res_stat", field)) + { + double *value = return_value_; + blasfeo_unpack_dvec(nv[stage], nlp_mem->nlp_res->res_stat + stage, 0, value, 1); + } + else if (!strcmp("res_eq", field)) + { + double *value = return_value_; + blasfeo_unpack_dvec(nx[stage+1], nlp_mem->nlp_res->res_eq + stage, 0, value, 1); + } + else + { + printf("\nerror: field %s not available in ocp_nlp_memory_get_at_stage\n", field); + exit(1); + } +} + void ocp_nlp_timings_reset(ocp_nlp_timings *timings) { timings->time_qp_sol = 0.0; diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 1f02190ef9..a573bd514b 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -503,6 +503,8 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims ocp_nlp_opts *opts, ocp_nlp_in *in, void *raw_memory); // void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const char *field, void *return_value_); +// +void ocp_nlp_memory_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_memory *nlp_mem, int stage, const char *field, void *return_value_); /************************************************ * workspace @@ -605,6 +607,9 @@ void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to); void ocp_nlp_cost_compute(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); // +void ocp_nlp_eval_constraints_common(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); +// 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); // diff --git a/examples/acados_matlab_octave/generic_nlp/main.m b/examples/acados_matlab_octave/generic_nlp/main.m index 926497bcc8..f6dccb36ae 100644 --- a/examples/acados_matlab_octave/generic_nlp/main.m +++ b/examples/acados_matlab_octave/generic_nlp/main.m @@ -68,7 +68,7 @@ %% solver options ocp.solver_options.N_horizon = 0; ocp.solver_options.nlp_solver_type = 'SQP'; -ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'; %TODO: Change after PARTIAL_CONDENSING_HPIPM fix +ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; %% create the solver ocp_solver = AcadosOcpSolver(ocp); @@ -85,7 +85,10 @@ % solve and time tic ocp_solver.solve(); -total_time = toc; +time_external = toc; +% internal timing +total_time = ocp_solver.get('time_tot'); + % check status status = ocp_solver.get('status'); @@ -97,7 +100,8 @@ x_opt = ocp_solver.get('x', 0); disp('Optimal solution:') % should be [1;1] for p = [1;1] disp(x_opt) -disp(['Total time: ', num2str(1e3*total_time), ' ms']) +disp(['Total time (internal): ', num2str(1e3*total_time), ' ms']) +disp(['Total time (external): ', num2str(1e3*time_external), ' ms']) % compare with the expected solution if all(p_value == [1;1]) 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 443abfc937..cfcdea4848 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -304,7 +304,7 @@ % get second SQP iterate % iteration index is 0-based with iterate 0 corresponding to the initial guess -iteration = 0; +iteration = 1; iterate = ocp_solver.get_iterate(iteration); iterates = ocp_solver.get_iterates(); x_traj = iterates.as_array('x'); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m index 54e1fa312c..ee69ff8726 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m @@ -95,7 +95,9 @@ D(2, 2) = if_else(theta == 0, hess_tan, grad_tan / theta); custom_hess_x = J' * D * J; - +if is_octave() + error("This example does not work in Octave, somehow blkdiag doesn't work with symbolic matrices. If you happen to know how to fix this, please open a pull request."); +end cost_expr_ext_cost_custom_hess = blkdiag(custom_hess_u, custom_hess_x); cost_expr_ext_cost_custom_hess_e = custom_hess_x; diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/nonlinear_constraint_test.m b/examples/acados_matlab_octave/pendulum_on_cart_model/nonlinear_constraint_test.m new file mode 100644 index 0000000000..e8699a2055 --- /dev/null +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/nonlinear_constraint_test.m @@ -0,0 +1,352 @@ +% +% 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.; + + +clear all; clc; + +check_acados_requirements() + +%% OCP DESCRIPTION +ocp = AcadosOcp(); + +%% SOLVER OPTIONS + +%% discretization +N = 10; % 40 +T = 2.0; % time horizon length +h = T/N; + +% uniform time grid +time_steps = T/N * ones(N,1); + +shooting_nodes = zeros(N+1, 1); +for i = 1:N + shooting_nodes(i+1) = sum(time_steps(1:i)); +end + +ocp.solver_options.tf = T; +ocp.solver_options.N_horizon = N; +ocp.solver_options.time_steps = time_steps; +ocp.solver_options.nlp_solver_type = 'SQP'; % 'SQP_RTI' +ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; % 'EXACT' +ocp.solver_options.regularize_method = 'CONVEXIFY'; +% NO_REGULARIZE, PROJECT, PROOJECT_REDUC_HESS, MIRROR, CONVEXIFY +ocp.solver_options.nlp_solver_max_iter = 50; +ocp.solver_options.nlp_solver_tol_stat = 1e-8; +ocp.solver_options.nlp_solver_tol_eq = 1e-8; +ocp.solver_options.nlp_solver_tol_ineq = 1e-8; +ocp.solver_options.nlp_solver_tol_comp = 1e-8; +ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; +ocp.solver_options.qp_solver_cond_N = 5; % for partial condensing +ocp.solver_options.qp_solver_cond_ric_alg = 0; +ocp.solver_options.qp_solver_ric_alg = 0; +ocp.solver_options.qp_solver_warm_start = 1; % 0: cold, 1: warm, 2: hot +ocp.solver_options.qp_solver_iter_max = 1000; % default is 50; OSQP needs a lot sometimes. +ocp.solver_options.qp_solver_mu0 = 1e4; +ocp.solver_options.print_level = 1; +ocp.solver_options.store_iterates = true; +ocp.solver_options.log_dual_step_norm = true; +ocp.solver_options.log_primal_step_norm = true; + +% can vary for integrators +sim_method_num_stages = 1 * ones(N,1); +sim_method_num_stages(3:end) = 2; +ocp.solver_options.sim_method_num_stages = sim_method_num_stages; +ocp.solver_options.sim_method_num_steps = ones(N,1); + +% integrator type +integrator = 1; +switch integrator +case 1 + ocp.solver_options.integrator_type = 'ERK'; +case 2 + ocp.solver_options.integrator_type = 'IRK'; +case 3 + if ~all(time_steps == T/N) + error('nonuniform time discretization with discrete dynamics should not be used'); + end + ocp.solver_options.integrator_type = 'DISCRETE'; +otherwise + ocp.solver_options.integrator_type = 'GNSF'; +end + +%% MODEL +model = get_pendulum_on_cart_model(); +ocp.model = model; + +% dimensions +nx = model.x.rows(); +nu = model.u.rows(); + + +%% COST +cost_formulation = 1; +switch cost_formulation +case 1 + cost_type = 'LINEAR_LS'; +case 2 + cost_type = 'EXTERNAL'; +otherwise + cost_type = 'AUTO'; +end + +ocp.cost.cost_type_0 = cost_type; +ocp.cost.cost_type = cost_type; +ocp.cost.cost_type_e = cost_type; + +W_x = diag([1e3, 1e3, 1e-2, 1e-2]); +W_u = 1e-2; + +cost_expr_ext_cost_e = 0.5 * model.x'* W_x * model.x; +cost_expr_ext_cost = cost_expr_ext_cost_e + 0.5 * model.u' * W_u * model.u; +cost_expr_ext_cost_0 = 0.5 * model.u' * W_u * model.u; + +ny_0 = nu; % number of outputs in initial cost term +Vx_0 = zeros(ny_0,nx); +Vu_0 = eye(nu); +y_ref_0 = zeros(ny_0, 1); + +ny = nx+nu; % number of outputs in lagrange term +Vx = [eye(nx); zeros(nu,nx)]; % state-to-output matrix in lagrange term +Vu = [zeros(nx, nu); eye(nu)]; % input-to-output matrix in lagrange term +y_ref = zeros(ny, 1); % output reference in lagrange term + +ny_e = nx; % number of outputs in terminal cost term +Vx_e = eye(ny_e, nx); +y_ref_e = zeros(ny_e, 1); + +if strcmp(cost_type, 'LINEAR_LS') + ocp.cost.Vu_0 = Vu_0; + ocp.cost.Vx_0 = Vx_0; + ocp.cost.W_0 = W_u; + ocp.cost.yref_0 = y_ref_0; + + ocp.cost.Vu = Vu; + ocp.cost.Vx = Vx; + ocp.cost.W = blkdiag(W_x, W_u); + ocp.cost.yref = y_ref; + + ocp.cost.Vx_e = Vx_e; + ocp.cost.W_e = W_x; + ocp.cost.yref_e = y_ref_e; +else % EXTERNAL, AUTO + ocp.model.cost_expr_ext_cost_0 = cost_expr_ext_cost_0; + ocp.model.cost_expr_ext_cost = cost_expr_ext_cost; + ocp.model.cost_expr_ext_cost_e = cost_expr_ext_cost_e; +end + +%% CONSTRAINTS +constraint_formulation_nonlinear = 0; +lbu = -80*ones(nu, 1); +ubu = 80*ones(nu, 1); + + +% bound on u +ocp.constraints.idxbu = [0]; +ocp.constraints.lbu = lbu; +ocp.constraints.ubu = ubu; + +ocp.constraints.idxbx = [0]; +ocp.constraints.lbx = -3; +ocp.constraints.ubx = 3; + +% nonlinear constraint +infty = get_acados_infty(); +ocp.model.con_h_expr = model.u*(model.x(1)+3.5); +ocp.constraints.lh = -infty; +% ocp.constraints.lh = lbu; +ocp.constraints.uh = ubu; + + +% initial state +x0 = [0; pi; 0; 0]; +ocp.constraints.x0 = x0; + +%% SOLVER +ocp_solver = AcadosOcpSolver(ocp); + +%% INITIALIZATION +x_traj_init = zeros(nx, N+1); +x_traj_init(2, :) = linspace(pi, 0, N+1); % initialize theta + +u_traj_init = zeros(nu, N); + +%% SOLVE +% prepare evaluation +n_executions = 1; +time_tot = zeros(n_executions,1); +time_lin = zeros(n_executions,1); +time_reg = zeros(n_executions,1); +time_qp_sol = zeros(n_executions,1); + +%% call ocp solver in loop +for i=1:n_executions + % initial state + ocp_solver.set('constr_x0', x0); + + % set trajectory initialization + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints + + % solve + ocp_solver.solve(); + % get solution + utraj = ocp_solver.get('u'); + xtraj = ocp_solver.get('x'); + + % evaluation + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot(i) = ocp_solver.get('time_tot'); + time_lin(i) = ocp_solver.get('time_lin'); + time_reg(i) = ocp_solver.get('time_reg'); + time_qp_sol(i) = ocp_solver.get('time_qp_sol'); + + if i == 1 || i == n_executions + ocp_solver.print('stat'); + end +end + +% test constraint evaluation +stat_mat = ocp_solver.get('stat'); +res_ineq = stat_mat(:, 4); +flag_test = 0; +for iteration = 1:3 + % load iterate + iterate = ocp_solver.get_iterate(iteration); + ocp_solver.load_iterate_from_obj(iterate); + + % expected infeasibility + ineq_res_as_iter = res_ineq(iteration+1); + + % evaluate constraints + ineq_fun = ocp_solver.evaluate_constraints_and_get_violation(); + violations = zeros(N+1, 1); + for i=1:length(ineq_fun) + violations(i) = max([ineq_fun{i}; 0]); + end + for i=2:N + if ineq_fun{i}(3) ~= 0.0 % 3 is lh index + error('inequality value corresponding to masked constraint should be 0.0.'); + end + end + [max_violation, index] = max(violations); + if abs(max_violation - ineq_res_as_iter) > 1e-6 + error('inequality constraint violation does not match expected value'); + else + fprintf('inequality constraint violation matches expected value: %f\n', max_violation); + end + violation_idx = ocp_solver.get_constraint_indices_with_violation(max_violation); + if max_violation ~= 0.0 + if ~isequal(size(violation_idx), [1, 2]) + error('expected violation index to be of size [1, 2], got %d, %d', size(violation_idx)); + end + if ineq_fun{violation_idx(1)+1}(violation_idx(2)+1) ~= max_violation + error('inequality constraint violation does not match expected value'); + else + fprintf('max. constraint violation index correctly identified as: %d %d\n', violation_idx(1), violation_idx(2)); + end + flag_test = 1; + end +end + +if ~flag_test + error('constraint violation index test was not done'); +end + +% test res_stat getter +res_stat_all = ocp_solver.get('res_stat_all'); +res_stat_norm = stat_mat(end, 2); +max_res_stat = zeros(1, length(res_stat_all)); +if length(res_stat_all) ~= N+1 + error('length of res_stat_all does not match N+1'); +end +flag_test = 0; +for i=1:length(res_stat_all) + max_res_stat(i) = max(res_stat_all{i}); + if max_res_stat(i) > res_stat_norm + error('max res_stat is larger than res_stat_norm'); + elseif max_res_stat(i) == res_stat_norm + flag_test = 1; + end +end + +if ~flag_test + error('did not find max res_stat equal to res_stat_norm'); +else + disp('Found max res_stat equal to res_stat_norm'); +end + +% test res_eq getter +res_eq_all = ocp_solver.get('res_eq_all'); +res_eq_norm = stat_mat(end, 3); +max_res_eq = zeros(1, length(res_eq_all)); +if length(res_eq_all) ~= N + error('length of res_eq_all does not match N'); +end +flag_test = 0; +for i=1:length(res_eq_all) + max_res_eq(i) = max(res_eq_all{i}); + if max_res_eq(i) > res_eq_norm + error('max res_eq is larger than res_eq_norm'); + elseif max_res_eq(i) == res_eq_norm + flag_test = 1; + end +end + +if ~flag_test + error('did not find max res_eq equal to res_eq_norm'); +else + disp('Found max res_eq equal to res_eq_norm'); +end + + + +%% Plot trajectories +if 0 + figure; hold on; + States = {'p', 'theta', 'v', 'dtheta'}; + for i=1:length(States) + subplot(length(States), 1, i); + plot(shooting_nodes, xtraj(i,:)); grid on; + ylabel(States{i}); + xlabel('t [s]') + end + + figure + stairs(shooting_nodes, [utraj'; utraj(end)]) + + ylabel('F [N]') + xlabel('t [s]') + grid on + if is_octave() + waitforbuttonpress; + end +end diff --git a/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m b/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m index 36cd6c7698..1ea7ae58f5 100644 --- a/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m +++ b/examples/acados_matlab_octave/test/run_matlab_examples_new_casadi.m @@ -38,6 +38,7 @@ '../p_global_example/main.m'; '../p_global_example/simulink_test_p_global.m'; '../mocp_transition_example/main_parametric_mocp.m'; + '../pendulum_on_cart_model/nonlinear_constraint_test.m'; '../dense_nlp/test_qpscaling.m'; }; diff --git a/examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py b/examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py index 7c02db3625..bc1d346e1d 100644 --- a/examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py +++ b/examples/acados_python/convex_problem_globalization_needed/convex_problem_globalization_necessary.py @@ -61,7 +61,7 @@ def solve_problem_with_setting(setting): ocp.solver_options.qp_tol = 5e-7 ocp.solver_options.nlp_solver_max_iter = 100 - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json', verbose=False) # initialize solver xinit = np.array([1.5]) diff --git a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py index 396b8cef81..4b07aa3651 100644 --- a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py +++ b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py @@ -198,12 +198,11 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): ocp.solver_options.qp_solver_tol_ineq = qp_tol ocp.solver_options.qp_solver_tol_comp = qp_tol ocp.solver_options.qp_solver_ric_alg = 1 - # ocp.solver_options.qp_solver_cond_ric_alg = 1 # set prediction horizon ocp.solver_options.tf = Tf - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_ocp.json') + ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_ocp.json', verbose=False) if globalization == "FUNNEL_L1PEN_LINESEARCH": # Test the options setters @@ -217,8 +216,6 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): ocp_solver.options_set('globalization_line_search_use_sufficient_descent', globalization_line_search_use_sufficient_descent) ocp_solver.options_set('globalization_use_SOC', globalization_use_SOC) ocp_solver.options_set('globalization_full_step_dual', 1) - else: - ocp if INITIALIZE:# initialize solver # [ocp_solver.set(i, "x", x0 + (i/N) * (x_goal-x0)) for i in range(N+1)] @@ -237,8 +234,6 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): simX = np.array([ocp_solver.get(i,"x") for i in range(N+1)]) simU = np.array([ocp_solver.get(i,"u") for i in range(N)]) pi_multiplier = [ocp_solver.get(i, "pi") for i in range(N)] - print(f"cost function value = {ocp_solver.get_cost()}") - # print summary print(f"solved Maratos test problem with settings {setting}") @@ -261,8 +256,6 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): if PLOT: plot_linear_mass_system_X_state_space(simX, circle=circle, x_goal=x_goal) plot_linear_mass_system_U(shooting_nodes, simU) - # plot_linear_mass_system_X(shooting_nodes, simX) - print(f"\n\n----------------------\n") if __name__ == '__main__': diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 49a4c871c0..2d53038a15 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -820,6 +820,10 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n { return dims->nx[stage+1]; } + else if (!strcmp(field, "nv")) + { + return dims->nv[stage]; + } else if (!strcmp(field, "u") || !strcmp(field, "nu")) { return dims->nu[stage]; @@ -953,6 +957,12 @@ void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims "nh", &dims_out[0]); return; } + else if (!strcmp(field, "ineq_fun")) + { + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "ni", &dims_out[0]); + dims_out[0] *= 2; + } // matrices else if (!strcmp(field, "C")) { @@ -1371,6 +1381,21 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out * ocp_nlp_cost_compute(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } +void ocp_nlp_eval_constraints(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out) +{ + ocp_nlp_config *config = solver->config; + ocp_nlp_memory *nlp_mem; + ocp_nlp_opts *nlp_opts; + ocp_nlp_workspace *nlp_work; + ocp_nlp_dims *dims = solver->dims; + + config->get(config, solver->dims, solver->mem, "nlp_mem", &nlp_mem); + config->opts_get(config, solver->dims, solver->opts, "nlp_opts", &nlp_opts); + config->work_get(config, solver->dims, solver->work, "nlp_work", &nlp_work); + + ocp_nlp_eval_constraints_common(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); +} + void ocp_nlp_eval_params_jac(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out) { @@ -1564,6 +1589,10 @@ void ocp_nlp_get_at_stage(ocp_nlp_solver *solver, int stage, const char *field, } xcond_solver_config->solver_get(xcond_solver_config, nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, field, stage, value, size1, size2); } + else if (!strcmp(field, "ineq_fun") || !strcmp(field, "res_stat") || !strcmp(field, "res_eq")) + { + ocp_nlp_memory_get_at_stage(config, dims, nlp_mem, stage, field, value); + } else if (!strcmp(field, "pcond_Q")) { ocp_qp_in *pcond_qp_in; diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 066e0d4325..d78b89e9bc 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -420,6 +420,9 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in * ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); +ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_constraints(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + /// Computes jacobian wrt params in all modules (preparation for solution sensitivities). /// /// \param solver The solver struct. diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 9a30e59d0e..fe3af9a56c 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -171,6 +171,29 @@ function eval_param_sens(obj, field, stage, index) value = obj.t_ocp.get_cost(); end + function value = evaluate_constraints_and_get_violation(obj) + % returns the constraint violations for all stages in a cell array. + % values > 0 indicate a violation of the constraints. + value = obj.t_ocp.evaluate_constraints_and_get_violation(); + end + + function violation_idx = get_constraint_indices_with_violation(obj, tol) + % computes the indices of the constraints that are violated with respect to the given tolerance, in the form + % `[stage_index_0, constraint_index_0, + % stage_index_1, constraint_index_1, + % ....] + % all indices are zero -based. + ineq_fun = obj.evaluate_constraints_and_get_violation(); + if nargin < 2 + tol = obj.solver_options.nlp_solver_tol_ineq; + end + violation_idx = []; + for i=1:length(ineq_fun) + idx_i = find(ineq_fun{i} >= tol); + violation_idx = [violation_idx; [(i-1)*ones(length(idx_i), 1), idx_i-1]]; + end + end + function set(obj, field, value, varargin) obj.t_ocp.set(field, value, varargin{:}); end @@ -266,6 +289,28 @@ function set(obj, field, value, varargin) obj.t_ocp.load_iterate(filename); end + function [] = load_iterate_from_obj(obj, iterate) + %%% Loads the iterate from an AcadosOcpIterate object. + %%% param1: iterate: AcadosOcpIterate object containing the iterate to load + + if ~isa(iterate, 'AcadosOcpIterate') + error('load_iterate_from_obj: iterate needs to be of type AcadosOcpIterate'); + end + + for i = 1:obj.solver_options.N_horizon + 1 + obj.t_ocp.set('x', iterate.x_traj{i, 1}, i-1); + obj.t_ocp.set('sl', iterate.sl_traj{i, 1}, i-1); + obj.t_ocp.set('su', iterate.su_traj{i, 1}, i-1); + obj.t_ocp.set('lam', iterate.lam_traj{i, 1}, i-1); + end + for i = 1:obj.solver_options.N_horizon + obj.t_ocp.set('u', iterate.u_traj{i, 1}, i-1); + obj.t_ocp.set('pi', iterate.pi_traj{i, 1}, i-1); + if ~isempty(iterate.z_traj{i, 1}) + obj.t_ocp.set('z', iterate.z_traj{i, 1}, i-1); + end + end + end function iterate = get_iterate(obj, iteration) if iteration > obj.get('nlp_iter') error("iteration needs to be nonnegative and <= nlp_iter."); diff --git a/interfaces/acados_matlab_octave/ocp_compile_interface.m b/interfaces/acados_matlab_octave/ocp_compile_interface.m index 137bb3f9fd..5e6894940c 100644 --- a/interfaces/acados_matlab_octave/ocp_compile_interface.m +++ b/interfaces/acados_matlab_octave/ocp_compile_interface.m @@ -46,7 +46,6 @@ function ocp_compile_interface(output_dir) mex_names = { ... - 'ocp_get_cost', ... 'ocp_get' ... 'ocp_eval_param_sens', ... }; diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index 7dacf39ca3..9a0666d88f 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -79,7 +79,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // field char *field = mxArrayToString( prhs[1] ); - // mexPrintf("\nin ocp_get: field%s\n", field); + // mexPrintf("\nin ocp_get: field %s\n", field); + if (field == NULL) + { + mexErrMsgTxt("got NULL pointer for field, maybe you put in a \" instead of \' in MATLAB.\n"); + } int N = dims->N; int stage; @@ -475,6 +479,61 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) ocp_nlp_get(solver, "status", &status); *mat_ptr = (double) status; } + else if (!strcmp(field, "cost_value")) + { + plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); + double *out_data = mxGetPr( plhs[0] ); + ocp_nlp_eval_cost(solver, in, out); + ocp_nlp_get(solver, "cost_value", out_data); + } + else if (!strcmp(field, "constraint_violation")) + { + int out_dims[2]; + // evaluate + ocp_nlp_eval_constraints(solver, in, out); + // create output + mxArray *cell_array = mxCreateCellMatrix(N+1, 1); + plhs[0] = cell_array; + mxArray *tmp_mat; + for (ii=0; ii -#include -#include -// acados -#include "acados_c/ocp_nlp_interface.h" -// mex -#include "mex.h" - - -void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) -{ - long long *ptr; - - // solver - ptr = (long long *) mxGetData( mxGetField( prhs[0], 0, "solver" ) ); - ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0]; - // in - ptr = (long long *) mxGetData( mxGetField( prhs[0], 0, "in" ) ); - ocp_nlp_in *in = (ocp_nlp_in *) ptr[0]; - // out - ptr = (long long *) mxGetData( mxGetField( prhs[0], 0, "out" ) ); - ocp_nlp_out *out = (ocp_nlp_out *) ptr[0]; - // config - ptr = (long long *) mxGetData( mxGetField( prhs[0], 0, "config" ) ); - ocp_nlp_config *config = (ocp_nlp_config *) ptr[0]; - - - plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); - double *out_data = mxGetPr( plhs[0] ); - - ocp_nlp_eval_cost(solver, in, out); - ocp_nlp_get(solver, "cost_value", out_data); - - return; - -} 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 77a51fee54..7b487caf53 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 @@ -114,7 +114,11 @@ function set_params_sparse(varargin) end function value = get_cost(obj) - value = ocp_get_cost(obj.C_ocp); + value = ocp_get(obj.C_ocp, 'cost_value'); + end + + function value = evaluate_constraints_and_get_violation(obj) + value = ocp_get(obj.C_ocp, 'constraint_violation'); end function eval_param_sens(obj, field, stage, index) From cb90ec672255aef84bf07c02c0747f58c8d7c7a3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Jun 2025 13:47:13 +0200 Subject: [PATCH 084/164] Replace `fmax` & `fmin` in acados with MAX, MIN macros (#1570) - fmax is often slower and has undesireable property of ignoring NaN. See e.g. https://github.com/giaf/blasfeo/pull/196 --- acados/ocp_nlp/ocp_nlp_common.c | 9 +++++---- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 7 ++++--- .../ocp_nlp_globalization_merit_backtracking.c | 3 ++- acados/ocp_nlp/ocp_nlp_qpscaling.c | 14 +++++++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 17 +++++++++-------- acados/ocp_qp/ocp_qp_osqp.c | 5 +++-- acados/utils/math.c | 2 +- 7 files changed, 31 insertions(+), 26 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 4030ad389d..93db7834b1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -46,6 +46,7 @@ // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" +#include "acados/utils/math.h" #include "acados/utils/strsep.h" // openmp #if defined(ACADOS_WITH_OPENMP) @@ -2586,7 +2587,7 @@ double ocp_nlp_compute_dual_pi_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out { for (j=0; jpi+i, j))); + norm_pi = MAX(norm_pi, fabs(BLASFEO_DVECEL(nlp_out->pi+i, j))); } } return norm_pi; @@ -2603,7 +2604,7 @@ double ocp_nlp_compute_dual_lam_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_ou { for (j=0; j<2*dims->ni[i]; j++) { - norm_lam = fmax(norm_lam, fabs(BLASFEO_DVECEL(nlp_out->lam+i, j))); + norm_lam = MAX(norm_lam, fabs(BLASFEO_DVECEL(nlp_out->lam+i, j))); } } return norm_lam; @@ -2850,13 +2851,13 @@ static void adaptive_levenberg_marquardt_update_mu(double iter, double step_size if (step_size == 1.0) { double mu_tmp = mem->adaptive_levenberg_marquardt_mu; - mem->adaptive_levenberg_marquardt_mu = fmax(opts->adaptive_levenberg_marquardt_mu_min, + mem->adaptive_levenberg_marquardt_mu = MAX(opts->adaptive_levenberg_marquardt_mu_min, mem->adaptive_levenberg_marquardt_mu_bar/(opts->adaptive_levenberg_marquardt_lam)); mem->adaptive_levenberg_marquardt_mu_bar = mu_tmp; } else { - mem->adaptive_levenberg_marquardt_mu = fmin(opts->adaptive_levenberg_marquardt_lam * mem->adaptive_levenberg_marquardt_mu, 1.0); + mem->adaptive_levenberg_marquardt_mu = MIN(opts->adaptive_levenberg_marquardt_lam * mem->adaptive_levenberg_marquardt_mu, 1.0); } } } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index ef81c83623..151fb134fd 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -49,6 +49,7 @@ // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" +#include "acados/utils/math.h" /************************************************ @@ -226,7 +227,7 @@ void *ocp_nlp_globalization_funnel_memory_assign(void *config_, void *dims_, voi void initialize_funnel_width(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double initial_infeasibility) { - mem->funnel_width = fmax(opts->initialization_upper_bound, + mem->funnel_width = MAX(opts->initialization_upper_bound, opts->initialization_increase_factor*initial_infeasibility); } @@ -251,7 +252,7 @@ void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem, } if (mem->penalty_parameter * pred_optimality + pred_infeasibility < opts->penalty_eta * pred_infeasibility) { - mem->penalty_parameter = fmax(0.0, //objective multiplier should always be >= 0! + mem->penalty_parameter = MAX(0.0, //objective multiplier should always be >= 0! fmin(opts->penalty_contraction * mem->penalty_parameter, ((1-opts->penalty_eta) * pred_infeasibility) / (-pred_optimality + 1e-9)) ); @@ -308,7 +309,7 @@ bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalizat double pred, double alpha) { - if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * fmax(pred, 0) + 1e-18, 0)) + if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * MAX(pred, 0) + 1e-18, 0)) { return true; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index e4c1848e8c..9f2a3275b7 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -44,6 +44,7 @@ #include "acados/ocp_nlp/ocp_nlp_globalization_common.h" #include "acados/ocp_nlp/ocp_nlp_common.h" #include "acados/utils/mem.h" +#include "acados/utils/math.h" // blasfeo #include "blasfeo_d_aux.h" @@ -945,7 +946,7 @@ static int ocp_nlp_ddp_backtracking_line_search(ocp_nlp_config *config, ocp_nlp_ negative_ared = trial_cost - nlp_mem->cost_value; // Check Armijo sufficient decrease condition - if (negative_ared <= fmin(-globalization_opts->eps_sufficient_descent*alpha* fmax(pred, 0) + 1e-18, 0)) + if (negative_ared <= fmin(-globalization_opts->eps_sufficient_descent*alpha* MAX(pred, 0) + 1e-18, 0)) { // IF step accepted: update x // reset evaluation point to SQP iterate diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c index 903735a443..b033af4920 100644 --- a/acados/ocp_nlp/ocp_nlp_qpscaling.c +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -295,7 +295,7 @@ static double norm_inf_matrix_col(int col_idx, int col_length, struct blasfeo_d for (int j = 0; j < col_length; ++j) { double tmp = BLASFEO_DMATEL(At, j, col_idx); - norm = fmax(norm, fabs(tmp)); + norm = MAX(norm, fabs(tmp)); } return norm; } @@ -475,14 +475,14 @@ void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, for (int stage = 0; stage <= dim->N; stage++) { compute_gershgorin_max_abs_eig_estimate(nx[stage]+nu[stage], RSQrq+stage, &tmp); - max_abs_eig = fmax(max_abs_eig, tmp); + max_abs_eig = MAX(max_abs_eig, tmp); // take Z into account blasfeo_dvecnrm_inf(2*ns[stage], qp_in->Z+stage, 0, &tmp); - max_abs_eig = fmax(max_abs_eig, tmp); + max_abs_eig = MAX(max_abs_eig, tmp); // norm gradient blasfeo_dvecnrm_inf(nx[stage]+nu[stage]+2*ns[stage], qp_in->rqz+stage, 0, &tmp); - nrm_inf_grad_obj = fmax(nrm_inf_grad_obj, fabs(tmp)); + nrm_inf_grad_obj = MAX(nrm_inf_grad_obj, fabs(tmp)); } if (max_abs_eig < opts->ub_max_abs_eig) @@ -507,7 +507,7 @@ void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, } lb_grad_norm_factor = opts->lb_norm_inf_grad_obj / nrm_inf_grad_obj; tmp = fmin(max_upscale_factor, lb_grad_norm_factor); - mem->obj_factor = fmax(mem->obj_factor, tmp); + mem->obj_factor = MAX(mem->obj_factor, tmp); mem->status = ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED; } if (opts->print_level > 0) @@ -552,10 +552,10 @@ void ocp_nlp_qpscaling_scale_constraints(ocp_nlp_qpscaling_dims *dims, void *opt mask_value_upper = BLASFEO_DVECEL(qp_in->d_mask+i, 2*nb[i]+ng[i]+j); // calculate scaling factor from row norm - double bound_max = fmax(fabs(mask_value_lower * BLASFEO_DVECEL(qp_in->d+i, nb[i]+j)), + double bound_max = MAX(fabs(mask_value_lower * BLASFEO_DVECEL(qp_in->d+i, nb[i]+j)), fabs(mask_value_upper * BLASFEO_DVECEL(qp_in->d+i, 2*nb[i]+ng[i]+j))); // only scale down. - scaling_factor = 1.0 / fmax(1.0, fmax(bound_max, coeff_norm)); + scaling_factor = 1.0 / MAX(1.0, MAX(bound_max, coeff_norm)); // store scaling factor BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j) = scaling_factor; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index f5483c2f20..b0b13428e7 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -56,6 +56,7 @@ #include "acados/utils/print.h" #include "acados/utils/timing.h" #include "acados/utils/types.h" +#include "acados/utils/math.h" #include "acados_c/ocp_qp_interface.h" /************************************************ @@ -656,7 +657,7 @@ static double calculate_pred_l1_inf(ocp_nlp_sqp_wfqp_opts* opts, ocp_nlp_sqp_wfq } else { - if (mem->l1_infeasibility < fmin(opts->nlp_opts->tol_ineq, opts->nlp_opts->tol_eq)) + if (mem->l1_infeasibility < MIN(opts->nlp_opts->tol_ineq, opts->nlp_opts->tol_eq)) { return 0.0; } @@ -689,11 +690,11 @@ static double calculate_qp_l1_infeasibility_from_slacks(ocp_nlp_dims *dims, ocp_ { // Add lower slack tmp1 = BLASFEO_DVECEL(qp_out->ux + i, nx[i]+nu[i]+ns[i] + j); - l1_inf += fmax(0.0, tmp1); + l1_inf += MAX(0.0, tmp1); // Add upper slack tmp2 = BLASFEO_DVECEL(qp_out->ux + i, nx[i]+nu[i]+2*ns[i]+nns[i] + j); - l1_inf += fmax(0.0, tmp2); + l1_inf += MAX(0.0, tmp2); } } #if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) @@ -775,15 +776,15 @@ static double calculate_qp_l1_infeasibility_manually(ocp_nlp_dims *dims, ocp_nlp if (j < nb[i] + ng[i]) { // maximum(0, lower_bound - value) - l1_inf += fmax(0.0, tmp_bound-tmp); - // printf("lower bounds: bound: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, fmax(0.0, tmp_bound-tmp)); + l1_inf += MAX(0.0, tmp_bound-tmp); + // printf("lower bounds: bound: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, MAX(0.0, tmp_bound-tmp)); } else { // upper bounds have the wrong sign! // it is lower_bounds <= value <= -upper_bounds, therefore plus below - // printf("upper bounds: value: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, fmax(0.0, tmp_bound+tmp)); - l1_inf += fmax(0.0, tmp_bound+tmp); + // printf("upper bounds: value: %.4e, value: %.4e, result: %.4e\n", tmp_bound, tmp, MAX(0.0, tmp_bound+tmp)); + l1_inf += MAX(0.0, tmp_bound+tmp); } } } @@ -1475,7 +1476,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); } - if (l1_inf_QP_feasibility/(fmax(1.0, (double) mem->absolute_nns)) < nlp_opts->tol_ineq) + if (l1_inf_QP_feasibility/(MAX(1.0, (double) mem->absolute_nns)) < nlp_opts->tol_ineq) { mem->watchdog_zero_slacks_counter += 1; } diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index 7df6f585af..e16b55712b 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -38,6 +38,7 @@ #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/ocp_qp/ocp_qp_osqp.h" #include "acados/utils/mem.h" +#include "acados/utils/math.h" #include "acados/utils/print.h" #include "acados/utils/timing.h" #include "acados/utils/types.h" @@ -1026,8 +1027,8 @@ void ocp_qp_osqp_opts_set(void *config_, void *opts_, const char *field, void *v // opts->osqp_opts->eps_rel = *tol; // opts->osqp_opts->eps_dual_inf = *tol; - opts->osqp_opts->eps_rel = fmax(*tol, 1e-5); - opts->osqp_opts->eps_dual_inf = fmax(*tol, 1e-5); + opts->osqp_opts->eps_rel = MAX(*tol, 1e-5); + opts->osqp_opts->eps_dual_inf = MAX(*tol, 1e-5); if (*tol <= 1e-3) { diff --git a/acados/utils/math.c b/acados/utils/math.c index a18d2ae6e5..7b2ec88ed0 100644 --- a/acados/utils/math.c +++ b/acados/utils/math.c @@ -1149,7 +1149,7 @@ void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double * } a = BLASFEO_DMATEL(A, ii, ii); lam_i = a - r_i; - lam = fmin(lam, lam_i); + lam = MIN(lam, lam_i); } *out = lam; } From 0ef748586cf28a7363bca0364a4daa39dc7f399a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 30 Jun 2025 14:08:35 +0200 Subject: [PATCH 085/164] Forgotten MIN changes in #1570 (#1572) --- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 4 ++-- acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c | 2 +- acados/ocp_nlp/ocp_nlp_qpscaling.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index 151fb134fd..ea84e5a601 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -253,7 +253,7 @@ void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem, if (mem->penalty_parameter * pred_optimality + pred_infeasibility < opts->penalty_eta * pred_infeasibility) { mem->penalty_parameter = MAX(0.0, //objective multiplier should always be >= 0! - fmin(opts->penalty_contraction * mem->penalty_parameter, + MIN(opts->penalty_contraction * mem->penalty_parameter, ((1-opts->penalty_eta) * pred_infeasibility) / (-pred_optimality + 1e-9)) ); } @@ -309,7 +309,7 @@ bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalizat double pred, double alpha) { - if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * MAX(pred, 0) + 1e-18, 0)) + if (negative_ared <= MIN(globalization_opts->eps_sufficient_descent * alpha * MAX(pred, 0) + 1e-18, 0)) { return true; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 9f2a3275b7..13415d0574 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -946,7 +946,7 @@ static int ocp_nlp_ddp_backtracking_line_search(ocp_nlp_config *config, ocp_nlp_ negative_ared = trial_cost - nlp_mem->cost_value; // Check Armijo sufficient decrease condition - if (negative_ared <= fmin(-globalization_opts->eps_sufficient_descent*alpha* MAX(pred, 0) + 1e-18, 0)) + if (negative_ared <= MIN(-globalization_opts->eps_sufficient_descent*alpha* MAX(pred, 0) + 1e-18, 0)) { // IF step accepted: update x // reset evaluation point to SQP iterate diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c index b033af4920..92be82cf09 100644 --- a/acados/ocp_nlp/ocp_nlp_qpscaling.c +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -506,7 +506,7 @@ void ocp_nlp_qpscaling_compute_obj_scaling_factor(ocp_nlp_qpscaling_dims *dims, printf("Gradient is very small! %.2e\n", mem->obj_factor*nrm_inf_grad_obj); } lb_grad_norm_factor = opts->lb_norm_inf_grad_obj / nrm_inf_grad_obj; - tmp = fmin(max_upscale_factor, lb_grad_norm_factor); + tmp = MIN(max_upscale_factor, lb_grad_norm_factor); mem->obj_factor = MAX(mem->obj_factor, tmp); mem->status = ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED; } From 0c351c88c32691e99e4afca1b573d496275d901c Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Mon, 30 Jun 2025 17:26:51 +0200 Subject: [PATCH 086/164] `AcadosCasadiOcp`: add support for general linear constraint (#1571) - Try to extend support for general linear constraint, i.e. $lg<= Cx + Du <= ug$ in `g` of casadi solver - add a ready test for testing constraint. --- .../casadi_tests/test_casadi_constraint.py | 117 ++++++++++++++++++ interfaces/CMakeLists.txt | 4 + .../acados_casadi_ocp_solver.py | 117 +++++++++++------- 3 files changed, 191 insertions(+), 47 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_constraint.py diff --git a/examples/acados_python/casadi_tests/test_casadi_constraint.py b/examples/acados_python/casadi_tests/test_casadi_constraint.py new file mode 100644 index 0000000000..a45826354a --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_constraint.py @@ -0,0 +1,117 @@ +import sys +sys.path.insert(0, '../getting_started') +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model +from utils import plot_pendulum + +PLOT = False + +def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set linear constraints + # -5 <= x_1 + u*0.1 <= 5 + ocp.constraints.C = np.array([[1, 0, 0, 0]]) + ocp.constraints.D = np.array([[0.1]]) + ocp.constraints.lg = np.array([-5]) + ocp.constraints.ug = np.array([5]) + + # set x_1 at the end of the horizon + ocp.constraints.C_e = np.array([[1, 0, 0, 0]]) + ocp.constraints.lg_e = np.array([0.3]) + ocp.constraints.ug_e = np.array([0.5]) + + # set h constraints + ocp.model.con_h_expr_0 = ca.norm_2(model.x) + ocp.constraints.lh_0 = np.array([0]) + ocp.constraints.uh_0 = np.array([3.16]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + N_horizon = 20 + Tf = 1.0 + ocp = formulate_ocp(Tf, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + if status != 0: + raise Exception(f'acados returned status {status}.') + # get solution + result = ocp_solver.store_iterate_to_obj() + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp,solver="ipopt",verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + result_casadi = casadi_ocp_solver.store_iterate_to_obj() + + # evaluate difference + result.flatten().allclose(other=result_casadi.flatten()) + + if PLOT: + Fmax = 80 + N = ocp.solver_options.N_horizon + acados_u = np.array([ocp_solver.get(i, "u") for i in range(N)]) + acados_x = np.array([ocp_solver.get(i, "x") for i in range(N+1)]) + casadi_u = np.array([casadi_ocp_solver.get(i, "u") for i in range(N)]) + casadi_x = np.array([casadi_ocp_solver.get(i, "x") for i in range(N+1)]) + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, acados_u, acados_x, latexify=False) + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, casadi_u, casadi_x, latexify=False) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index d12408b5b0..fbce7c55eb 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -373,6 +373,9 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME test_casadi_p_in_constraint_and_cost COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_p_in_constraint_and_cost.py) + add_test(NAME test_casadi_constraint + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_constraint.py) # Sim add_test(NAME python_pendulum_ext_sim_example @@ -451,6 +454,7 @@ add_test(NAME python_pendulum_ocp_example_cmake # casadi_tests set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS test_casadi_p_in_constraint_and_cost) set_tests_properties(test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS test_casadi_parametric) + set_tests_properties(test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index f602eee167..d11184363e 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -80,12 +80,8 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): raise NotImplementedError("AcadosCasadiOcpSolver does not support soft constraints yet.") if dims.nz > 0: raise NotImplementedError("AcadosCasadiOcpSolver does not support algebraic variables (z) yet.") - if any([dims.ng, dims.ng_e]): - raise NotImplementedError("AcadosCasadiOcpSolver does not support general nonlinear constraints (g) yet.") if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") - if any([dims.ns_0, dims.ns, dims.ns_e]): - raise NotImplementedError("CasADi NLP formulation not implemented for formulations with soft constraints yet.") # create primal variables indexed by shooting nodes ca_symbol = model.get_casadi_symbol() @@ -238,18 +234,27 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # nonlinear constraints # initial stage if i == 0 and N_horizon > 0: - # h_0 - h_0_nlp_expr = h_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) - g.append(h_0_nlp_expr) - lbg.append(constraints.lh_0) - ubg.append(constraints.uh_0) - if with_hessian and dims.nh_0 > 0: - lam_h_0 = ca_symbol(f'lam_h_0', dims.nh_0, 1) - lam_g.append(lam_h_0) - # add hessian contribution - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_0_nlp_expr, w, lam_h_0, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + if dims.ng > 0: + C = constraints.C + D = constraints.D + linear_constr_expr = ca.mtimes(C, xtraj_node[0]) + ca.mtimes(D, utraj_node[0]) + g.append(linear_constr_expr) + lbg.append(constraints.lg) + ubg.append(constraints.ug) + + if dims.nh_0 > 0: + # h_0 + h_0_nlp_expr = h_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) + g.append(h_0_nlp_expr) + lbg.append(constraints.lh_0) + ubg.append(constraints.uh_0) + if with_hessian: + lam_h_0 = ca_symbol(f'lam_h_0', dims.nh_0, 1) + lam_g.append(lam_h_0) + # add hessian contribution + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_0_nlp_expr, w, lam_h_0, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi_0 > 0: conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) @@ -267,21 +272,31 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): dr_dw = ca.jacobian(r_in_nlp, w) hess_l += dr_dw.T @ outer_hess_r @ dr_dw - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_0 + dims.nphi_0))) - offset += dims.nh_0 + dims.nphi_0 - + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng + dims.nh_0 + dims.nphi_0))) + offset += dims.ng + dims.nh_0 + dims.nphi_0 + + # intermediate stages elif i < N_horizon: - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - g.append(h_i_nlp_expr) - lbg.append(constraints.lh) - ubg.append(constraints.uh) - if with_hessian and dims.nh > 0: - # add hessian contribution - lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) - lam_g.append(lam_h) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + if dims.ng > 0: + C = constraints.C + D = constraints.D + linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) + g.append(linear_constr_expr) + lbg.append(constraints.lg) + ubg.append(constraints.ug) + + if dims.nh > 0: + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + g.append(h_i_nlp_expr) + lbg.append(constraints.lh) + ubg.append(constraints.uh) + if with_hessian and dims.nh > 0: + # add hessian contribution + lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) + lam_g.append(lam_h) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi > 0: g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) @@ -297,22 +312,30 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): dr_dw = ca.jacobian(r_in_nlp, w) hess_l += dr_dw.T @ outer_hess_r @ dr_dw - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh + dims.nphi))) - offset += dims.nphi + dims.nh - + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng + dims.nh + dims.nphi))) + offset += dims.ng + dims.nphi + dims.nh + + # terminal stage else: - # terminal stage - h_e_nlp_expr = h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global) - g.append(h_e_nlp_expr) - lbg.append(constraints.lh_e) - ubg.append(constraints.uh_e) - if with_hessian and dims.nh_e > 0: - # add hessian contribution - lam_h_e = ca_symbol(f'lam_h_e', dims.nh_e, 1) - lam_g.append(lam_h_e) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_e_nlp_expr, w, lam_h_e, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + if dims.ng_e > 0: + C_e = constraints.C_e + linear_constr_expr_e = ca.mtimes(C_e, xtraj_node[-1]) + g.append(linear_constr_expr_e) + lbg.append(constraints.lg_e) + ubg.append(constraints.ug_e) + + if dims.nh_e > 0: + h_e_nlp_expr = h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global) + g.append(h_e_nlp_expr) + lbg.append(constraints.lh_e) + ubg.append(constraints.uh_e) + if with_hessian and dims.nh_e > 0: + # add hessian contribution + lam_h_e = ca_symbol(f'lam_h_e', dims.nh_e, 1) + lam_g.append(lam_h_e) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_e_nlp_expr, w, lam_h_e, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) if dims.nphi_e > 0: g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) @@ -328,8 +351,8 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): dr_dw = ca.jacobian(r_in_nlp, w) hess_l += dr_dw.T @ outer_hess_r @ dr_dw - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.nh_e + dims.nphi_e))) - offset += dims.nh_e + dims.nphi_e + index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng_e + dims.nh_e + dims.nphi_e))) + offset += dims.ng_e + dims.nh_e + dims.nphi_e ### Cost # initial cost term From 7b8a9b242f1e3134703ecea01bdc7f4661780830 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 2 Jul 2025 12:15:41 +0200 Subject: [PATCH 087/164] C: Avoid initializing Hessian explicitly (#1565) - Addressed some TODOs in `SQP_WFQP` - cost integration: Now cost scaling happens in dynamics module if cost is integrated, only slack contributions to cost, are always scaled in cost module. - fix if `cost_scaling != time_step`. - Implement separate pointers for Hessian of cost and dynamics correctly --------- Co-authored-by: d.kiessling --- acados/ocp_nlp/ocp_nlp_common.c | 48 ++++++++++--- acados/ocp_nlp/ocp_nlp_cost_common.h | 2 + acados/ocp_nlp/ocp_nlp_cost_conl.c | 71 ++++++++++++++----- acados/ocp_nlp/ocp_nlp_cost_conl.h | 1 + acados/ocp_nlp/ocp_nlp_cost_external.c | 38 +++++++++- acados/ocp_nlp/ocp_nlp_cost_external.h | 1 + acados/ocp_nlp/ocp_nlp_cost_ls.c | 47 ++++++++++-- acados/ocp_nlp/ocp_nlp_cost_ls.h | 1 + acados/ocp_nlp/ocp_nlp_cost_nls.c | 50 ++++++++++--- acados/ocp_nlp/ocp_nlp_cost_nls.h | 1 + acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 29 ++++++-- acados/ocp_nlp/ocp_nlp_dynamics_cont.h | 2 + acados/ocp_nlp/ocp_nlp_dynamics_disc.c | 12 ++-- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 37 ++++++---- acados/sim/sim_irk_integrator.c | 18 ++++- acados/sim/sim_irk_integrator.h | 2 + docs/developer_guide/index.md | 17 ++--- .../simple_dae_model/example_ocp.m | 2 +- interfaces/acados_matlab_octave/AcadosOcp.m | 3 + .../acados_template/acados_ocp.py | 8 ++- .../c_templates_tera/acados_solver.in.c | 8 ++- 21 files changed, 309 insertions(+), 89 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 93db7834b1..02ff5d0617 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1438,13 +1438,26 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value else if (!strcmp(field, "exact_hess")) { int N = config->N; + int *exact_hess_ptr = (int *) value; + int exact_hess = *exact_hess_ptr; + int add_cost_hess_contribution = 1; + if (!exact_hess) + { + add_cost_hess_contribution = 0; + } // cost for (int i=0; i<=N; i++) + { config->cost[i]->opts_set(config->cost[i], opts->cost[i], "exact_hess", value); + } // dynamics for (int i=0; idynamics[i]->opts_set(config->dynamics[i], opts->dynamics[i], - "compute_hess", value); + "compute_hess", value); + // if no dynamics Hessian, then cost module should write its Hessian instead of adding. + config->cost[i]->opts_set(config->cost[i], opts->cost[i], "add_hess_contribution", &add_cost_hess_contribution); + } // constraints for (int i=0; i<=N; i++) config->constraints[i]->opts_set(config->constraints[i], opts->constraints[i], @@ -1460,9 +1473,19 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value else if (!strcmp(field, "exact_hess_dyn")) { int N = config->N; + int *exact_hess_ptr = (int *) value; + int exact_hess = *exact_hess_ptr; + int add_hess_contribution; for (int i=0; idynamics[i]->opts_set(config->dynamics[i], opts->dynamics[i], - "compute_hess", value); + "compute_hess", value); + if (!exact_hess) + { + add_hess_contribution = 0; + config->cost[i]->opts_set(config->cost[i], opts->cost[i], "add_hess_contribution", &add_hess_contribution); + } + } } else if (!strcmp(field, "exact_hess_constr")) { @@ -2735,6 +2758,8 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di struct blasfeo_dmat *W_chol = config->cost[i]->memory_get_W_chol_ptr(nlp_mem->cost[i]); struct blasfeo_dvec *W_chol_diag = config->cost[i]->memory_get_W_chol_diag_ptr(nlp_mem->cost[i]); double *outer_hess_is_diag = config->cost[i]->get_outer_hess_is_diag_ptr(nlp_mem->cost[i], nlp_in->cost[i]); + double *cost_scaling = config->cost[i]->model_get_scaling_ptr(nlp_in->cost[i]); + int *add_cost_hess_contribution = config->cost[i]->opts_get_add_hess_contribution_ptr(config->cost[i], opts->cost[i]); config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "cost_grad", cost_grad); config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "cost_fun", cost_fun); @@ -2742,6 +2767,8 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "W_chol", W_chol); config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "W_chol_diag", W_chol_diag); config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "outer_hess_is_diag", outer_hess_is_diag); + config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "cost_scaling_ptr", cost_scaling); + config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "add_cost_hess_contribution_ptr", add_cost_hess_contribution); } } @@ -2922,22 +2949,23 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, #endif for (int i = 0; i <= N; i++) { - // init Hessian to 0 - if (mem->compute_hess) - { - blasfeo_dgese(nu[i] + nx[i], nu[i] + nx[i], 0.0, mem->qp_in->RSQrq+i, 0, 0); - } + // // init Hessian to 0 + // if (mem->compute_hess) + // { + // blasfeo_dgese(nu[i] + nx[i], nu[i] + nx[i], 0.0, mem->qp_in->RSQrq+i, 0, 0); + // } + // NOTE: removed init and directly write cost contribution into Hessian + // dynamics: NOTE: has to be first, as it computes z, which is used in cost and constraints. if (i < N) { - // dynamics config->dynamics[i]->update_qp_matrices(config->dynamics[i], dims->dynamics[i], - in->dynamics[i], opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); + in->dynamics[i], opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); } // cost config->cost[i]->update_qp_matrices(config->cost[i], dims->cost[i], in->cost[i], - opts->cost[i], mem->cost[i], work->cost[i]); + opts->cost[i], mem->cost[i], work->cost[i]); // constraints config->constraints[i]->update_qp_matrices(config->constraints[i], dims->constraints[i], diff --git a/acados/ocp_nlp/ocp_nlp_cost_common.h b/acados/ocp_nlp/ocp_nlp_cost_common.h index f37d462979..a569bd82da 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -70,10 +70,12 @@ typedef struct void (*opts_initialize_default)(void *config, void *dims, void *opts); void (*opts_update)(void *config, void *dims, void *opts); void (*opts_set)(void *config, void *opts, const char *field, void *value); + int *(*opts_get_add_hess_contribution_ptr)(void *config, void *opts); acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); double *(*memory_get_fun_ptr)(void *memory); struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory); struct blasfeo_dvec *(*model_get_y_ref_ptr)(void *memory); + double *(*model_get_scaling_ptr)(void *memory); struct blasfeo_dmat *(*memory_get_W_chol_ptr)(void *memory_); struct blasfeo_dvec *(*memory_get_W_chol_diag_ptr)(void *memory_); double *(*get_outer_hess_is_diag_ptr)(void *memory_, void *model_); diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c index 1c3919f461..1f1c825b94 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.c +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.c @@ -365,6 +365,7 @@ void ocp_nlp_cost_conl_opts_initialize_default(void *config_, void *dims_, void ocp_nlp_cost_conl_opts *opts = opts_; opts->gauss_newton_hess = 1; + opts->add_hess_contribution = 0; return; } @@ -392,6 +393,11 @@ void ocp_nlp_cost_conl_opts_set(void *config_, void *opts_, const char *field, v int *opt_val = (int *) value; opts->integrator_cost = *opt_val; } + else if (!strcmp(field, "add_hess_contribution")) + { + int* int_ptr = value; + opts->add_hess_contribution = *int_ptr; + } else if(!strcmp(field, "with_solution_sens_wrt_params")) { // not implemented yet @@ -408,6 +414,14 @@ void ocp_nlp_cost_conl_opts_set(void *config_, void *opts_, const char *field, v } +int* ocp_nlp_cost_conl_opts_get_add_hess_contribution_ptr(void *config_, void *opts_) +{ + ocp_nlp_cost_conl_opts *opts = opts_; + + return &opts->add_hess_contribution; +} + + /************************************************ * memory @@ -510,6 +524,12 @@ struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_) } +double *ocp_nlp_cost_conl_model_get_scaling_ptr(void *in_) +{ + ocp_nlp_cost_conl_model *model = in_; + return &model->scaling; +} + struct blasfeo_dvec *ocp_nlp_cost_conl_model_get_y_ref_ptr(void *in_) { @@ -691,6 +711,12 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode ocp_nlp_cost_conl_cast_workspace(config_, dims, opts_, work_); + double prev_RSQ_factor = 0.0; + if (opts->add_hess_contribution) + { + prev_RSQ_factor = 1.0; + } + int nx = dims->nx; int nz = dims->nz; int nu = dims->nu; @@ -803,7 +829,7 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode } // RSQrq += scaling * tmp_nv_ny * tmp_nv_ny^T blasfeo_dsyrk_ln(nu+nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, 0, 0, - 1.0, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); + prev_RSQ_factor, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); } // slack update gradient @@ -818,10 +844,18 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); // scale - if(model->scaling!=1.0) + if (model->scaling!=1.0) { - blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); - memory->fun *= model->scaling; + if (opts->integrator_cost == 0) + { + blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + memory->fun *= model->scaling; + } + else + { + // only scale the slack gradient + blasfeo_dvecsc(2*ns, model->scaling, &memory->grad, nu+nx); + } } return; @@ -907,25 +941,13 @@ void ocp_nlp_cost_conl_compute_gradient(void *config_, void *dims_, void *model_ // grad = Jt_ux_tilde * tmp_ny blasfeo_dgemv_n(nu+nx, ny, 1.0, &work->Jt_ux_tilde, 0, 0, &work->tmp_ny, 0, 0.0, &memory->grad, 0, &memory->grad, 0); - - // // tmp_nv_ny = Jt_ux_tilde * W_chol - // blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, - // &work->Jt_ux_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); } else { // grad = Jt_ux * tmp_ny blasfeo_dgemv_n(nu+nx, ny, 1.0, &work->Jt_ux, 0, 0, &work->tmp_ny, 0, 0.0, &memory->grad, 0, &memory->grad, 0); - - // // tmp_nv_ny = Jt_ux * W_chol, where W_chol is lower triangular - // blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, &memory->W_chol, 0, 0, &work->Jt_ux, 0, 0, - // &work->tmp_nv_ny, 0, 0); - } - // // RSQrq += scaling * tmp_nv_ny * tmp_nv_ny^T - // blasfeo_dsyrk_ln(nu+nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, 0, 0, - // 1.0, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); } // slack update gradient @@ -933,9 +955,18 @@ void ocp_nlp_cost_conl_compute_gradient(void *config_, void *dims_, void *model_ blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); // scale - if(model->scaling!=1.0) + if (model->scaling!=1.0) { - blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + if (opts->integrator_cost == 0) + { + // scale the whole gradient + blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + } + else + { + // only scale the slack gradient + blasfeo_dvecsc(2*ns, model->scaling, &memory->grad, nu+nx); + } } return; @@ -1003,7 +1034,7 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_, memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale - if (model->scaling!=1.0) + if (model->scaling!=1.0 && opts->integrator_cost == 0) { memory->fun *= model->scaling; } @@ -1066,6 +1097,7 @@ void ocp_nlp_cost_conl_config_initialize_default(void *config_, int stage) config->opts_initialize_default = &ocp_nlp_cost_conl_opts_initialize_default; config->opts_update = &ocp_nlp_cost_conl_opts_update; config->opts_set = &ocp_nlp_cost_conl_opts_set; + config->opts_get_add_hess_contribution_ptr = &ocp_nlp_cost_conl_opts_get_add_hess_contribution_ptr; config->memory_calculate_size = &ocp_nlp_cost_conl_memory_calculate_size; config->memory_assign = &ocp_nlp_cost_conl_memory_assign; config->memory_get_fun_ptr = &ocp_nlp_cost_conl_memory_get_fun_ptr; @@ -1074,6 +1106,7 @@ void ocp_nlp_cost_conl_config_initialize_default(void *config_, int stage) config->get_outer_hess_is_diag_ptr = &ocp_nlp_cost_conl_get_outer_hess_is_diag_ptr; config->memory_get_W_chol_diag_ptr = &ocp_nlp_cost_conl_memory_get_W_chol_diag_ptr; config->model_get_y_ref_ptr = &ocp_nlp_cost_conl_model_get_y_ref_ptr; + config->model_get_scaling_ptr = &ocp_nlp_cost_conl_model_get_scaling_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_conl_memory_set_ux_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_cost_conl_memory_set_z_alg_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.h b/acados/ocp_nlp/ocp_nlp_cost_conl.h index 14f777b09e..ee871bee88 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.h +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.h @@ -114,6 +114,7 @@ typedef struct { bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian int integrator_cost; // > 0 indicating that cost is propagated within integrator instead of cost module, only add slack contributions + int add_hess_contribution; } ocp_nlp_cost_conl_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index aabdf351a8..5e1536ce75 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -330,6 +330,11 @@ int ocp_nlp_cost_external_model_get(void *config_, void *dims_, void *model_, return status; } +double *ocp_nlp_cost_external_model_get_scaling_ptr(void *in_) +{ + ocp_nlp_cost_external_model *model = in_; + return &model->scaling; +} /************************************************ * options @@ -373,6 +378,7 @@ void ocp_nlp_cost_external_opts_initialize_default(void *config_, void *dims_, v opts->use_numerical_hessian = 0; opts->with_solution_sens_wrt_params = 0; + opts->add_hess_contribution = 0; return; } @@ -405,6 +411,11 @@ void ocp_nlp_cost_external_opts_set(void *config_, void *opts_, const char *fiel int *opt_val = (int *) value; opts->use_numerical_hessian = *opt_val; } + else if (!strcmp(field, "add_hess_contribution")) + { + int* int_ptr = value; + opts->add_hess_contribution = *int_ptr; + } else if(!strcmp(field, "with_solution_sens_wrt_params")) { int *opt_val = (int *) value; @@ -420,7 +431,12 @@ void ocp_nlp_cost_external_opts_set(void *config_, void *opts_, const char *fiel } +int* ocp_nlp_cost_external_opts_get_add_hess_contribution_ptr(void *config_, void *opts_) +{ + ocp_nlp_cost_external_opts *opts = opts_; + return &opts->add_hess_contribution; +} /************************************************ * memory @@ -718,7 +734,14 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void * model->ext_cost_fun_jac->evaluate(model->ext_cost_fun_jac, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); // custom hessian - blasfeo_dgead(nx+nu, nx+nu, model->scaling, &model->numerical_hessian, 0, 0, memory->RSQrq, 0, 0); + if (opts->add_hess_contribution) + { + blasfeo_dgead(nx+nu, nx+nu, model->scaling, &model->numerical_hessian, 0, 0, memory->RSQrq, 0, 0); + } + else + { + blasfeo_dgecpsc(nx+nu, nx+nu, model->scaling, &model->numerical_hessian, 0, 0, memory->RSQrq, 0, 0); + } } else { @@ -735,7 +758,16 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void * ext_fun_in, ext_fun_type_out, ext_fun_out); // hessian contribution from xu with scaling - blasfeo_dgead(nx+nu, nx+nu, model->scaling, &work->tmp_nunx_nunx, 0, 0, memory->RSQrq, 0, 0); + if (opts->add_hess_contribution) + { + // add to RSQrq + blasfeo_dgead(nx+nu, nx+nu, model->scaling, &work->tmp_nunx_nunx, 0, 0, memory->RSQrq, 0, 0); + } + else + { + // copy to RSQrq + blasfeo_dgecpsc(nx+nu, nx+nu, model->scaling, &work->tmp_nunx_nunx, 0, 0, memory->RSQrq, 0, 0); + } if (nz > 0) { @@ -1091,11 +1123,13 @@ void ocp_nlp_cost_external_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_cost_external_model_assign; config->model_set = &ocp_nlp_cost_external_model_set; config->model_get = &ocp_nlp_cost_external_model_get; + config->model_get_scaling_ptr = &ocp_nlp_cost_external_model_get_scaling_ptr; config->opts_calculate_size = &ocp_nlp_cost_external_opts_calculate_size; config->opts_assign = &ocp_nlp_cost_external_opts_assign; config->opts_initialize_default = &ocp_nlp_cost_external_opts_initialize_default; config->opts_update = &ocp_nlp_cost_external_opts_update; config->opts_set = &ocp_nlp_cost_external_opts_set; + config->opts_get_add_hess_contribution_ptr = &ocp_nlp_cost_external_opts_get_add_hess_contribution_ptr; config->memory_calculate_size = &ocp_nlp_cost_external_memory_calculate_size; config->memory_assign = &ocp_nlp_cost_external_memory_assign; config->memory_get_fun_ptr = &ocp_nlp_cost_external_memory_get_fun_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.h b/acados/ocp_nlp/ocp_nlp_cost_external.h index 361a230004..2ba22e9c1d 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -99,6 +99,7 @@ typedef struct { int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation int with_solution_sens_wrt_params; + int add_hess_contribution; } ocp_nlp_cost_external_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index f4e8364d30..443e76ea7c 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -446,6 +446,12 @@ int ocp_nlp_cost_ls_model_get(void *config_, void *dims_, void *model_, } +double *ocp_nlp_cost_ls_model_get_scaling_ptr(void *in_) +{ + ocp_nlp_cost_ls_model *model = in_; + return &model->scaling; +} + //////////////////////////////////////////////////////////////////////////////// // options // //////////////////////////////////////////////////////////////////////////////// @@ -487,6 +493,7 @@ void ocp_nlp_cost_ls_opts_initialize_default(void *config_, { ocp_nlp_cost_ls_opts *opts = opts_; opts->compute_hess = 1; + opts->add_hess_contribution = 0; return; } @@ -516,12 +523,17 @@ void ocp_nlp_cost_ls_opts_set(void *config_, void *opts_, const char *field, voi int* int_ptr = value; opts->compute_hess = *int_ptr; } - else if(!strcmp(field, "with_solution_sens_wrt_params")) + else if (!strcmp(field, "with_solution_sens_wrt_params")) { // not implemented yet // int *opt_val = (int *) value; // opts->with_solution_sens_wrt_params = *opt_val; } + else if (!strcmp(field, "add_hess_contribution")) + { + int* int_ptr = value; + opts->add_hess_contribution = *int_ptr; + } else { printf("\nerror: field %s not available in ocp_nlp_cost_ls_opts_set\n", field); @@ -533,6 +545,13 @@ void ocp_nlp_cost_ls_opts_set(void *config_, void *opts_, const char *field, voi } +int* ocp_nlp_cost_ls_opts_get_add_hess_contribution_ptr(void *config_, void *opts_) +{ + ocp_nlp_cost_ls_opts *opts = opts_; + + return &opts->add_hess_contribution; +} + //////////////////////////////////////////////////////////////////////////////// // memory // @@ -875,6 +894,9 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, struct blasfeo_dmat *Cyt = &model->Cyt; ocp_nlp_cost_ls_opts *opts = opts_; + + + if (nz > 0) { // eliminate algebraic variables and update Cyt and y_ref @@ -903,11 +925,16 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, } // add hessian of the cost contribution + double prev_RSQ_factor = 0.0; + if (opts->add_hess_contribution) + { + prev_RSQ_factor = 1.0; + } if (opts->compute_hess) { - // RSQrq += scaling * tmp_nv_ny * tmp_nv_ny^T + // RSQrq = scaling * tmp_nv_ny * tmp_nv_ny^T blasfeo_dsyrk_ln(nu + nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, - 0, 0, 1.0, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); + 0, 0, prev_RSQ_factor, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); } // compute gradient, function @@ -922,8 +949,16 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, { if (opts->compute_hess) { - // add hessian of the cost contribution - blasfeo_dgead(nx + nu, nx + nu, 1.0, &memory->hess, 0, 0, memory->RSQrq, 0, 0); + if (opts->add_hess_contribution) + { + // add + blasfeo_dgead(nx + nu, nx + nu, 1.0, &memory->hess, 0, 0, memory->RSQrq, 0, 0); + } + else + { + // write cost contribution into hessian + blasfeo_dgecp(nx + nu, nx + nu, &memory->hess, 0, 0, memory->RSQrq, 0, 0); + } } // compute gradient, function @@ -1106,11 +1141,13 @@ void ocp_nlp_cost_ls_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_cost_ls_model_assign; config->model_set = &ocp_nlp_cost_ls_model_set; config->model_get = &ocp_nlp_cost_ls_model_get; + config->model_get_scaling_ptr = &ocp_nlp_cost_ls_model_get_scaling_ptr; config->opts_calculate_size = &ocp_nlp_cost_ls_opts_calculate_size; config->opts_assign = &ocp_nlp_cost_ls_opts_assign; config->opts_initialize_default = &ocp_nlp_cost_ls_opts_initialize_default; config->opts_update = &ocp_nlp_cost_ls_opts_update; config->opts_set = &ocp_nlp_cost_ls_opts_set; + config->opts_get_add_hess_contribution_ptr = &ocp_nlp_cost_ls_opts_get_add_hess_contribution_ptr; config->memory_calculate_size = &ocp_nlp_cost_ls_memory_calculate_size; config->memory_assign = &ocp_nlp_cost_ls_memory_assign; config->memory_get_fun_ptr = &ocp_nlp_cost_ls_memory_get_fun_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.h b/acados/ocp_nlp/ocp_nlp_cost_ls.h index 39834ec004..2bfd83a12e 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -138,6 +138,7 @@ int ocp_nlp_cost_ls_model_get(void *config_, void *dims_, void *model_, typedef struct { int compute_hess; + int add_hess_contribution; } ocp_nlp_cost_ls_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index 10929947f2..7715c0cc86 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -385,7 +385,11 @@ int ocp_nlp_cost_nls_model_get(void *config_, void *dims_, void *model_, return status; } - +double *ocp_nlp_cost_nls_model_get_scaling_ptr(void *in_) +{ + ocp_nlp_cost_nls_model *model = in_; + return &model->scaling; +} /************************************************ * options @@ -427,6 +431,7 @@ void ocp_nlp_cost_nls_opts_initialize_default(void *config_, void *dims_, void * ocp_nlp_cost_nls_opts *opts = opts_; opts->gauss_newton_hess = 1; + opts->add_hess_contribution = 0; return; } @@ -465,6 +470,11 @@ void ocp_nlp_cost_nls_opts_set(void *config_, void *opts_, const char *field, vo opts->gauss_newton_hess = 0; } } + else if (!strcmp(field, "add_hess_contribution")) + { + int* int_ptr = value; + opts->add_hess_contribution = *int_ptr; + } else if(!strcmp(field, "integrator_cost")) { int *opt_val = (int *) value; @@ -487,6 +497,14 @@ void ocp_nlp_cost_nls_opts_set(void *config_, void *opts_, const char *field, vo } +int* ocp_nlp_cost_nls_opts_get_add_hess_contribution_ptr(void *config_, void *opts_) +{ + ocp_nlp_cost_nls_opts *opts = opts_; + + return &opts->add_hess_contribution; +} + + /************************************************ * memory @@ -834,6 +852,12 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model struct blasfeo_dvec_args x_in; // input x of external fun; struct blasfeo_dvec_args u_in; // input u of external fun; + double prev_RSQ_factor = 0.0; + if (opts->add_hess_contribution) + { + prev_RSQ_factor = 1.0; + } + if (opts->integrator_cost == 0) { x_in.x = memory->ux; @@ -933,9 +957,9 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model /* hessian */ if (opts->gauss_newton_hess) { - // RSQrq += scaling * tmp_nv_ny * tmp_nv_ny^T + // RSQrq = scaling * tmp_nv_ny * tmp_nv_ny^T blasfeo_dsyrk_ln(nu+nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, 0, 0, - 1.0, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); + prev_RSQ_factor, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); } else { @@ -961,9 +985,9 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model model->nls_y_hess->evaluate(model->nls_y_hess, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); - // RSQrq += scaling * (tmp_nv_nv + tmp_nv_ny * tmp_nv_ny^T) + // RSQrq = scaling * (tmp_nv_nv + tmp_nv_ny * tmp_nv_ny^T) blasfeo_dsyrk_ln(nu+nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, 0, 0, - 1.0, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); + prev_RSQ_factor, memory->RSQrq, 0, 0, memory->RSQrq, 0, 0); blasfeo_dgead(nu+nx, nu+nx, model->scaling, &work->tmp_nv_nv, 0, 0, memory->RSQrq, 0, 0); } } // end if (opts->integrator_cost == 0) @@ -982,8 +1006,16 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model // scale if (model->scaling!=1.0) { - blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); - memory->fun *= model->scaling; + if (opts->integrator_cost == 0) + { + blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + memory->fun *= model->scaling; + } + else + { + // only scale the slack gradient + blasfeo_dvecsc(2*ns, model->scaling, &memory->grad, nu+nx); + } } // printf("cost_fun: %e\n", memory->fun); @@ -1091,7 +1123,7 @@ void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_, memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale - if (model->scaling!=1.0) + if (model->scaling!=1.0 && opts->integrator_cost == 0) { memory->fun *= model->scaling; } @@ -1154,11 +1186,13 @@ void ocp_nlp_cost_nls_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_cost_nls_model_assign; config->model_set = &ocp_nlp_cost_nls_model_set; config->model_get = &ocp_nlp_cost_nls_model_get; + config->model_get_scaling_ptr = &ocp_nlp_cost_nls_model_get_scaling_ptr; config->opts_calculate_size = &ocp_nlp_cost_nls_opts_calculate_size; config->opts_assign = &ocp_nlp_cost_nls_opts_assign; config->opts_initialize_default = &ocp_nlp_cost_nls_opts_initialize_default; config->opts_update = &ocp_nlp_cost_nls_opts_update; config->opts_set = &ocp_nlp_cost_nls_opts_set; + config->opts_get_add_hess_contribution_ptr = &ocp_nlp_cost_nls_opts_get_add_hess_contribution_ptr; config->memory_calculate_size = &ocp_nlp_cost_nls_memory_calculate_size; config->memory_assign = &ocp_nlp_cost_nls_memory_assign; config->memory_get_fun_ptr = &ocp_nlp_cost_nls_memory_get_fun_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.h b/acados/ocp_nlp/ocp_nlp_cost_nls.h index d81456d5f6..fa08281b93 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -117,6 +117,7 @@ typedef struct { bool gauss_newton_hess; // gauss-newton hessian approximation int integrator_cost; // > 0 indicating that cost is propagated within integrator instead of cost module, only add slack contributions + int add_hess_contribution; } ocp_nlp_cost_nls_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index cb202e8d89..bdc76f3021 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -548,6 +548,15 @@ void ocp_nlp_dynamics_cont_memory_set(void *config_, void *dims_, void *mem_, co { sim->memory_set(sim, dims->sim, mem->sim_solver, field, value); } + else if (!strcmp(field, "cost_scaling_ptr")) + { + mem->cost_scaling_ptr = value; + sim->memory_set(sim, dims->sim, mem->sim_solver, field, value); + } + else if (!strcmp(field, "add_cost_hess_contribution_ptr")) + { + mem->add_cost_hess_contribution_ptr = value; + } else { printf("\nerror: ocp_nlp_dynamics_cont_memory_set: field %s not available\n", field); @@ -845,12 +854,9 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * blasfeo_dveccp(nx1, mem->pi, 0, &mem->adj, nu+nx); } - // hessian + /* Hessian */ if (opts->compute_hess) { - -// d_print_mat(nu+nx, nu+nx, work->sim_out->S_hess, nu+nx); - // unpack d*_d2u blasfeo_pack_dmat(nu, nu, &work->sim_out->S_hess[(nx+nu)*nx + nx], nx+nu, &work->hess, 0, 0); // unpack d*_dux: mem-hess: nx x nu @@ -858,8 +864,8 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * // unpack d*_d2x blasfeo_pack_dmat(nx, nx, &work->sim_out->S_hess[0], nx+nu, &work->hess, nu, nu); - // Add hessian contribution - blasfeo_dgead(nx+nu, nx+nu, 1.0, &work->hess, 0, 0, mem->RSQrq, 0, 0); + // Write hessian contribution + blasfeo_dgecp(nx+nu, nx+nu, &work->hess, 0, 0, mem->RSQrq, 0, 0); } int cost_computation; @@ -872,7 +878,15 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * mem->sim_solver, "cost_hess", &cost_hess); // printf("dynamics: RSQrq before cost contribution\n"); // blasfeo_print_exp_dmat(nx+nu, nx+nu, mem->RSQrq, 0, 0); - blasfeo_dgead(nx+nu, nx+nu, model->T, cost_hess, 0, 0, mem->RSQrq, 0, 0); + if (*mem->add_cost_hess_contribution_ptr) + { + // Add hessian contribution + blasfeo_dgead(nx+nu, nx+nu, mem->cost_scaling_ptr[0], cost_hess, 0, 0, mem->RSQrq, 0, 0); + } + else + { + blasfeo_dgecpsc(nx+nu, nx+nu, mem->cost_scaling_ptr[0], cost_hess, 0, 0, mem->RSQrq, 0, 0); + } // printf("dynamics: cost contribution\n"); // blasfeo_print_exp_dmat(nx+nu, nx+nu, cost_hess, 0, 0); @@ -880,6 +894,7 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * // blasfeo_print_exp_dmat(nx+nu, nx+nu, mem->RSQrq, 0, 0); } + return; } diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 91fcd15137..29e46e2260 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -114,6 +114,8 @@ typedef struct struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *z_alg; // pointer to output z at t = 0 bool *set_sim_guess; // indicate if initialization for integrator is set from outside + double *cost_scaling_ptr; // pointer to cost scaling factor + int *add_cost_hess_contribution_ptr; // pointer to cost module option struct blasfeo_dvec *sim_guess; // initializations for integrator // struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c index d96e2d7e5a..9fa539d1e8 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c @@ -266,6 +266,11 @@ void ocp_nlp_dynamics_disc_opts_get(void *config_, void *opts_, const char *fiel int *int_ptr = value; *int_ptr = opts->cost_computation; } + else if (!strcmp(field, "compute_hess")) + { + int *int_ptr = value; + *int_ptr = opts->compute_hess; + } else { printf("\nerror: field %s not available in ocp_nlp_dynamics_disc_opts_get\n", field); @@ -610,7 +615,7 @@ void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims_, void * // ocp_nlp_dynamics_config *config = config_; ocp_nlp_dynamics_disc_dims *dims = dims_; ocp_nlp_dynamics_disc_opts *opts = opts_; - ocp_nlp_dynamics_disc_workspace *work = work_; + // ocp_nlp_dynamics_disc_workspace *work = work_; ocp_nlp_dynamics_disc_memory *memory = mem_; ocp_nlp_dynamics_disc_model *model = model_; @@ -649,7 +654,7 @@ void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims_, void * pi_in.xi = 0; struct blasfeo_dmat_args hess_out; - hess_out.A = &work->tmp_nv_nv; + hess_out.A = memory->RSQrq; hess_out.ai = 0; hess_out.aj = 0; @@ -670,9 +675,6 @@ void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims_, void * // call external function model->disc_dyn_fun_jac_hess->evaluate(model->disc_dyn_fun_jac_hess, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); - - // Add hessian contribution - blasfeo_dgead(nx+nu, nx+nu, 1.0, &work->tmp_nv_nv, 0, 0, memory->RSQrq, 0, 0); } else { diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index b0b13428e7..3d79a7423c 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -843,26 +843,33 @@ static void set_pointers_for_hessian_evaluation(ocp_nlp_config *config, exit(1); } -#if defined(ACADOS_WITH_OPENMP) - #pragma omp parallel for -#endif - for (int i = 0; i <= N; i++) - { - // TODO: first compute cost hessian (without adding) and avoid setting everything to zero? - // init Hessians to 0 - - // TODO: avoid setting qp_in->RSQ to zero in ocp_nlp_approximate_qp_matrices? - blasfeo_dgese(nu[i] + nx[i], nu[i] + nx[i], 0.0, mem->RSQ_constr+i, 0, 0); - blasfeo_dgese(nu[i] + nx[i], nu[i] + nx[i], 0.0, mem->RSQ_cost+i, 0, 0); - } + // init terminal constraint Hessians to 0, as dynamics do not write into them. + // NOTE: one can implement add_hess_contribution to avoid set + blasfeo_dgese(nu[N] + nx[N], nu[N] + nx[N], 0.0, mem->RSQ_constr+N, 0, 0); + // in Hessian computation modules are called in order: + // dyn, cost, constr. + int dyn_compute_hess; for (int i = 0; i < N; i++) { config->dynamics[i]->memory_set_RSQrq_ptr(mem->RSQ_constr+i, nlp_mem->dynamics[i]); + // dynamics always write into hess directly, if hess is computed + config->dynamics[i]->opts_get(config->dynamics[i], opts->dynamics[i], "compute_hess", &dyn_compute_hess); + if (!dyn_compute_hess) + { + // if dynamics do not compute Hessian, we set it to 0 + blasfeo_dgese(nx[i] + nu[i], nx[i] + nu[i], 0.0, mem->RSQ_constr+i, 0, 0); + } } + // write cost hess contribution to RSQ_cost + int add_cost_hess_contribution = 0; for (int i = 0; i <= N; i++) { + config->cost[i]->opts_set(config->cost[i], opts->cost[i], "add_hess_contribution", &add_cost_hess_contribution); config->cost[i]->memory_set_RSQrq_ptr(mem->RSQ_cost+i, nlp_mem->cost[i]); + } + for (int i = 0; i <= N; i++) + { config->constraints[i]->memory_set_RSQrq_ptr(mem->RSQ_constr+i, nlp_mem->constraints[i]); } return; @@ -1310,7 +1317,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf blasfeo_dveccp(2*n_nominal_ineq_nlp+ns[i], nominal_qp_in->d + i, 0, relaxed_qp_in->d + i, 0); blasfeo_dveccp(ns[i], nominal_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i], relaxed_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i]+nns[i]); } - // setup d_mask; TODO: this is only needed at the start of each NLP solve + // setup d_mask if (nlp_mem->iter == 0) { int offset_dmask; @@ -1318,7 +1325,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf { offset_dmask = 2*(dims->nb[i]+dims->ng[i]+dims->ni_nl[i]); blasfeo_dveccp(offset_dmask, nominal_qp_in->d_mask+i, 0, relaxed_qp_in->d_mask+i, 0); - blasfeo_dvecse(2*relaxed_qp_in->dim->ns[i], 1.0, relaxed_qp_in->d_mask+i,offset_dmask); + blasfeo_dvecse(2*relaxed_qp_in->dim->ns[i], 1.0, relaxed_qp_in->d_mask+i, offset_dmask); } } } @@ -1585,8 +1592,8 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } /* Prepare the QP data */ // linearize NLP and update QP matrices - set_pointers_for_hessian_evaluation(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work); acados_tic(&timer1); + set_pointers_for_hessian_evaluation(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work); // nominal QP solver ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); diff --git a/acados/sim/sim_irk_integrator.c b/acados/sim/sim_irk_integrator.c index 6397922111..74a19173e8 100644 --- a/acados/sim/sim_irk_integrator.c +++ b/acados/sim/sim_irk_integrator.c @@ -497,6 +497,10 @@ int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field { mem->y_ref = value; } + else if (!strcmp(field, "cost_scaling_ptr")) + { + mem->cost_scaling_ptr = value; + } else if (!strcmp(field, "guesses_blasfeo")) { int nx, nz; @@ -1199,6 +1203,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo struct blasfeo_dvec *cost_grad = mem->cost_grad; struct blasfeo_dvec *nls_res = workspace->nls_res; struct blasfeo_dvec *tmp_ny = workspace->tmp_ny; + double cost_scaling; struct blasfeo_dmat *cost_hess = mem->cost_hess; struct blasfeo_dmat *J_y_tilde = workspace->J_y_tilde; @@ -1315,6 +1320,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo printf("\nIRK cost_computation not implemented for nz>0!\n\n"); exit(1); } + cost_scaling = mem->cost_scaling_ptr[0]; } // pack @@ -1645,7 +1651,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo } // cost_grad += b * tmp_ny^T * tmp_ny_nux = b * tmp_ny_nux^T * tmp_ny - blasfeo_dgemv_n(nx+nu, ny, b_vec[ii]/num_steps, tmp_nux_ny2, 0, 0, tmp_ny, 0, + blasfeo_dgemv_n(nx+nu, ny, cost_scaling * b_vec[ii]/num_steps, tmp_nux_ny2, 0, 0, tmp_ny, 0, 1.0, cost_grad, 0, cost_grad, 0); // cost_hess += b * tmp_nux_ny_2 * tmp_nux_ny_2^T @@ -1735,7 +1741,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // &workspace->Jt_z, 0, 0, 1.0, &workspace->tmp_nux_ny, 0, 0, &Jt_ux_tilde, 0, 0); // // cost_grad += b * Jt_ux_tilde * tmp_ny - // blasfeo_dgemv_n(nu+nx, ny, b_vec[ii]/num_steps, &Jt_ux_tilde, 0, 0, tmp_ny, 0, + // blasfeo_dgemv_n(nu+nx, ny, cost_scaling * b_vec[ii]/num_steps, &Jt_ux_tilde, 0, 0, tmp_ny, 0, // 1.0, cost_grad, 0, cost_grad, 0); // // tmp_nv_ny = Jt_ux_tilde * W_chol @@ -1773,7 +1779,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo } // cost_grad += b * J_y_tilde^T * tmp_ny - blasfeo_dgemv_t(ny, nx+nu, b_vec[ii]/num_steps, J_y_tilde, 0, 0, tmp_ny, 0, + blasfeo_dgemv_t(ny, nx+nu, cost_scaling * b_vec[ii]/num_steps, J_y_tilde, 0, 0, tmp_ny, 0, 1.0, cost_grad, 0, cost_grad, 0); } // cost_hess += b * tmp_nux_ny2 * tmp_nux_ny2^T @@ -1916,6 +1922,12 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo } } // end step loop (ss) + if (opts->cost_computation) + { + // scale cost function value + mem->cost_fun[0] *= cost_scaling; + } + // extract results from forward sweep to output blasfeo_unpack_dvec(nx, xn, 0, x_out, 1); diff --git a/acados/sim/sim_irk_integrator.h b/acados/sim/sim_irk_integrator.h index bcd99f061d..9d38341161 100644 --- a/acados/sim/sim_irk_integrator.h +++ b/acados/sim/sim_irk_integrator.h @@ -166,6 +166,8 @@ typedef struct double *cost_fun; double *outer_hess_is_diag; + double *cost_scaling_ptr; + struct blasfeo_dmat *W_chol; // cholesky factor of weight matrix struct blasfeo_dvec *W_chol_diag; struct blasfeo_dvec *y_ref; // y_ref for NLS cost diff --git a/docs/developer_guide/index.md b/docs/developer_guide/index.md index 354a2fe623..58346a71f6 100644 --- a/docs/developer_guide/index.md +++ b/docs/developer_guide/index.md @@ -98,17 +98,14 @@ The Hessian of the QP is computed in the `ocp_nlp_sqp`, `ocp_nlp_sqp_rti` module The following steps are carried out: - `ocp_nlp_approximate_qp_matrices()` - - sets all Hessian blocks to 0.0 - for `i in range(N+1)` - - if `iTs[i] * opts->levenberg_marquardt` - - add the contribution of the dynamics module (can be turned off via `exact_hess_dyn`) - - if `i==N:` - - add to the diagonal of the Hessian of block `N` the term `opts->levenberg_marquardt`. - - add the cost contribution to the Hessian - - Gauss-Newton Hessian (available in least-squares case) - - or exact Hessian (always used with `EXTERNAL` cost module) if no "custom hessian" is set (see `cost_expr_ext_cost_custom_hess`, `cost_expr_ext_cost_custom_hess_0`, `cost_expr_ext_cost_custom_hess_e`) - - add the inequality constraints contribution to the Hessian (can be turned off via `exact_hess_constr`) + - write the cost contribution into the QP Hessian + - Gauss-Newton Hessian (available in least-squares case) + - or exact Hessian (always used with `EXTERNAL` cost module) if no "custom hessian" is set (see `cost_expr_ext_cost_custom_hess`, `cost_expr_ext_cost_custom_hess_0`, `cost_expr_ext_cost_custom_hess_e`) + - add the contribution of the dynamics module (can be turned off via `exact_hess_dyn`) + - add the inequality constraints contribution to the Hessian (can be turned off via `exact_hess_constr`) +- `ocp_nlp_add_levenberg_marquardt_term()`: + - add to the diagonal of the Hessian of block `i` the term `scaling[i] * opts->levenberg_marquardt` - call the regularization module (`regularize`, see [`regularize_method`](https://docs.acados.org/python_interface/index.html?highlight=regularize#acados_template.acados_ocp_options.AcadosOcpOptions.regularize_method)) diff --git a/examples/acados_matlab_octave/legacy_interface/simple_dae_model/example_ocp.m b/examples/acados_matlab_octave/legacy_interface/simple_dae_model/example_ocp.m index 42ebe4a2cf..091bfda178 100644 --- a/examples/acados_matlab_octave/legacy_interface/simple_dae_model/example_ocp.m +++ b/examples/acados_matlab_octave/legacy_interface/simple_dae_model/example_ocp.m @@ -215,7 +215,7 @@ %end format short e -% get solution for initialization of next NLP +% get solution, e.g. for initialization of next NLP x_traj = ocp_solver.get('x'); u_traj = ocp_solver.get('u'); pi_traj = ocp_solver.get('pi'); diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 21f7692e4d..3c01d7d2a5 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -867,6 +867,9 @@ function make_consistent(self, is_mocp_phase) if ~(strcmp(cost.cost_type_0, "NONLINEAR_LS") || strcmp(cost.cost_type_0, "CONVEX_OVER_NONLINEAR")) error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for initial cost.') end + if strcmp(opts.nlp_solver_type, 'SQP_WITH_FEASIBLE_QP') + error('cost_discretization == INTEGRATOR is not compatible with SQP_WITH_FEASIBLE_QP yet.') + end end diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index fc937a02c2..08b4e99005 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -920,9 +920,11 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None # cost integration if opts.N_horizon > 0: supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] - if opts.cost_discretization == 'INTEGRATOR' and \ - any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): - raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') + if opts.cost_discretization == 'INTEGRATOR': + if any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): + raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') + if opts.nlp_solver_type == "SQP_WITH_FEASIBLE_QP": + raise ValueError('cost_discretization == INTEGRATOR is not compatible with SQP_WITH_FEASIBLE_QP yet.') ## constraints self._make_consistent_constraints_initial() 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 8d9e72be8c..b50f9578fa 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 @@ -3315,7 +3315,13 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul ocp_nlp_get(capsule->nlp_solver, "stat_n", &stat_n); ocp_nlp_get(capsule->nlp_solver, "stat_m", &stat_m); -{% set stat_n_max = 12 %} +{% set stat_n_max = 16 %} + int stat_n_max = {{ stat_n_max }}; + if (stat_n > stat_n_max) + { + printf("stat_n_max = %d is too small, increase it in the template!\n", stat_n_max); + exit(1); + } double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}]; ocp_nlp_get(capsule->nlp_solver, "statistics", stat); From 8621b11ceb72830aa509ca0f67338be4db29d4f3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 3 Jul 2025 09:52:33 +0200 Subject: [PATCH 088/164] Docs: add videos from Saviolo, fix formatting (#1574) --- docs/real_world_examples/index.rst | 36 ++++++++++++++++++++++-------- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index 493717bc0c..8fe8596898 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -1,8 +1,8 @@ .. _real_world_examples: -================== +=================== Real-world examples -================== +=================== This page shows some real-world examples enabled by ``acados``. The list is not complete and is meant to show a variety of domains and applications. @@ -15,7 +15,7 @@ Enjoy! |:popcorn:| .. rubric:: Drone racing: Quadrotor control -|:linked_paperclips:| `Romero et al. (2022) `_ + |:linked_paperclips:| `Romero et al. (2022) `_ .. youtube:: zBVpx3bgI6E :width: 50% @@ -23,7 +23,7 @@ Enjoy! |:popcorn:| .. rubric:: Driving assistance: acados is used within openpilot -See blog post `here `_ + See blog post `here `_ .. youtube:: 0aq4Wi2rsOk :width: 50% @@ -31,7 +31,7 @@ See blog post `here `_ .. rubric:: Bird-like drones -|:linked_paperclips:| `Wüest et al. (2024) `_ + |:linked_paperclips:| `Wüest et al. (2024) `_ .. youtube:: Mv3I3Bv8UyQ :width: 50% @@ -39,7 +39,7 @@ See blog post `here `_ .. rubric:: Flying Hand: End-Effector-Centric Framework for Versatile Aerial Manipulation Teleoperation and Policy Learning -|:linked_paperclips:| `He et al. (2024) `_ + |:linked_paperclips:| `He et al. (2024) `_ .. raw:: html @@ -72,7 +72,8 @@ See blog post `here `_ -```` + ```` + .. rubric:: Swinging up a Custom-Made Furuta Pendulum with NMPC using acados (Slow Motion) .. youtube:: oJYyD5beMqM @@ -80,15 +81,32 @@ See blog post `here `_ :aspect: 9:16 +.. rubric:: Flight Control: Dynamics Learning for Quadrotor Control + |:linked_paperclips:| `Saviolo et al. (2023) `_ + +.. youtube:: QmEhSTcWob4 + :width: 50% + :url_parameters: ?start=64 + + .. rubric:: Flight Control: Overactuated Tiltable-Quadrotors -|:linked_paperclips:| `Li et al. (2024) `_ + |:linked_paperclips:| `Li et al. (2024) `_ .. youtube:: 8_pYdeuQnC0 :width: 50% :url_parameters: ?start=59 + +.. rubric:: Flight Control: Reactive Collision Avoidance for Safe Agile Navigation + |:linked_paperclips:| `Saviolo et al. (2024) `_ + +.. youtube:: pUiWym4NsvA + :width: 50% + :url_parameters: ?start=37 + + .. rubric:: MPC of an Autonomous Warehouse Vehicle with Tricycle Kinematic -|:linked_paperclips:| `Subash et al. (2024) `_ + |:linked_paperclips:| `Subash et al. (2024) `_ .. youtube:: NDta6AD5WCA :width: 50% From 977f41b82f57dd3d1566b0b64d8a24cf61edc9d5 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 4 Jul 2025 17:14:46 +0200 Subject: [PATCH 089/164] Update HPIPM & BLASFEO (#1577) changes in submodules: 1) QP residual computation takes bounds into account -> now don't do it in acados anymore. Added corresponding test. 2) BLASFEO and HPIPM now use minimum CMake 3.5, which mitigates issues with CMake > 4.0 --- .github/workflows/full_build.yml | 14 ++++---- acados/dense_qp/dense_qp_common.c | 26 +++++++------- acados/ocp_qp/ocp_qp_common.c | 34 +++++++++---------- .../main_convex_onesided.py | 5 +++ .../hock_schittkowsky/hs016_test.py | 3 -- external/blasfeo | 2 +- external/hpipm | 2 +- .../acados_template/acados_ocp_solver.py | 10 ++++++ 8 files changed, 54 insertions(+), 42 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 9674ecbf53..c98ea0a514 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -37,7 +37,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 -DCMAKE_POLICY_VERSION_MINIMUM=3.5 + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 - name: Build & Install working-directory: ${{runner.workspace}}/acados/build @@ -166,7 +166,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Export Paths for octave working-directory: ${{runner.workspace}}/acados @@ -283,7 +283,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -342,7 +342,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -403,7 +403,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -458,7 +458,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Run Simulink closed-loop test uses: matlab-actions/run-command@v2 @@ -534,7 +534,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/acados/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE - name: Run CMake Octave tests (ctest) working-directory: ${{runner.workspace}}/acados/build diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index a5d28d2df8..3864504a8c 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -381,19 +381,19 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res // compute residuals d_dense_qp_res_compute(qp_in, qp_out, qp_res, res_ws); - // mask out disregarded constraints - int nb = qp_res->dim->nb; - int nv = qp_res->dim->nv; - int ng = qp_res->dim->ng; - int ns = qp_res->dim->ns; - int ni = nb + ng + ns; - - // stationarity wrt slacks - blasfeo_dvecmul(2*ns, qp_in->d_mask, 2*nb+2*ng, qp_res->res_g, nv, qp_res->res_g, nv); - // ineq - blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_d, 0, qp_res->res_d, 0); - // comp - blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_m, 0, qp_res->res_m, 0); + /* mask out disregarded constraints; NOTE: not needed anymore as done in HPIPM */ + // int nb = qp_res->dim->nb; + // int nv = qp_res->dim->nv; + // int ng = qp_res->dim->ng; + // int ns = qp_res->dim->ns; + // int ni = nb + ng + ns; + + // // stationarity wrt slacks + // blasfeo_dvecmul(2*ns, qp_in->d_mask, 2*nb+2*ng, qp_res->res_g, nv, qp_res->res_g, nv); + // // ineq + // blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_d, 0, qp_res->res_d, 0); + // // comp + // blasfeo_dvecmul(2*ni, qp_in->d_mask, 0, qp_res->res_m, 0, qp_res->res_m, 0); } diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 7557083370..d605101ff5 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -546,12 +546,6 @@ ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws) { - int *nx = qp_res->dim->nx; - int *nu = qp_res->dim->nu; - int *nb = qp_res->dim->nb; - int *ng = qp_res->dim->ng; - int *ns = qp_res->dim->ns; - int ni_stage; qp_info *info = (qp_info *) qp_out->misc; @@ -563,17 +557,23 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res d_ocp_qp_res_compute(qp_in, qp_out, qp_res, res_ws); - // mask out disregarded constraints - for (int ii = 0; ii <= qp_res->dim->N; ii++) - { - ni_stage = ocp_qp_dims_get_ni(qp_res->dim, ii); - // stationarity wrt slacks - blasfeo_dvecmul(2*ns[ii], qp_in->d_mask+ii, 2*nb[ii]+2*ng[ii], &qp_res->res_g[ii], nx[ii]+nu[ii], &qp_res->res_g[ii], nx[ii]+nu[ii]); - // ineq - blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_d[ii], 0, &qp_res->res_d[ii], 0); - // comp - blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_m[ii], 0, &qp_res->res_m[ii], 0); - } + /* mask out disregarded constraints; NOTE: not needed anymore as done in HPIPM */ + // int *nx = qp_res->dim->nx; + // int *nu = qp_res->dim->nu; + // int *nb = qp_res->dim->nb; + // int *ng = qp_res->dim->ng; + // int *ns = qp_res->dim->ns; + // int ni_stage; + // for (int ii = 0; ii <= qp_res->dim->N; ii++) + // { + // ni_stage = ocp_qp_dims_get_ni(qp_res->dim, ii); + // // stationarity wrt slacks + // blasfeo_dvecmul(2*ns[ii], qp_in->d_mask+ii, 2*nb[ii]+2*ng[ii], &qp_res->res_g[ii], nx[ii]+nu[ii], &qp_res->res_g[ii], nx[ii]+nu[ii]); + // // ineq + // blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_d[ii], 0, &qp_res->res_d[ii], 0); + // // comp + // blasfeo_dvecmul(2*ni_stage, qp_in->d_mask+ii, 0, &qp_res->res_m[ii], 0, &qp_res->res_m[ii], 0); + // } return; } diff --git a/examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py b/examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py index 59f7f86fa6..fb34e8b00b 100644 --- a/examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py +++ b/examples/acados_python/convex_ocp_with_onesided_constraints/main_convex_onesided.py @@ -250,6 +250,7 @@ def solve_ocp(modification=1, constraint_formulation="BGH", hessian_approx="EXAC ocp.solver_options.qp_solver_mu0 = 1e3 ocp.solver_options.store_iterates = True ocp.solver_options.eval_residual_at_max_iter = True + ocp.solver_options.nlp_solver_ext_qp_res = 1 # set prediction horizon ocp.solver_options.tf = Tf @@ -269,6 +270,10 @@ def solve_ocp(modification=1, constraint_formulation="BGH", hessian_approx="EXAC ocp_solver.solve() ocp_solver.print_statistics() + qp_res_ineq = ocp_solver.get_stats("qp_res_ineq") + if qp_res_ineq[-1] > 1e-6: + raise ValueError(f"qp_res_ineq at last iteration is {qp_res_ineq[-1]}, which is larger than 1e-6.") + # get solution for i in range(N): sol_X[i,:] = ocp_solver.get(i, "x") diff --git a/examples/acados_python/hock_schittkowsky/hs016_test.py b/examples/acados_python/hock_schittkowsky/hs016_test.py index 58e044fab4..b15114cd17 100644 --- a/examples/acados_python/hock_schittkowsky/hs016_test.py +++ b/examples/acados_python/hock_schittkowsky/hs016_test.py @@ -31,8 +31,6 @@ from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, ACADOS_INFTY, AcadosOcpFlattenedIterate import numpy as np from casadi import * -from matplotlib import pyplot as plt -from itertools import product def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM', scale_qp_constraints: bool = False): @@ -51,7 +49,6 @@ def solve_problem(qp_solver: str = 'FULL_CONDENSING_HPIPM', scale_qp_constraints model.name = f'hs_016' ocp.model = model - # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = 100*(x[1] - x[0]**2)**2 + (1 - x[0])**2 diff --git a/external/blasfeo b/external/blasfeo index e33fe98acc..75078e2b61 160000 --- a/external/blasfeo +++ b/external/blasfeo @@ -1 +1 @@ -Subproject commit e33fe98accb757e7d46611ab3ad2cf65f6f96a6d +Subproject commit 75078e2b6153d1c8bc5329e83a82d4d4d3eefd76 diff --git a/external/hpipm b/external/hpipm index 945645e8c0..36cef134c5 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 945645e8c0ac473f3983fb3c7f22dbae50d864cc +Subproject commit 36cef134c5dbca94088f3dc0a4fc2f57c12486fd diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 05607bd412..3b00e95139 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1610,6 +1610,16 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: elif self.__solver_options['nlp_solver_type'] == 'SQP_RTI': return full_stats[2, :] + elif field_ == "qp_res_ineq": + if not self.__solver_options['nlp_solver_ext_qp_res']: + raise ValueError("qp_res_ineq only supported if nlp_solver_ext_qp_res is enabled.") + full_stats = self.get_stats('statistics') + if self.__solver_options['nlp_solver_type'] == 'SQP': + return full_stats[10, :] + else: + raise ValueError("qp_res_ineq only supported for SQP solver.") + + elif field_ == 'alpha': full_stats = self.get_stats('statistics') if self.__solver_options['nlp_solver_type'] == 'SQP': From 5e736b65fcdc71fc19286f90cbfff3538fecdedf Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 4 Jul 2025 17:15:35 +0200 Subject: [PATCH 090/164] Minor fixes (#1576) --- acados/ocp_nlp/ocp_nlp_common.c | 2 +- interfaces/acados_matlab_octave/AcadosOcp.m | 4 ++++ .../acados_template/acados_casadi_ocp_solver.py | 6 ++++-- .../acados_template/acados_template/acados_ocp.py | 14 ++++++++++---- .../acados_template/acados_ocp_options.py | 4 ++-- .../acados_template/acados_ocp_solver.py | 1 - 6 files changed, 21 insertions(+), 10 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 02ff5d0617..a2de843b0c 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1504,7 +1504,7 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value int* log_dual_step_norm = (int *) value; opts->log_dual_step_norm = *log_dual_step_norm; } - else if (!strcmp(field, "max_iter")) + else if (!strcmp(field, "max_iter") || !strcmp(field, "nlp_solver_max_iter")) { int* max_iter = (int *) value; diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 3c01d7d2a5..86569340a0 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -1079,6 +1079,10 @@ function make_consistent(self, is_mocp_phase) opts.globalization_fixed_step_length = opts.nlp_solver_step_length; end + if opts.globalization_fixed_step_length < 0.0 || opts.globalization_fixed_step_length > 1.0 + error('globalization_fixed_step_length must be in [0, 1].'); + end + % Set default parameters for globalization if isempty(opts.globalization_alpha_min) % if strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index d11184363e..67ac4ad8f6 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -405,7 +405,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): @property def nlp(self): """ - Dict containing all symbolics needed to create a `casadi.nlpsol` solver. + Dict containing all symbolics needed to create a `casadi.nlpsol` solver, namely entries 'x', 'p', 'g', 'f'. """ return self.__nlp @@ -422,7 +422,7 @@ def p_nlp_values(self): Default parameter vector p_nlp in the form of [p_0,..., p_N, p_global] for given NLP. """ return self.__p - + @property def bounds(self): """ @@ -471,6 +471,8 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, # create casadi NLP formulation casadi_nlp_obj = AcadosCasadiOcp(ocp, with_hessian=use_acados_hessian) + self.acados_casadi_ocp = casadi_nlp_obj + self.casadi_nlp = casadi_nlp_obj.nlp self.bounds = casadi_nlp_obj.bounds self.w0 = casadi_nlp_obj.w0 diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 08b4e99005..cc092d78f6 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2155,6 +2155,9 @@ def ensure_solution_sensitivities_available(self, parametric=True) -> None: def get_initial_cost_expression(self): model = self.model if self.cost.cost_type == "LINEAR_LS": + if is_empty(self.cost.Vx_0): + return 0 + y = self.cost.Vx_0 @ model.x + self.cost.Vu_0 @ model.u if not is_empty(self.cost.Vz_0): @@ -2181,6 +2184,9 @@ def get_initial_cost_expression(self): def get_path_cost_expression(self): model = self.model if self.cost.cost_type == "LINEAR_LS": + if is_empty(self.cost.Vx): + return 0 + y = self.cost.Vx @ model.x + self.cost.Vu @ model.u if not is_empty(self.cost.Vz): @@ -2213,18 +2219,18 @@ def get_terminal_cost_expression(self): residual = y - self.cost.yref_e cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) - elif self.cost.cost_type == "NONLINEAR_LS": + elif self.cost.cost_type_e == "NONLINEAR_LS": residual = model.cost_y_expr_e - self.cost.yref_e cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) - elif self.cost.cost_type == "EXTERNAL": + elif self.cost.cost_type_e == "EXTERNAL": cost_dot = model.cost_expr_ext_cost_e - elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": + elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": cost_dot = ca.substitute( model.cost_psi_expr_e, model.cost_r_in_psi_expr_e, model.cost_y_expr_e) else: - raise ValueError("create_model_with_cost_state: Unknown terminal cost type.") + raise ValueError(f"create_model_with_cost_state: Unknown terminal cost type {self.cost.cost_type_e}.") return cost_dot diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 4ced0c5819..01701f3f5e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -1720,10 +1720,10 @@ def cost_discretization(self, cost_discretization): @globalization_fixed_step_length.setter def globalization_fixed_step_length(self, globalization_fixed_step_length): - if isinstance(globalization_fixed_step_length, float) and globalization_fixed_step_length >= 0.: + if isinstance(globalization_fixed_step_length, float) and globalization_fixed_step_length >= 0 or globalization_fixed_step_length <= 1.0: self.__globalization_fixed_step_length = globalization_fixed_step_length else: - raise ValueError('Invalid globalization_fixed_step_length value. globalization_fixed_step_length must be a positive float.') + raise ValueError('Invalid globalization_fixed_step_length value. globalization_fixed_step_length must be a float in [0, 1].') @qpscaling_ub_max_abs_eig.setter def qpscaling_ub_max_abs_eig(self, qpscaling_ub_max_abs_eig): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 3b00e95139..8f772d59d8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -2335,7 +2335,6 @@ def options_set(self, field_, value_): if (field_ == 'max_iter' or field_ == 'nlp_solver_max_iter') and value_ > self.__solver_options['nlp_solver_max_iter']: raise ValueError('AcadosOcpSolver.options_set() cannot increase nlp_solver_max_iter' \ f' above initial value {self.__nlp_solver_max_iter} (you have {value_})') - return if field_ == 'rti_phase': if value_ < 0 or value_ > 2: From 99306c45d32af7891997393a93fc0dd76706209e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 7 Jul 2025 15:50:15 +0200 Subject: [PATCH 091/164] Python: solution sensitivity update (#1578) - deprecate string "params_global" - example: call `setup_qp_matrices_and_factorize`, add variant without squared parameter - Interface: `setup_qp_matrices_and_factorize` also works with `FULL_CONDENSING_HPIPM` - set `dims.nbxe_0 = 0` for `N_horizon = 0` for consistency --- .../non_ocp_example.py | 39 ++++++++++++++----- interfaces/acados_matlab_octave/AcadosOcp.m | 1 + .../acados_template/acados_ocp.py | 1 + .../acados_template/acados_ocp_options.py | 8 +++- .../acados_template/acados_ocp_solver.py | 12 +----- 5 files changed, 41 insertions(+), 20 deletions(-) diff --git a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py index 66346fadc4..b7e93e4f19 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py @@ -35,12 +35,21 @@ import matplotlib.pyplot as plt latexify_plot() -def export_parametric_ocp() -> AcadosOcp: +P_SQUARED = False +if P_SQUARED: + PROBLEM_NAME = "non_ocp_p_squared" +else: + PROBLEM_NAME = "non_ocp_p_linear" + +def export_parametric_nlp() -> AcadosOcp: model = AcadosModel() model.x = ca.SX.sym("x", 1) model.p_global = ca.SX.sym("p_global", 1) - model.cost_expr_ext_cost_e = (model.x - model.p_global**2)**2 + if P_SQUARED: + model.cost_expr_ext_cost_e = (model.x - model.p_global**2)**2 + else: + model.cost_expr_ext_cost_e = (model.x - model.p_global)**2 model.name = "non_ocp" ocp = AcadosOcp() ocp.model = model @@ -64,7 +73,7 @@ def export_parametric_ocp() -> AcadosOcp: def solve_and_compute_sens(p_test, tau): np_test = p_test.shape[0] - ocp = export_parametric_ocp() + ocp = export_parametric_nlp() ocp.solver_options.tau_min = tau ocp.solver_options.qp_solver_t0_init = 0 ocp.solver_options.nlp_solver_max_iter = 2 # QP should converge in one iteration @@ -87,6 +96,11 @@ def solve_and_compute_sens(p_test, tau): # print(f"OCP solver returned status {status} at {i}th p value {p}, {tau=}.") # breakpoint() + status = ocp_solver.setup_qp_matrices_and_factorize() + if status != 0: + ocp_solver.print_statistics() + raise Exception(f"OCP solver returned status {status} in setup_qp_matrices_and_factorize at {i}th p value {p}, {tau=}.") + # Calculate the policy gradient out_dict = ocp_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=True, return_sens_u=False) sens_x[i] = out_dict['sens_x'].item() @@ -124,9 +138,9 @@ def main(): sol_list.append(sol_tau) plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, - title=None, parameter_name=r"$\theta$", fig_filename="solution_sens_non_ocp.pdf") + title=None, parameter_name=r"$\theta$", fig_filename=f"solution_sens_{PROBLEM_NAME}.pdf") plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, - title=None, parameter_name=r"$\theta$", fig_filename="solution_sens_non_ocp_transposed.pdf", horizontal_plot=True) + title=None, parameter_name=r"$\theta$", fig_filename=f"solution_sens_{PROBLEM_NAME}_transposed.pdf", horizontal_plot=True) def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list, title=None, parameter_name="", fig_filename=None, horizontal_plot=False): p_min = p_test[0] @@ -141,10 +155,14 @@ def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list isub = 0 # plot analytic solution - ax[isub].plot([p_min, -1], [1, 1], "k-", linewidth=2, label="analytic") - ax[isub].plot([1, p_max], [1, 1], "k-", linewidth=2) x_vals = np.linspace(-1, 1, 100) - y_vals = x_vals**2 + if P_SQUARED: + ax[isub].plot([p_min, -1], [1, 1], "k-", linewidth=2, label="analytic") + y_vals = x_vals**2 + else: + ax[isub].plot([p_min, -1], [-1, -1], "k-", linewidth=2, label="analytic") + y_vals = x_vals + ax[isub].plot([1, p_max], [1, 1], "k-", linewidth=2) ax[isub].plot(x_vals, y_vals, "k-", linewidth=2) for i, sol in enumerate(sol_list): @@ -160,7 +178,10 @@ def plot_solution_sensitivities_results(p_test, sol_list, sens_list, labels_list # plot analytic sensitivity ax[isub].plot([p_min, -1], [0, 0], "k-", linewidth=2, label="analytic") ax[isub].plot([1, p_max], [0, 0], "k-", linewidth=2) - ax[isub].plot([-1, 1], [-2, 2], "k-", linewidth=2) + if P_SQUARED: + ax[isub].plot([-1, 1], [-2, 2], "k-", linewidth=2) + else: + ax[isub].plot([-1, 1], [1, 1], "k-", linewidth=2) # plot numerical sensitivities for i, sens_x_tau in enumerate(sens_list): diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 86569340a0..c99351cf8d 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -212,6 +212,7 @@ function make_consistent_constraints_initial(self) constraints = self.constraints; model = self.model; if self.solver_options.N_horizon == 0 + dims.nbxe_0 = 0; return end diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index cc092d78f6..87fa749f5e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -384,6 +384,7 @@ def _make_consistent_constraints_initial(self): model = self.model opts = self.solver_options if opts.N_horizon == 0: + dims.nbxe_0 = 0 return nbx_0 = constraints.idxbx_0.shape[0] diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 01701f3f5e..578114bf81 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -853,7 +853,11 @@ def alpha_min(self): @property def reg_epsilon(self): - """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'].""" + """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT']. + + Type: float. + Default: 1e-4. + """ return self.__reg_epsilon @property @@ -1496,6 +1500,8 @@ def globalization(self, globalization): @reg_epsilon.setter def reg_epsilon(self, reg_epsilon): + if not isinstance(reg_epsilon, float) or reg_epsilon < 0: + raise ValueError(f'Invalid reg_epsilon value, expected float >= 0, got {reg_epsilon}') self.__reg_epsilon = reg_epsilon @reg_max_cond_block.setter diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 8f772d59d8..6728ea4d65 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -482,8 +482,8 @@ def setup_qp_matrices_and_factorize(self) -> int: This is only implemented for HPIPM QP solver without condensing. """ - if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM': - raise NotImplementedError('This function is only implemented for PARTIAL_CONDENSING_HPIPM!') + if self.__solver_options["qp_solver"] not in ['PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM']: + raise NotImplementedError('This function is only implemented for PARTIAL_CONDENSING_HPIPM and FULL_CONDENSING_HPIPM!') self.status = getattr(self.shared_lib, f"{self.name}_acados_setup_qp_matrices_and_factorize")(self.capsule) @@ -627,10 +627,6 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st :param with_respect_to: string in ["initial_state", "initial_control", "p_global"] """ - if with_respect_to == "params_global": - print("Deprecation warning: 'params_global' is deprecated and has been renamed to 'p_global'.") - with_respect_to = "p_global" - if with_respect_to == "initial_state": if not self.__has_x0: raise ValueError("OCP does not have an initial state constraint.") @@ -727,10 +723,6 @@ def eval_solution_sensitivity(self, .. note:: Solution sensitivities with respect to parameters are currently implemented only for parametric discrete dynamics, parametric external costs and parametric nonlinear constraints (h). """ - if with_respect_to == "params_global": - print("Deprecation warning: 'params_global' is deprecated and has been renamed to 'p_global'.") - with_respect_to = "p_global" - stages_is_list = isinstance(stages, list) stages_ = stages if stages_is_list else [stages] From 0428b689f36702228dd8c5ca29545b9d9167fa0e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 7 Jul 2025 16:01:14 +0200 Subject: [PATCH 092/164] Python: add flexible `plot_trajectories` function and cleanup examples (#1559) --- .../getting_started/minimal_example_ocp.py | 22 ++- .../acados_template/__init__.py | 2 +- .../acados_template/plot_utils.py | 161 +++++++++++++++++- 3 files changed, 180 insertions(+), 5 deletions(-) diff --git a/examples/acados_python/getting_started/minimal_example_ocp.py b/examples/acados_python/getting_started/minimal_example_ocp.py index ee94e1c4c6..cfe5b98321 100644 --- a/examples/acados_python/getting_started/minimal_example_ocp.py +++ b/examples/acados_python/getting_started/minimal_example_ocp.py @@ -28,11 +28,10 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver +from acados_template import AcadosOcp, AcadosOcpSolver, plot_trajectories from pendulum_model import export_pendulum_ode_model import numpy as np import casadi as ca -from utils import plot_pendulum def main(): # create ocp object to formulate the OCP @@ -102,7 +101,24 @@ def main(): simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") - plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=True, time_label=model.t_label, x_labels=model.x_labels, u_labels=model.u_labels) + + plot_trajectories( + x_traj_list=[simX], + u_traj_list=[simU], + time_traj_list=[np.linspace(0, Tf, N+1)], + time_label=model.t_label, + labels_list=['OCP result'], + x_labels=model.x_labels, + u_labels=model.u_labels, + idxbu=ocp.constraints.idxbu, + lbu=ocp.constraints.lbu, + ubu=ocp.constraints.ubu, + X_ref=None, + U_ref=None, + fig_filename='pendulum_ocp.png', + x_min=None, + x_max=None, + ) if __name__ == '__main__': diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index cbaf54d92f..19f05d64d4 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -53,7 +53,7 @@ from .builders import ocp_get_default_cmake_builder, sim_get_default_cmake_builder -from .plot_utils import latexify_plot, plot_convergence, plot_contraction_rates +from .plot_utils import latexify_plot, plot_convergence, plot_contraction_rates, plot_trajectories from .penalty_utils import symmetric_huber_penalty, one_sided_huber_penalty, huber_loss diff --git a/interfaces/acados_template/acados_template/plot_utils.py b/interfaces/acados_template/acados_template/plot_utils.py index 354ba62121..4dae811e0c 100644 --- a/interfaces/acados_template/acados_template/plot_utils.py +++ b/interfaces/acados_template/acados_template/plot_utils.py @@ -34,7 +34,7 @@ import matplotlib.pyplot as plt import numpy as np -from typing import Optional +from typing import Optional, List def latexify_plot() -> None: text_usetex = True if shutil.which('latex') else False @@ -100,3 +100,162 @@ def plot_contraction_rates(rates_list: list, if fig_filename is not None: plt.savefig(fig_filename, dpi=300, bbox_inches='tight', pad_inches=0.01) plt.show() + + +def plot_trajectories( + x_traj_list: List[np.array], + u_traj_list: List[np.array], + labels_list: List[str], + time_traj_list: List[np.array], + x_labels=None, + u_labels=None, + idxbu=[], + lbu=None, + ubu=None, + X_ref=None, + U_ref=None, + fig_filename=None, + x_min=None, + x_max=None, + title=None, + idxpx=None, + idxpu=None, + color_list=None, + linestyle_list=None, + single_column = False, + alpha_list = None, + time_label = None, + idx_xlogy = None, + show_legend = True, + bbox_to_anchor = None, + ncol_legend = 2, + figsize=None, + show_plot: bool = True, + latexify: bool = True, +): + if latexify: + latexify_plot() + + nx = x_traj_list[0].shape[1] + nu = u_traj_list[0].shape[1] + Ntraj = len(x_traj_list) + + if idxpx is None: + idxpx = list(range(nx)) + if idxpu is None: + idxpu = list(range(nu)) + + if color_list is None: + color_list = [f"C{i}" for i in range(Ntraj)] + if linestyle_list is None: + linestyle_list = Ntraj * ['-'] + if alpha_list is None: + alpha_list = Ntraj * [0.8] + + if idx_xlogy is None: + idx_xlogy = [] + + if time_label is None: + time_label = "$t$" + + if x_labels is None: + x_labels = [f"$x_{i}$" for i in range(nx)] + if u_labels is None: + u_labels = [f"$u_{i}$" for i in range(nu)] + + nxpx = len(idxpx) + nxpu = len(idxpu) + nrows = max(nxpx, nxpu) + + if figsize is None: + if single_column: + figsize = (6.0, 2*(nxpx+nxpu+1)) + else: + figsize = (10, (nxpx+nxpu)) + + if single_column: + fig, axes = plt.subplots(ncols=1, nrows=nxpx+nxpu, figsize=figsize, sharex=True) + else: + fig, axes = plt.subplots(ncols=2, nrows=nrows, figsize=figsize, sharex=True) + axes = np.ravel(axes, order='F') + + if title is not None: + axes[0].set_title(title) + + for i in idxpx: + isubplot = idxpx.index(i) + for x_traj, time_traj, label, color, linestyle, alpha in zip(x_traj_list, time_traj_list, labels_list, color_list, linestyle_list, alpha_list): + axes[isubplot].plot(time_traj, x_traj[:, i], label=label, alpha=alpha, color=color, linestyle=linestyle) + + if X_ref is not None: + axes[isubplot].step( + time_traj_list[0], + X_ref[:, i], + alpha=0.8, + where="post", + label="reference", + linestyle="dotted", + color="k", + ) + axes[isubplot].set_ylabel(x_labels[i]) + axes[isubplot].grid() + axes[isubplot].set_xlim(time_traj_list[0][0], time_traj_list[0][-1]) + + if i in idx_xlogy: + axes[isubplot].set_yscale('log') + + if x_min is not None: + axes[isubplot].set_ylim(bottom=x_min[i]) + + if x_max is not None: + axes[isubplot].set_ylim(top=x_max[i]) + + for i in idxpu: + for u_traj, time_traj, label, color, linestyle, alpha in zip(u_traj_list, time_traj_list, labels_list, color_list, linestyle_list, alpha_list): + vals = u_traj[:, i] + axes[i+nrows].step(time_traj, np.append([vals[0]], vals), label=label, alpha=alpha, color=color, linestyle=linestyle) + + if U_ref is not None: + axes[i+nrows].step(time_traj, np.append([U_ref[0, i]], U_ref[:, i]), alpha=0.8, + label="reference", linestyle="dotted", color="k") + + axes[i+nrows].set_ylabel(u_labels[i]) + axes[i+nrows].grid() + + if i in idxbu: + axes[i+nrows].hlines( + ubu[i], time_traj[0], time_traj[-1], linestyles="dashed", alpha=0.4, color="k" + ) + axes[i+nrows].hlines( + lbu[i], time_traj[0], time_traj[-1], linestyles="dashed", alpha=0.4, color="k" + ) + axes[i+nrows].set_xlim(time_traj[0], time_traj[-1]) + bound_margin = 0.05 + u_lower = (1-bound_margin) * lbu[i] if lbu[i] > 0 else (1+bound_margin) * lbu[i] + axes[i+nrows].set_ylim(bottom=u_lower, top=(1+bound_margin) * ubu[i]) + + axes[nxpx+nxpu-1].set_xlabel(time_label) + if not single_column: + axes[nxpx-1].set_xlabel(time_label) + + if bbox_to_anchor is None and single_column: + bbox_to_anchor=(0.5, -0.75) + elif bbox_to_anchor is None: + bbox_to_anchor=(0.5, -1.5) + + if show_legend: + axes[nxpx+nxpu-1].legend(loc="lower center", ncol=ncol_legend, bbox_to_anchor=bbox_to_anchor) + + fig.align_ylabels() + # fig.tight_layout() + + if not single_column: + for i in range(nxpu, nxpx): + fig.delaxes(axes[i+nrows]) + + if fig_filename is not None: + plt.savefig(fig_filename, bbox_inches="tight", transparent=True, pad_inches=0.05) + print(f"\nstored figure in {fig_filename}") + + if show_plot: + plt.show() From 3cad96ef2a8d5f11da2685f9bf707e0461f53699 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 8 Jul 2025 09:54:18 +0200 Subject: [PATCH 093/164] Investigate split variant of anderson acceleration for slacks (#1579) additionally: - AcadosOcpFlattenedIterate: add __add__, __sub__, inf_norm --- acados/ocp_qp/ocp_qp_common.c | 19 ++- .../furuta_pendulum/convergence_experiment.py | 13 +- .../furuta_pendulum/furuta_common.py | 26 +++- .../slack_eq_convergence_experiment.py | 114 ++++++++++++++++++ .../acados_template/acados_ocp_iterate.py | 32 +++++ 5 files changed, 197 insertions(+), 7 deletions(-) create mode 100644 examples/acados_python/furuta_pendulum/slack_eq_convergence_experiment.py diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index d605101ff5..128067ace5 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -384,7 +384,18 @@ double ocp_qp_out_ddot(ocp_qp_out *x, ocp_qp_out *y, struct blasfeo_dvec *work_t for (int i = 0; i <= N; i++) { // primal + /* standard variant */ +#if 1 out += blasfeo_ddot(nx[i]+nu[i]+2*ns[i], x->ux+i, 0, y->ux+i, 0); +#else + // split variant + out += blasfeo_ddot(nx[i]+nu[i], x->ux+i, 0, y->ux+i, 0); + // slacks + blasfeo_daxpy(ns[i], -1.0, x->ux+i, nx[i]+nu[i], x->ux+i, nx[i]+nu[i]+ns[i], work_tmp_2ni, 0); + blasfeo_daxpy(ns[i], -1.0, y->ux+i, nx[i]+nu[i], y->ux+i, nx[i]+nu[i]+ns[i], work_tmp_2ni, ns[i]); + out += blasfeo_ddot(ns[i], work_tmp_2ni, 0, work_tmp_2ni, ns[i]); +#endif + // dual tmp_nbg = nbu[i]+nbx[i]+ng[i]; /* setup multipliers as lower - upper bound */ @@ -396,8 +407,14 @@ double ocp_qp_out_ddot(ocp_qp_out *x, ocp_qp_out *y, struct blasfeo_dvec *work_t // add dot product out += blasfeo_ddot(tmp_nbg, work_tmp_2ni, 0, work_tmp_2ni, tmp_nbg); // multipliers wrt slack bounds +#if 1 out += blasfeo_ddot(2*ns[i], x->lam+i, 2*tmp_nbg, y->lam+i, 2*tmp_nbg); - +#else + // split variant + blasfeo_daxpy(ns[i], -1.0, x->lam+i, 2*tmp_nbg, x->lam+i, 2*tmp_nbg+ns[i], work_tmp_2ni, 0); + blasfeo_daxpy(ns[i], -1.0, y->lam+i, 2*tmp_nbg, y->lam+i, 2*tmp_nbg+ns[i], work_tmp_2ni, ns[i]); + out += blasfeo_ddot(ns[i], work_tmp_2ni, 0, work_tmp_2ni, ns[i]); +#endif if (i < N) { out += blasfeo_ddot(nx[i+1], x->pi+i, 0, y->pi+i, 0); diff --git a/examples/acados_python/furuta_pendulum/convergence_experiment.py b/examples/acados_python/furuta_pendulum/convergence_experiment.py index 51c55a321d..d506df3218 100644 --- a/examples/acados_python/furuta_pendulum/convergence_experiment.py +++ b/examples/acados_python/furuta_pendulum/convergence_experiment.py @@ -30,22 +30,24 @@ # from furuta_common import setup_ocp_solver +from utils import plot_furuta_pendulum import numpy as np from acados_template import AcadosOcpFlattenedIterate, plot_convergence, plot_contraction_rates from typing import Tuple import matplotlib.pyplot as plt +N_HORIZON = 8 # number of shooting intervals +UMAX = .45 def test_solver(with_anderson_acceleration: bool) -> Tuple[AcadosOcpFlattenedIterate, np.ndarray]: x0 = np.array([0.0, np.pi, 0.0, 0.0]) - umax = .45 Tf = .350 # total prediction time - N_horizon = 8 # number of shooting intervals dt_0 = 0.025 # sampling time = length of first shooting interval - solver = setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, with_anderson_acceleration=with_anderson_acceleration, nlp_solver_max_iter = 500, tol = 1e-8) + solver = setup_ocp_solver(x0, UMAX, dt_0, N_HORIZON, Tf, with_anderson_acceleration=with_anderson_acceleration, nlp_solver_max_iter = 500, tol = 1e-8) + t_grid = solver.acados_ocp.solver_options.shooting_nodes status = solver.solve() solver.print_statistics() @@ -54,7 +56,7 @@ def test_solver(with_anderson_acceleration: bool) -> Tuple[AcadosOcpFlattenedIte res_all = solver.get_stats('res_all') kkt_norms = np.linalg.norm(res_all, axis=1) - return solution, kkt_norms + return solution, kkt_norms, t_grid def raise_test_failure_message(msg: str): # print(f"ERROR: {msg}") @@ -67,7 +69,7 @@ def main(): sol_list = [] labels = [] for with_anderson_acceleration in [True, False]: - sol, kkt_norms = test_solver(with_anderson_acceleration=with_anderson_acceleration) + sol, kkt_norms, t_grid = test_solver(with_anderson_acceleration=with_anderson_acceleration) # compute contraction rates contraction_rates = kkt_norms[1:-1]/kkt_norms[0:-2] # append results @@ -101,6 +103,7 @@ def main(): labels, # fig_filename="contraction_rates_furuta_pendulum.png" ) + plot_furuta_pendulum(t_grid, ref_sol.x.reshape((N_HORIZON + 1, -1)), ref_sol.u.reshape((N_HORIZON, -1)), UMAX) plt.show() if __name__ == "__main__": diff --git a/examples/acados_python/furuta_pendulum/furuta_common.py b/examples/acados_python/furuta_pendulum/furuta_common.py index 52bb649489..afb7136e89 100644 --- a/examples/acados_python/furuta_pendulum/furuta_common.py +++ b/examples/acados_python/furuta_pendulum/furuta_common.py @@ -81,7 +81,7 @@ def get_furuta_model(): return model -def setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, RTI=False, timeout_max_time=0.0, heuristic="ZERO", with_anderson_acceleration=False, nlp_solver_max_iter = 20, tol = 1e-6): +def setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, RTI=False, timeout_max_time=0.0, heuristic="ZERO", with_anderson_acceleration=False, nlp_solver_max_iter = 20, tol = 1e-6, with_abs_cost=False): ocp = AcadosOcp() model = get_furuta_model() @@ -114,6 +114,30 @@ def setup_ocp_solver(x0, umax, dt_0, N_horizon, Tf, RTI=False, timeout_max_time= ocp.constraints.ubu = np.array([+umax]) ocp.constraints.idxbu = np.array([0]) + if with_abs_cost: + val = 1.4 + # add cost term abs(x[0]-val) via slacks + # ocp.constraints.idxbx_e = np.array([0]) + # ocp.constraints.lbx_e = np.array([val]) + # ocp.constraints.ubx_e = np.array([val]) + # ocp.constraints.idxsbx_e = np.array([0]) + + # ocp.cost.zl_e = 1e2 * np.array([1.0]) + # ocp.cost.zu_e = 1e2 * np.array([1.0]) + # ocp.cost.Zl_e = np.array([10.0]) + # ocp.cost.Zu_e = np.array([10.0]) + + ocp.constraints.idxbx = np.array([0]) + ocp.constraints.lbx = np.array([val]) + ocp.constraints.ubx = np.array([val]) + ocp.constraints.idxsbx = np.array([0]) + + ocp.cost.zl = 1e3 * np.array([1.0]) + ocp.cost.zu = 1e3 * np.array([1.0]) + ocp.cost.Zl = 0.0 * np.array([1.0]) + ocp.cost.Zu = 0.0 * np.array([1.0]) + + ocp.constraints.x0 = x0 ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES diff --git a/examples/acados_python/furuta_pendulum/slack_eq_convergence_experiment.py b/examples/acados_python/furuta_pendulum/slack_eq_convergence_experiment.py new file mode 100644 index 0000000000..8bdac431a5 --- /dev/null +++ b/examples/acados_python/furuta_pendulum/slack_eq_convergence_experiment.py @@ -0,0 +1,114 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +from furuta_common import setup_ocp_solver +from utils import plot_furuta_pendulum +import numpy as np + +from acados_template import AcadosOcpFlattenedIterate, plot_convergence, plot_contraction_rates +from typing import Tuple +import matplotlib.pyplot as plt +N_HORIZON = 8 # number of shooting intervals +UMAX = .45 + + +def test_solver(with_anderson_acceleration: bool) -> Tuple[AcadosOcpFlattenedIterate, np.ndarray]: + x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + Tf = .350 # total prediction time + dt_0 = 0.025 # sampling time = length of first shooting interval + + solver = setup_ocp_solver(x0, UMAX, dt_0, N_HORIZON, Tf, with_anderson_acceleration=with_anderson_acceleration, nlp_solver_max_iter = 500, tol = 1e-8, with_abs_cost=True) + t_grid = solver.acados_ocp.solver_options.shooting_nodes + + status = solver.solve() + solver.print_statistics() + solution = solver.store_iterate_to_flat_obj() + + res_all = solver.get_stats('res_all') + kkt_norms = np.linalg.norm(res_all, axis=1) + + return solution, kkt_norms, t_grid + +def raise_test_failure_message(msg: str): + print(f"ERROR: {msg}") + # raise Exception(msg) + +def main(): + # test with anderson acceleration + kkt_norm_list = [] + contraction_rates_list = [] + sol_list = [] + labels = [] + for with_anderson_acceleration in [True, False]: + sol, kkt_norms, t_grid = test_solver(with_anderson_acceleration=with_anderson_acceleration) + # compute contraction rates + contraction_rates = kkt_norms[1:-1]/kkt_norms[0:-2] + # append results + kkt_norm_list.append(kkt_norms) + contraction_rates_list.append(contraction_rates) + sol_list.append(sol) + labels.append("AA(1)-GN" if with_anderson_acceleration else "GN") + # checks + n_iter = len(kkt_norms) + if with_anderson_acceleration: + assert n_iter < 30, f"Expected less than 30 iterations with Anderson acceleration, got {n_iter}" + else: + assert n_iter > 60, f"Expected more than 60 iterations without Anderson acceleration, got {n_iter}" + + # checks + ref_sol = sol_list[0] + for i, sol in enumerate(sol_list[1:]): + if not ref_sol.allclose(sol, atol=1e-4): + print(f"Solution mismatch for {labels[i]}: difference: {(ref_sol-sol).inf_norm()}") + + else: + print(f"Solution for {labels[i]} matches reference solution.") + + # plot results + plot_convergence( + kkt_norm_list, + labels, + # fig_filename="convergence_furuta_pendulum.png" + ) + plot_contraction_rates( + contraction_rates_list, + labels, + # fig_filename="contraction_rates_furuta_pendulum.png" + ) + plot_furuta_pendulum(t_grid, ref_sol.x.reshape((N_HORIZON + 1, -1)), ref_sol.u.reshape((N_HORIZON, -1)), UMAX, plt_show=False) + sol1 = sol_list[1] + plot_furuta_pendulum(t_grid, sol1.x.reshape((N_HORIZON + 1, -1)), sol1.u.reshape((N_HORIZON, -1)), UMAX, plt_show=False) + plt.show() + +if __name__ == "__main__": + main() + diff --git a/interfaces/acados_template/acados_template/acados_ocp_iterate.py b/interfaces/acados_template/acados_template/acados_ocp_iterate.py index 1a2a4be88b..b42010e7ea 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_iterate.py +++ b/interfaces/acados_template/acados_template/acados_ocp_iterate.py @@ -61,6 +61,38 @@ def allclose(self, other, rtol=1e-5, atol=1e-6) -> bool: np.allclose(self.lam, other.lam, rtol=rtol, atol=atol) ) + def __add__(self, other): + if not isinstance(other, AcadosOcpFlattenedIterate): + raise TypeError(f"Expected AcadosOcpFlattenedIterate, got {type(other)}") + return AcadosOcpFlattenedIterate( + x=self.x + other.x, + u=self.u + other.u, + z=self.z + other.z, + sl=self.sl + other.sl, + su=self.su + other.su, + pi=self.pi + other.pi, + lam=self.lam + other.lam + ) + + def __sub__(self, other): + if not isinstance(other, AcadosOcpFlattenedIterate): + raise TypeError(f"Expected AcadosOcpFlattenedIterate, got {type(other)}") + return AcadosOcpFlattenedIterate( + x=self.x - other.x, + u=self.u - other.u, + z=self.z - other.z, + sl=self.sl - other.sl, + su=self.su - other.su, + pi=self.pi - other.pi, + lam=self.lam - other.lam + ) + + def inf_norm(self) -> float: + """ + Returns the infinity norm of the iterate, which is the maximum absolute value of its elements. + """ + return np.max(np.abs(np.concatenate((self.x, self.u, self.z, self.sl, self.su, self.pi, self.lam)))) + @dataclass class AcadosOcpFlattenedBatchIterate: From d20759cd1114c2cd2c6a48fbded6194717168ff1 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Tue, 8 Jul 2025 10:14:48 +0200 Subject: [PATCH 094/164] Python: allow suppressing the note about `OpenMP` in `AcadosOcpBatchSolver` (#1580) When `Verbose` is set to `False`, to reduce the amount of printing in projects such as `leap-c` --- .../acados_template/acados_ocp_batch_solver.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 80b5948e79..066c542dda 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -115,7 +115,9 @@ def __init__(self, ocp: AcadosOcp, N_batch_max: int, msg += "i.e. with the flags -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1.\n" + \ "See https://github.com/acados/acados/pull/1089 for more details." - print(msg) + + if verbose: + print(msg) @property From ff2e83f339df0346266f6fe27c023fcca3f53076 Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Tue, 8 Jul 2025 14:50:34 +0200 Subject: [PATCH 095/164] Moved Second-Order Correction to function in `ocp_nlp_common` (#1582) The Second-Order Correction functionality was moved from `ocp_nlp_globalization_merit_backtracking` to `ocp_nlp_common` such that it can be used for other globalized algorithms as well. --- acados/ocp_nlp/ocp_nlp_common.c | 161 ++++++++++++++++++ acados/ocp_nlp/ocp_nlp_common.h | 5 + ...ocp_nlp_globalization_merit_backtracking.c | 154 +---------------- 3 files changed, 170 insertions(+), 150 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index a2de843b0c..a1809a7070 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -4135,6 +4135,167 @@ void ocp_nlp_common_eval_lagr_grad_p(ocp_nlp_config *config, ocp_nlp_dims *dims, } } +int ocp_nlp_perform_second_order_correction(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, + ocp_qp_in *qp_in, ocp_qp_out *qp_out) +{ + // Second Order Correction (SOC): following Nocedal2006: p.557, eq. (18.51) -- (18.56) + // Paragraph: APPROACH III: S l1 QP (SEQUENTIAL l1 QUADRATIC PROGRAMMING), + // Section 18.8 TRUST-REGION SQP METHODS + // - just no trust region radius here. + if (nlp_opts->print_level > 0) + { + printf("performing SOC\n\n"); + } + int ii; + int *nb = qp_in->dim->nb; + int *ng = qp_in->dim->ng; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int N = dims->N; + ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; + // int *nv = dims->nv; + // int *ni = dims->ni; + + /* evaluate constraints & dynamics at new step */ + // NOTE: setting up the new iterate and evaluating is not needed here, + // since this evaluation was perfomed just before this call in the early terminated line search. + + // NOTE: similar to ocp_nlp_evaluate_merit_fun + // update QP rhs + // d_i = c_i(x_k + p_k) - \nabla c_i(x_k)^T * p_k + struct blasfeo_dvec *tmp_fun_vec; + + for (ii = 0; ii <= N; ii++) + { + if (ii < N) + { + // b -- dynamics + tmp_fun_vec = config->dynamics[ii]->memory_get_fun_ptr(nlp_mem->dynamics[ii]); + // add - \nabla c_i(x_k)^T * p_k + // c_i = f(x_k, u_k) - x_{k+1} (see dynamics module) + blasfeo_dgemv_t(nx[ii]+nu[ii], nx[ii+1], -1.0, qp_in->BAbt+ii, 0, 0, + qp_out->ux+ii, 0, -1.0, tmp_fun_vec, 0, qp_in->b+ii, 0); + // NOTE: not sure why it is - tmp_fun_vec here! + blasfeo_dvecad(nx[ii+1], 1.0, qp_out->ux+ii+1, nu[ii+1], qp_in->b+ii, 0); + } + + /* INEQUALITIES */ + // d -- constraints + tmp_fun_vec = config->constraints[ii]->memory_get_fun_ptr(nlp_mem->constraints[ii]); + /* SOC for bounds can be skipped (because linear) */ + // NOTE: SOC can also be skipped for truely linear constraint, i.e. ng of nlp, + // now using ng of QP = (nh+ng) + + // upper & lower + blasfeo_dveccp(ng[ii], tmp_fun_vec, nb[ii], qp_in->d+ii, nb[ii]); // lg + blasfeo_dveccp(ng[ii], tmp_fun_vec, 2*nb[ii]+ng[ii], qp_in->d+ii, 2*nb[ii]+ng[ii]); // ug + // general linear / linearized! + // tmp_2ni = D * u + C * x + blasfeo_dgemv_t(nu[ii]+nx[ii], ng[ii], 1.0, qp_in->DCt+ii, 0, 0, qp_out->ux+ii, 0, + 0.0, &nlp_work->tmp_2ni, 0, &nlp_work->tmp_2ni, 0); + // d[nb:nb+ng] += tmp_2ni (lower) + blasfeo_dvecad(ng[ii], 1.0, &nlp_work->tmp_2ni, 0, qp_in->d+ii, nb[ii]); + // d[nb:nb+ng] -= tmp_2ni + blasfeo_dvecad(ng[ii], -1.0, &nlp_work->tmp_2ni, 0, qp_in->d+ii, 2*nb[ii]+ng[ii]); + + // add slack contributions + // d[nb:nb+ng] += slack[idx] + // qp_in->idxs_rev + for (int j = 0; j < nb[ii]+ng[ii]; j++) + { + int slack_index = qp_in->idxs_rev[ii][j]; + if (slack_index >= 0) + { + // add slack contribution for lower and upper constraint + // lower + BLASFEO_DVECEL(qp_in->d+ii, j) -= + BLASFEO_DVECEL(qp_out->ux+ii, slack_index+nx[ii]+nu[ii]); + // upper + BLASFEO_DVECEL(qp_in->d+ii, j+nb[ii]+ng[ii]) -= + BLASFEO_DVECEL(qp_out->ux+ii, slack_index+nx[ii]+nu[ii]+ns[ii]); + } + } + + // NOTE: bounds on slacks can be skipped, since they are linear. + // blasfeo_daxpy(2*ns[ii], -1.0, qp_out->ux+ii, nx[ii]+nu[ii], qp_in->d+ii, 2*nb[ii]+2*ng[ii], qp_in->d+ii, 2*nb[ii]+2*ng[ii]); + + // printf("SOC: qp_in->d final value\n"); + // blasfeo_print_exp_dvec(2*nb[ii]+2*ng[ii], qp_in->d+ii, 0); + } + + if (nlp_opts->print_level > 3) + { + printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", nlp_mem->iter); + // print_ocp_qp_in(qp_in); + } + +#if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 1); +#endif + + // solve QP + // acados_tic(&timer1); + int qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out, + nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); + // NOTE: QP is not timed, since this computation time is attributed to globalization. + + // compute correct dual solution in case of Hessian regularization + config->regularize->correct_dual_sol(config->regularize, dims->regularize, + nlp_opts->regularize, nlp_mem->regularize_mem); + + // ocp_qp_out_get(qp_out, "qp_info", &qp_info_); + // int qp_iter = qp_info_->num_iter; + + // save statistics of last qp solver call + // TODO: SOC QP solver call should be warm / hot started! + // if (nlp_mem->iter+1 < nlp_mem->stat_m) + // { + // // mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; + // // add qp_iter; should maybe be in a seperate statistic + // nlp_mem->stat[nlp_mem->stat_n*(nlp_mem->iter+1)+5] += qp_iter; + // } + + // compute external QP residuals (for debugging) + // if (nlp_opts->ext_qp_res) + // { + // ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); + // if (nlp_mem->iter+1 < nlp_mem->stat_m) + // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(nlp_mem->iter+1)+7)); + // } + + // if (nlp_opts->print_level > 3) + // { + // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", nlp_mem->iter); + // print_ocp_qp_out(qp_out); + // } + +#if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 1); +#endif + + // exit conditions on QP status + if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) + { +#ifndef ACADOS_SILENT + printf("\nQP solver returned error status %d in SQP iteration %d for SOC QP.\n", + qp_status, nlp_mem->iter); +#endif + // if (nlp_opts->print_level > 1) + // { + // printf("\nFailed to solve the following QP:\n"); + // if (nlp_opts->print_level > 3) + // print_ocp_qp_in(qp_in); + // } + nlp_mem->status = ACADOS_QP_FAILURE; + + return 1; + } + + return 0; +} int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index a573bd514b..988aa5462e 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -640,6 +640,11 @@ double ocp_nlp_compute_dual_lam_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_ou // double ocp_nlp_get_l1_infeasibility(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_memory *nlp_mem); // +int ocp_nlp_perform_second_order_correction(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, + ocp_qp_in *qp_in, ocp_qp_out *qp_out); +// int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, bool precondensed_lhs, ocp_qp_in *scaled_qp_in_, ocp_qp_in *qp_in_, ocp_qp_out *scaled_qp_out_,ocp_qp_out *qp_out_, diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 13415d0574..e9611e9966 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -565,10 +565,6 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp static bool ocp_nlp_soc_line_search(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) { - int ii; - int N = dims->N; - - ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; ocp_qp_in *qp_in = nlp_mem->qp_in; ocp_qp_out *qp_out = nlp_mem->qp_out; ocp_nlp_globalization_merit_backtracking_memory *merit_mem = nlp_mem->globalization; @@ -594,155 +590,13 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, return false; } // else perform SOC (below) - - // Second Order Correction (SOC): following Nocedal2006: p.557, eq. (18.51) -- (18.56) - // Paragraph: APPROACH III: S l1 QP (SEQUENTIAL l1 QUADRATIC PROGRAMMING), - // Section 18.8 TRUST-REGION SQP METHODS - // - just no trust region radius here. - if (nlp_opts->print_level > 0) - printf("ocp_nlp_sqp: performing SOC, since prelim. line search returned %d\n\n", line_search_status); - int *nb = qp_in->dim->nb; - int *ng = qp_in->dim->ng; - int *nx = dims->nx; - int *nu = dims->nu; - int *ns = dims->ns; - // int *nv = dims->nv; - // int *ni = dims->ni; - - /* evaluate constraints & dynamics at new step */ - // NOTE: setting up the new iterate and evaluating is not needed here, - // since this evaluation was perfomed just before this call in the early terminated line search. - - // NOTE: similar to ocp_nlp_evaluate_merit_fun - // update QP rhs - // d_i = c_i(x_k + p_k) - \nabla c_i(x_k)^T * p_k - struct blasfeo_dvec *tmp_fun_vec; - - for (ii = 0; ii <= N; ii++) + int soc_status = ocp_nlp_perform_second_order_correction(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, qp_in, qp_out); + // line search does not care about status in soc?? + if (soc_status == ACADOS_SUCCESS) { - if (ii < N) - { - // b -- dynamics - tmp_fun_vec = config->dynamics[ii]->memory_get_fun_ptr(nlp_mem->dynamics[ii]); - // add - \nabla c_i(x_k)^T * p_k - // c_i = f(x_k, u_k) - x_{k+1} (see dynamics module) - blasfeo_dgemv_t(nx[ii]+nu[ii], nx[ii+1], -1.0, qp_in->BAbt+ii, 0, 0, - qp_out->ux+ii, 0, -1.0, tmp_fun_vec, 0, qp_in->b+ii, 0); - // NOTE: not sure why it is - tmp_fun_vec here! - blasfeo_dvecad(nx[ii+1], 1.0, qp_out->ux+ii+1, nu[ii+1], qp_in->b+ii, 0); - } - - /* INEQUALITIES */ - // d -- constraints - tmp_fun_vec = config->constraints[ii]->memory_get_fun_ptr(nlp_mem->constraints[ii]); - /* SOC for bounds can be skipped (because linear) */ - // NOTE: SOC can also be skipped for truely linear constraint, i.e. ng of nlp, - // now using ng of QP = (nh+ng) - - // upper & lower - blasfeo_dveccp(ng[ii], tmp_fun_vec, nb[ii], qp_in->d+ii, nb[ii]); // lg - blasfeo_dveccp(ng[ii], tmp_fun_vec, 2*nb[ii]+ng[ii], qp_in->d+ii, 2*nb[ii]+ng[ii]); // ug - // general linear / linearized! - // tmp_2ni = D * u + C * x - blasfeo_dgemv_t(nu[ii]+nx[ii], ng[ii], 1.0, qp_in->DCt+ii, 0, 0, qp_out->ux+ii, 0, - 0.0, &nlp_work->tmp_2ni, 0, &nlp_work->tmp_2ni, 0); - // d[nb:nb+ng] += tmp_2ni (lower) - blasfeo_dvecad(ng[ii], 1.0, &nlp_work->tmp_2ni, 0, qp_in->d+ii, nb[ii]); - // d[nb:nb+ng] -= tmp_2ni - blasfeo_dvecad(ng[ii], -1.0, &nlp_work->tmp_2ni, 0, qp_in->d+ii, 2*nb[ii]+ng[ii]); - - // add slack contributions - // d[nb:nb+ng] += slack[idx] - // qp_in->idxs_rev - for (int j = 0; j < nb[ii]+ng[ii]; j++) - { - int slack_index = qp_in->idxs_rev[ii][j]; - if (slack_index >= 0) - { - // add slack contribution for lower and upper constraint - // lower - BLASFEO_DVECEL(qp_in->d+ii, j) -= - BLASFEO_DVECEL(qp_out->ux+ii, slack_index+nx[ii]+nu[ii]); - // upper - BLASFEO_DVECEL(qp_in->d+ii, j+nb[ii]+ng[ii]) -= - BLASFEO_DVECEL(qp_out->ux+ii, slack_index+nx[ii]+nu[ii]+ns[ii]); - } - } - - // NOTE: bounds on slacks can be skipped, since they are linear. - // blasfeo_daxpy(2*ns[ii], -1.0, qp_out->ux+ii, nx[ii]+nu[ii], qp_in->d+ii, 2*nb[ii]+2*ng[ii], qp_in->d+ii, 2*nb[ii]+2*ng[ii]); - - // printf("SOC: qp_in->d final value\n"); - // blasfeo_print_exp_dvec(2*nb[ii]+2*ng[ii], qp_in->d+ii, 0); - } - - if (nlp_opts->print_level > 3) - { - printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", nlp_mem->iter); - // print_ocp_qp_in(qp_in); - } - -#if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 1); -#endif - - // solve QP - // acados_tic(&timer1); - int qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out, - nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); - // NOTE: QP is not timed, since this computation time is attributed to globalization. - - // compute correct dual solution in case of Hessian regularization - config->regularize->correct_dual_sol(config->regularize, dims->regularize, - nlp_opts->regularize, nlp_mem->regularize_mem); - - // ocp_qp_out_get(qp_out, "qp_info", &qp_info_); - // int qp_iter = qp_info_->num_iter; - - // save statistics of last qp solver call - // TODO: SOC QP solver call should be warm / hot started! - // if (nlp_mem->iter+1 < nlp_mem->stat_m) - // { - // // mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; - // // add qp_iter; should maybe be in a seperate statistic - // nlp_mem->stat[nlp_mem->stat_n*(nlp_mem->iter+1)+5] += qp_iter; - // } - - // compute external QP residuals (for debugging) - // if (nlp_opts->ext_qp_res) - // { - // ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); - // if (nlp_mem->iter+1 < nlp_mem->stat_m) - // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(nlp_mem->iter+1)+7)); - // } - - // if (nlp_opts->print_level > 3) - // { - // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", nlp_mem->iter); - // print_ocp_qp_out(qp_out); - // } - -#if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 1); -#endif - - // exit conditions on QP status - if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) - { -#ifndef ACADOS_SILENT - printf("\nQP solver returned error status %d in SQP iteration %d for SOC QP.\n", - qp_status, nlp_mem->iter); -#endif - // if (nlp_opts->print_level > 1) - // { - // printf("\nFailed to solve the following QP:\n"); - // if (nlp_opts->print_level > 3) - // print_ocp_qp_in(qp_in); - // } - nlp_mem->status = ACADOS_QP_FAILURE; - return true; } + return true; } From c26cec533fc865b513fd96791623f093bdad0c56 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 8 Jul 2025 15:53:44 +0200 Subject: [PATCH 096/164] qpDUNES: fix Python and test on actions (#1581) --- .github/workflows/full_build.yml | 2 +- .../ocp/example_ocp_dynamics_formulations.py | 18 ++- .../ocp/example_sqp_qpDUNES.py | 117 ------------------ interfaces/CMakeLists.txt | 23 ++-- interfaces/acados_matlab_octave/AcadosOcp.m | 11 +- .../acados_template/acados_ocp.py | 5 +- 6 files changed, 34 insertions(+), 142 deletions(-) delete mode 100644 examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index c98ea0a514..d4c9a2a525 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -16,7 +16,7 @@ env: ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON ACADOS_WITH_DAQP: ON - ACADOS_WITH_QPDUNES: OFF + ACADOS_WITH_QPDUNES: ON ACADOS_ON_CI: ON jobs: diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py index 97c4ce8e32..3498e3812e 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py @@ -43,7 +43,9 @@ INTEGRATOR_TYPES = ['ERK', 'IRK', 'GNSF', 'DISCRETE'] BUILD_SYSTEMS = ['cmake', 'make'] - +QP_SOLVERS = ['PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ + 'FULL_CONDENSING_DAQP'] if __name__ == "__main__": parser = argparse.ArgumentParser(description='test Python interface on pendulum example.') parser.add_argument('--INTEGRATOR_TYPE', dest='INTEGRATOR_TYPE', default="ERK", @@ -51,6 +53,8 @@ parser.add_argument('--BUILD_SYSTEM', dest='BUILD_SYSTEM', default='make', help=f'BUILD_SYSTEM: supports {BUILD_SYSTEMS}') + parser.add_argument('--QP_SOLVER', dest='QP_SOLVER', default='PARTIAL_CONDENSING_HPIPM', + help=f'QP_SOLVER: supports {QP_SOLVERS}') args = parser.parse_args() @@ -66,6 +70,12 @@ f' {BUILD_SYSTEMS}, got {build_system}.' raise Exception(msg) + qp_solver = args.QP_SOLVER + if qp_solver not in QP_SOLVERS: + msg = f'Invalid unit test value {qp_solver} for parameter QP_SOLVER. Possible values are' \ + f' {QP_SOLVERS}, got {qp_solver}.' + raise Exception(msg) + # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -91,6 +101,10 @@ # set cost Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2*np.diag([1e-2]) + if qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + # NOTE: qpDUNES requires very similar values on diag of hessian. + Q = 2*np.diag([1e3, 1e3, 1e-0, 1e-0]) + R = 2*np.diag([1e-0]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) @@ -114,7 +128,7 @@ ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.idxbu = np.array([0]) - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.qp_solver = qp_solver ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = integrator_type ocp.solver_options.print_level = 1 diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py b/examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py deleted file mode 100644 index 264a09d639..0000000000 --- a/examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py +++ /dev/null @@ -1,117 +0,0 @@ -# -# 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.; -# - -import sys -sys.path.insert(0, '../common') - -from acados_template import AcadosOcp, AcadosOcpSolver -from pendulum_model import export_pendulum_ode_model -import numpy as np -import scipy.linalg -from utils import plot_pendulum - -# create ocp object to formulate the OCP -ocp = AcadosOcp() - -# set model -model = export_pendulum_ode_model() -ocp.model = model - -Tf = 1.0 -nx = model.x.rows() -nu = model.u.rows() -ny = nx + nu -ny_e = nx -N = 20 - -# set dimensions -ocp.solver_options.N_horizon = N - -# set cost -# NOTE: qpDUNES requires very similar values on diag of hessian. -Q = 2*np.diag([1e3, 1e3, 1e-0, 1e-0]) -R = 2*np.diag([1e-0]) - -ocp.cost.W_e = Q -ocp.cost.W = scipy.linalg.block_diag(Q, R) - -ocp.cost.cost_type = 'LINEAR_LS' -ocp.cost.cost_type_e = 'LINEAR_LS' - -ocp.cost.Vx = np.zeros((ny, nx)) -ocp.cost.Vx[:nx,:nx] = np.eye(nx) - -Vu = np.zeros((ny, nu)) -Vu[4,0] = 1.0 -ocp.cost.Vu = Vu - -ocp.cost.Vx_e = np.eye(nx) - -ocp.cost.yref = np.zeros((ny, )) -ocp.cost.yref_e = np.zeros((ny_e, )) - -# set constraints -Fmax = 80 -ocp.constraints.lbu = np.array([-Fmax]) -ocp.constraints.ubu = np.array([+Fmax]) -ocp.constraints.idxbu = np.array([0]) - -ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) - -# set options -ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_QPDUNES' # FULL_CONDENSING_QPOASES, PARTIAL_CONDENSING_HPIPM, PARTIAL_CONDENSING_OSQP, 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 - -# set prediction horizon -ocp.solver_options.tf = Tf - -ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - -simX = np.zeros((N+1, nx)) -simU = np.zeros((N, nu)) - -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}.') - -# get solution -for i in range(N): - simX[i,:] = ocp_solver.get(i, "x") - 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") - -plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index fbce7c55eb..70a295c047 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -132,6 +132,7 @@ if(ACADOS_OCTAVE) set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS octave_test_OSQP) endif() if(ACADOS_WITH_QPDUNES) + set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS octave_test_qpDUNES) set_tests_properties(octave_test_OSQP PROPERTIES DEPENDS octave_test_qpDUNES) endif() endif() @@ -316,13 +317,6 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/multiphase_nonlinear_constraints python main.py) - # qpDUNES example - if(ACADOS_WITH_QPDUNES) - add_test(NAME python_qpDUNES_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_sqp_qpDUNES.py) - endif() - # OSQP test if(ACADOS_WITH_OSQP) add_test(NAME python_OSQP_test @@ -397,9 +391,13 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_pendulum_ocp_IRK COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=IRK) - add_test(NAME python_pendulum_ocp_ERK - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=ERK) + + if(ACADOS_WITH_QPDUNES) + add_test(NAME python_pendulum_ocp_ERK_qpDUNES + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp + python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=ERK --QP_SOLVER=PARTIAL_CONDENSING_QPDUNES) + endif() + add_test(NAME python_pendulum_ocp_GNSF COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=GNSF) @@ -512,13 +510,12 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_render_simulink_wrapper PROPERTIES DEPENDS python_constraints_expression_example) set_tests_properties(python_constraints_expression_example PROPERTIES DEPENDS pendulum_optimal_value_gradient) set_tests_properties(pendulum_optimal_value_gradient PROPERTIES DEPENDS python_pendulum_ocp_IRK) - set_tests_properties(python_pendulum_ocp_IRK PROPERTIES DEPENDS python_pendulum_ocp_ERK) - set_tests_properties(python_pendulum_ocp_ERK PROPERTIES DEPENDS python_pendulum_ocp_GNSF) + set_tests_properties(python_pendulum_ocp_IRK PROPERTIES DEPENDS python_pendulum_ocp_GNSF) set_tests_properties(python_pendulum_ocp_GNSF PROPERTIES DEPENDS python_example_ocp_dynamics_formulations_cmake) set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS py_qp_scaling_time_opt_swingup) if(ACADOS_WITH_QPDUNES) - set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS python_qpDUNES_test) + set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS python_pendulum_ocp_ERK_qpDUNES) endif() endif() diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index c99351cf8d..3ecb327241 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -875,6 +875,11 @@ function make_consistent(self, is_mocp_phase) %% constraints + % qpdunes + if ~isempty(strfind(opts.qp_solver, 'QPDUNES')) + constraints.idxbxe_0 = []; + dims.nbxe_0 = 0; + end self.make_consistent_constraints_initial(); self.make_consistent_constraints_path(); self.make_consistent_constraints_terminal(); @@ -913,12 +918,6 @@ function make_consistent(self, is_mocp_phase) self.make_consistent_simulation(); - % qpdunes - if ~isempty(strfind(opts.qp_solver,'qpdunes')) - constraints.idxbxe_0 = []; - dims.nbxe_0 = 0; - end - if strcmp(opts.qp_solver, "PARTIAL_CONDENSING_HPMPC") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_QPDUNES") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_OOQP") diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 87fa749f5e..4afb4a296a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -928,6 +928,8 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None raise ValueError('cost_discretization == INTEGRATOR is not compatible with SQP_WITH_FEASIBLE_QP yet.') ## constraints + if opts.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + self.remove_x0_elimination() self._make_consistent_constraints_initial() self._make_consistent_constraints_path() self._make_consistent_constraints_terminal() @@ -958,9 +960,6 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None self._make_consistent_simulation() - if opts.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': - self.remove_x0_elimination() - # fixed hessian if opts.fixed_hess: if opts.hessian_approx == 'EXACT': From fe0c3854c663dc7630e5dca064537ddd15a36b31 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Jul 2025 11:38:25 +0200 Subject: [PATCH 097/164] Fix memory alignment issue in convex-over-nonlinear constraint module (#1584) --- acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 1 + acados/utils/mem.c | 25 ++++++++++++++++++++++++ acados/utils/mem.h | 3 +++ 3 files changed, 29 insertions(+) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index ed81e44276..3366c132b4 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -316,6 +316,7 @@ acados_size_t ocp_nlp_constraints_bgp_model_calculate_size(void *config, void *d size += blasfeo_memsize_dmat(nu + nx, ng); // DCt size += 64; // blasfeo_mem align + make_int_multiple_of(8, &size); return size; } diff --git a/acados/utils/mem.c b/acados/utils/mem.c index 4adac29706..befdbb7055 100644 --- a/acados/utils/mem.c +++ b/acados/utils/mem.c @@ -227,3 +227,28 @@ void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, *ptr += sA->memsize; #endif } + + +void print_pointer_alignment(char **ptr) +{ + if ((size_t) *ptr % 64 == 0) + { + printf("Pointer is aligned to 64 bytes\n"); + } + else if ((size_t) *ptr % 32 == 0) + { + printf("Pointer is aligned to 32 bytes\n"); + } + else if ((size_t) *ptr % 16 == 0) + { + printf("Pointer is aligned to 16 bytes\n"); + } + else if ((size_t) *ptr % 8 == 0) + { + printf("Pointer is aligned to 8 bytes\n"); + } + else + { + printf("Pointer is NOT aligned to 8 bytes\n"); + } +} diff --git a/acados/utils/mem.h b/acados/utils/mem.h index 809d2c8f00..ee792375eb 100644 --- a/acados/utils/mem.h +++ b/acados/utils/mem.h @@ -91,6 +91,9 @@ void assign_and_advance_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char ** // allocate strmat and advance pointer void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr); +// print pointer alignment +void print_pointer_alignment(char **ptr); + #ifdef __cplusplus } /* extern "C" */ #endif From 082a1d8f083bcfc9fd5c3e2a33f33e515466ea9e Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Thu, 10 Jul 2025 12:46:08 +0200 Subject: [PATCH 098/164] Bug fix in FUNNEL globalization (#1586) The predicted reduction in the merit function in the penalty phase was not calculated correctly. For the `NOMINAL`-step mode, the previous version was correct, but for `BYRD_OMOJOKUN`, it was wrong. --- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index ea84e5a601..46363c2cfe 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -440,14 +440,14 @@ int backtracking_line_search(ocp_nlp_config *config, double current_infeasibility = mem->l1_infeasibility; double current_cost = nlp_mem->cost_value; - // do the penalty parameter update here .... might be changed later mem->penalty_parameter = nlp_mem->objective_multiplier; - print_debug_output_double("pred_optimality", pred_optimality, nlp_opts->print_level, 2); - print_debug_output_double("pred_infeasibility", pred_infeasibility, nlp_opts->print_level, 2); update_funnel_penalty_parameter(mem, opts, nlp_opts, pred_optimality, pred_infeasibility); double current_merit = mem->penalty_parameter*current_cost + current_infeasibility; // Shouldn't this be the update below?? nlp_mem->objective_multiplier = mem->penalty_parameter; + print_debug_output_double("pred_optimality", pred_optimality, nlp_opts->print_level, 2); + print_debug_output_double("pred_infeasibility", pred_infeasibility, nlp_opts->print_level, 2); + int i; while (true) @@ -506,7 +506,7 @@ int backtracking_line_search(ocp_nlp_config *config, /////////////////////////////////////////////////////////////////////// // Evaluate merit function at trial point double trial_merit = mem->penalty_parameter*trial_cost + trial_infeasibility; - pred_merit = mem->penalty_parameter * pred_optimality + current_infeasibility; + pred_merit = mem->penalty_parameter * pred_optimality + pred_infeasibility; ared = nlp_mem->cost_value - trial_cost; // Funnel globalization From bb8c5e1ef93e31ca0458febce52943a3fbd9dd67 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 11 Jul 2025 15:35:18 +0200 Subject: [PATCH 099/164] Update docstring on `setup_qp_matrices_and_factorize` (#1587) -`setup_qp_matrices_and_factorize` is needed regardless of two-solver approach. Otherwise the Hessian factorization corresponds to the one of the second to last QP iterate, instead of the last one. This can be quite inaccurate, see the examples in #1578 - Additionally: initialize option in C --- acados/ocp_nlp/ocp_nlp_common.c | 2 ++ interfaces/acados_template/acados_template/acados_ocp_solver.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index a1809a7070..cca5e135b4 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1253,8 +1253,10 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) constraints[i]->opts_initialize_default(constraints[i], dims->constraints[i], opts->constraints[i]); } + // solution sens opts->with_solution_sens_wrt_params = 0; opts->with_value_sens_wrt_params = 0; + opts->solution_sens_qp_t_lam_min = 1e-9; // adaptive Levenberg-Marquardt options opts->adaptive_levenberg_marquardt_mu_min = 1e-16; diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 6728ea4d65..4d7d2c4153 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -478,7 +478,7 @@ def setup_qp_matrices_and_factorize(self) -> int: The t and lambda values are clipped according to `solution_sens_qp_t_lam_min` to avoid ill-conditioning. Then the Hessian matrix is factorized. - If a two-solver approach is used to obtain solution sensitivities from acados, this function should be called before calling `eval_solution_sensitivity`, or `eval_adjoint_solution_sensitivity()`. + To obtain accurate solution sensitivities from acados, this function should be called before calling `eval_solution_sensitivity`, or `eval_adjoint_solution_sensitivity()`. This is only implemented for HPIPM QP solver without condensing. """ From f590390a26b34ed2871190d820123c65128443ba Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 14 Jul 2025 12:55:41 +0200 Subject: [PATCH 100/164] Python: convert `TERA_PATH` to absolute path (#1561) Fixes https://github.com/acados/tera_renderer/issues/10 at least for some people. I think relative paths lead to more issues down the line, but at least with better error messages. --- interfaces/acados_template/acados_template/utils.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index afca4d2db8..22b210b60b 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -99,6 +99,9 @@ def get_tera_exec_path(): TERA_PATH = os.environ.get('TERA_PATH') if not TERA_PATH: TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer') + get_binary_ext() + + # convert to absolute path + TERA_PATH = os.path.abspath(TERA_PATH) return TERA_PATH From aa78aa0d8d56a119ecbbff937c5ad1ffe6919c5f Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Mon, 14 Jul 2025 12:56:39 +0200 Subject: [PATCH 101/164] Major bug fix in `FUNNEL_L1PEN_LINESEARCH` (#1588) The Armijo condition was wrong for the `f`-type steps and also for the `p`-type steps. This was corrected by introducing a funtion that checks the Armijo condition and which can be used either for `f`- or `p`-type steps. --- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 90 +++++++++---------- acados/ocp_nlp/ocp_nlp_globalization_funnel.h | 10 +-- 2 files changed, 50 insertions(+), 50 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index 46363c2cfe..b685bb95bc 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -239,22 +239,22 @@ void initialize_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *me void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, ocp_nlp_opts *nlp_opts, - double pred_optimality, - double pred_infeasibility) + double predicted_reduction_objective, + double predicted_reduction_infeasibility) { print_debug_output("-- Objective Multiplier Update: \n", nlp_opts->print_level, 1); - print_debug_output_double("left hand side: ", mem->penalty_parameter * pred_optimality + pred_infeasibility, nlp_opts->print_level, 2); - print_debug_output_double("right hand side: ", opts->penalty_eta * pred_infeasibility, nlp_opts->print_level, 2); + print_debug_output_double("left hand side: ", mem->penalty_parameter * predicted_reduction_objective + predicted_reduction_infeasibility, nlp_opts->print_level, 2); + print_debug_output_double("right hand side: ", opts->penalty_eta * predicted_reduction_infeasibility, nlp_opts->print_level, 2); //TODO(david): What do we do here to make it correct? We would like to avoid numerical noise - if (pred_optimality < 0 && pred_optimality > -1e-4) + if (predicted_reduction_objective < 0 && predicted_reduction_objective > -1e-4) { - pred_optimality = 0.0; + predicted_reduction_objective = 0.0; } - if (mem->penalty_parameter * pred_optimality + pred_infeasibility < opts->penalty_eta * pred_infeasibility) + if (mem->penalty_parameter * predicted_reduction_objective + predicted_reduction_infeasibility < opts->penalty_eta * predicted_reduction_infeasibility) { mem->penalty_parameter = MAX(0.0, //objective multiplier should always be >= 0! MIN(opts->penalty_contraction * mem->penalty_parameter, - ((1-opts->penalty_eta) * pred_infeasibility) / (-pred_optimality + 1e-9)) + ((1-opts->penalty_eta) * predicted_reduction_infeasibility) / (-predicted_reduction_objective + 1e-9)) ); } assert(mem->penalty_parameter >= 0.0); @@ -290,11 +290,9 @@ bool is_funnel_sufficient_decrease_satisfied(ocp_nlp_globalization_funnel_memory } } -bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double pred_optimality, double step_size, double pred_infeasibility) +bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double predicted_reduction_objective, double step_size, double predicted_reduction_infeasibility) { - // if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility) - // if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility * pred_infeasibility) - if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility) + if (step_size * predicted_reduction_objective >= opts->fraction_switching_condition * predicted_reduction_infeasibility) { return true; } @@ -304,12 +302,10 @@ bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, d } } -bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts, - double negative_ared, - double pred, - double alpha) +bool is_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts, + double actual_reduction, double predicted_reduction, double alpha) { - if (negative_ared <= MIN(globalization_opts->eps_sufficient_descent * alpha * MAX(pred, 0) + 1e-18, 0)) + if (actual_reduction >= globalization_opts->eps_sufficient_descent * alpha * MAX(0.0, predicted_reduction-1e-9)) { return true; } @@ -321,15 +317,17 @@ bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalizat bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_opts *nlp_opts, - double pred, double ared, double alpha, - double current_infeasibility, - double trial_infeasibility, - double current_objective, - double trial_objective, - double current_merit, - double trial_merit, - double pred_merit, - double pred_infeasibility) + double predicted_reduction_objective, + double actual_reduction_objective, + double alpha, + double current_infeasibility, + double trial_infeasibility, + double current_objective, + double trial_objective, + double current_merit, + double trial_merit, + double predicted_reduction_merit, + double predicted_reduction_infeasibility) { ocp_nlp_globalization_funnel_opts *opts = nlp_opts->globalization; ocp_nlp_globalization_opts *globalization_opts = opts->globalization_opts; @@ -339,7 +337,14 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * print_debug_output_double("current infeasibility", current_infeasibility, nlp_opts->print_level, 2); print_debug_output_double("trial objective", trial_objective, nlp_opts->print_level, 2); print_debug_output_double("trial infeasibility", trial_infeasibility, nlp_opts->print_level, 2); - print_debug_output_double("pred", pred, nlp_opts->print_level, 2); + print_debug_output_double("predicted_reduction_objective", predicted_reduction_objective, nlp_opts->print_level, 2); + print_debug_output_double("predicted_reduction_infeasibility", predicted_reduction_infeasibility, nlp_opts->print_level, 2); + print_debug_output_double("predicted_reduction_merit", predicted_reduction_merit, nlp_opts->print_level, 2); + + if (alpha < 1.0 && trial_infeasibility > current_infeasibility) + { + printf("IPOPT would trigger SOC!\n"); + } if (opts->use_merit_fun_only) // We only check the penalty method but not the funnel! { @@ -352,10 +357,10 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * if (!mem->funnel_penalty_mode) { print_debug_output("Penalty Mode not active!\n", nlp_opts->print_level, 1); - if (is_switching_condition_satisfied(opts, pred, alpha, pred_infeasibility)) + if (is_switching_condition_satisfied(opts, predicted_reduction_objective, alpha, predicted_reduction_infeasibility)) { print_debug_output("Switching condition IS satisfied!\n", nlp_opts->print_level, 1); - if (is_f_type_armijo_condition_satisfied(globalization_opts, -ared, pred, alpha)) + if (is_armijo_condition_satisfied(globalization_opts, actual_reduction_objective, predicted_reduction_objective, alpha)) { print_debug_output("f-type step: Armijo condition satisfied\n", nlp_opts->print_level, 1); accept_step = true; @@ -365,7 +370,6 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * { print_debug_output("f-type step: Armijo condition NOT satisfied\n", nlp_opts->print_level, 1); } - } else if (is_funnel_sufficient_decrease_satisfied(mem, opts, trial_infeasibility)) { @@ -379,8 +383,7 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * { print_debug_output("Switching condition is NOT satisfied!\n", nlp_opts->print_level, 1); print_debug_output("Entered penalty check!\n", nlp_opts->print_level, 1); - //TODO move to function and test more - if (trial_merit <= current_merit + globalization_opts->eps_sufficient_descent * alpha * pred_merit) + if (trial_infeasibility < current_infeasibility && is_armijo_condition_satisfied(globalization_opts, current_merit-trial_merit, predicted_reduction_merit, alpha)) { print_debug_output("Penalty Function accepted\n", nlp_opts->print_level, 1); accept_step = true; @@ -392,7 +395,7 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * else { print_debug_output("Penalty mode active\n", nlp_opts->print_level,1); - if (trial_merit <= current_merit + globalization_opts->eps_sufficient_descent * alpha * pred_merit) + if (is_armijo_condition_satisfied(globalization_opts, current_merit-trial_merit, predicted_reduction_merit, alpha)) { print_debug_output("p-type step: accepted iterate\n", nlp_opts->print_level, 1); accept_step = true; @@ -429,25 +432,22 @@ int backtracking_line_search(ocp_nlp_config *config, ocp_nlp_globalization_funnel_memory *mem = nlp_mem->globalization; int N = dims->N; - double pred_merit = 0.0; // Calculate this here - double pred_optimality = nlp_mem->predicted_optimality_reduction; - double pred_infeasibility = nlp_mem->predicted_infeasibility_reduction; + double predicted_reduction_merit = 0.0; // Initialize this here + double predicted_reduction_objective = nlp_mem->predicted_optimality_reduction; + double predicted_reduction_infeasibility = nlp_mem->predicted_infeasibility_reduction; double alpha = 1.0; double trial_cost; double trial_infeasibility = 0.0; - double ared; + double actual_reduction_objective; bool accept_step; double current_infeasibility = mem->l1_infeasibility; double current_cost = nlp_mem->cost_value; mem->penalty_parameter = nlp_mem->objective_multiplier; - update_funnel_penalty_parameter(mem, opts, nlp_opts, pred_optimality, pred_infeasibility); + update_funnel_penalty_parameter(mem, opts, nlp_opts, predicted_reduction_objective, predicted_reduction_infeasibility); double current_merit = mem->penalty_parameter*current_cost + current_infeasibility; // Shouldn't this be the update below?? nlp_mem->objective_multiplier = mem->penalty_parameter; - print_debug_output_double("pred_optimality", pred_optimality, nlp_opts->print_level, 2); - print_debug_output_double("pred_infeasibility", pred_infeasibility, nlp_opts->print_level, 2); - int i; while (true) @@ -506,16 +506,16 @@ int backtracking_line_search(ocp_nlp_config *config, /////////////////////////////////////////////////////////////////////// // Evaluate merit function at trial point double trial_merit = mem->penalty_parameter*trial_cost + trial_infeasibility; - pred_merit = mem->penalty_parameter * pred_optimality + pred_infeasibility; - ared = nlp_mem->cost_value - trial_cost; + predicted_reduction_merit = mem->penalty_parameter * predicted_reduction_objective + predicted_reduction_infeasibility; + actual_reduction_objective = nlp_mem->cost_value - trial_cost; // Funnel globalization accept_step = is_trial_iterate_acceptable_to_funnel(mem, nlp_opts, - pred_optimality, ared, + predicted_reduction_objective, actual_reduction_objective, alpha, current_infeasibility, trial_infeasibility, current_cost, trial_cost, current_merit, trial_merit, - pred_merit, pred_infeasibility); + predicted_reduction_merit, predicted_reduction_infeasibility); if (accept_step) { diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.h b/acados/ocp_nlp/ocp_nlp_globalization_funnel.h index b262f4dbd1..362aacbcc1 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.h +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.h @@ -119,14 +119,14 @@ bool is_funnel_sufficient_decrease_satisfied(ocp_nlp_globalization_funnel_memory // bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double pred_optimality, double step_size, double pred_infeasibility); // -bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts, - double negative_ared, - double pred, - double alpha); +bool is_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts, + double ared, double pred, double alpha); // bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_opts *nlp_opts, - double pred, double ared, double alpha, + double pred_optimality, + double ared_optimality, + double alpha, double current_infeasibility, double trial_infeasibility, double current_objective, From ef8577614bc0d82c77db191f5fa2ca1dfb451ead Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Mon, 14 Jul 2025 12:57:38 +0200 Subject: [PATCH 102/164] MATLAB: add solver status checks to `getting_started` examples (#1589) The closed-loop example was returning non-zero status (`ACADOS_MAXITER`) so I changed the solver settings. Also made the printing in `detect_constraint_structure` slightly more understandable. --- .../getting_started/extensive_example_ocp.m | 1 + .../minimal_example_closed_loop.m | 2 ++ .../getting_started/minimal_example_ocp.m | 1 + .../detect_constraint_structure.m | 32 +++++++++++-------- 4 files changed, 22 insertions(+), 14 deletions(-) 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 cfcdea4848..29434efd62 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -234,6 +234,7 @@ % evaluation status = ocp_solver.get('status'); + assert(status == 0, sprintf('solver failed with status %d', status)) sqp_iter = ocp_solver.get('sqp_iter'); time_tot(i) = ocp_solver.get('time_tot'); time_lin(i) = ocp_solver.get('time_lin'); diff --git a/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m b/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m index 60e7766f27..29a1203c19 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m @@ -59,6 +59,7 @@ % FULL_CONDENSING_QPOASES, PARTIAL_CONDENSING_OSQP ocp.solver_options.qp_solver_cond_N = 5; % for partial condensing ocp.solver_options.globalization = 'MERIT_BACKTRACKING'; % turns on globalization +ocp.solver_options.nlp_solver_max_iter = 200; % we add some model-plant mismatch by choosing different integration % methods for model (within the OCP) and plant: @@ -184,6 +185,7 @@ % get solution u0 = ocp_solver.get('u', 0); status = ocp_solver.get('status'); % 0 - success + assert(status == 0, sprintf('solver failed with status %d', status)) % simulate one step x_sim(:,i+1) = sim_solver.simulate(x0, u0); diff --git a/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m b/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m index 9c00c3eb43..9eadc43878 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m @@ -131,6 +131,7 @@ xtraj = ocp_solver.get('x'); status = ocp_solver.get('status'); % 0 - success +assert(status == 0, sprintf('solver failed with status %d', status)) ocp_solver.print('stat') %% plots diff --git a/interfaces/acados_matlab_octave/detect_constraint_structure.m b/interfaces/acados_matlab_octave/detect_constraint_structure.m index 7778eca4d5..791df2ee6d 100644 --- a/interfaces/acados_matlab_octave/detect_constraint_structure.m +++ b/interfaces/acados_matlab_octave/detect_constraint_structure.m @@ -50,7 +50,7 @@ if isa(x, 'casadi.SX') isSX = true; else - error('constraint detection only works for casadi.SX!'); + error('Constraint detection only works for casadi.SX!'); end if strcmp(stage_type, 'initial') @@ -79,7 +79,7 @@ if ~(isa(expr_constr, 'casadi.SX') || isa(expr_constr, 'casadi.MX')) disp('expr_constr =') disp(expr_constr) - error("Constraint type detection require definition of constraints as CasADi SX or MX.") + error("Constraint type detection requires definition of constraints as CasADi SX or MX.") end % initialize @@ -108,8 +108,9 @@ constr_expr_h = vertcat(constr_expr_h, c); lh = [ lh; LB(ii)]; uh = [ uh; UB(ii)]; - disp(['constraint ', num2str(ii), ' is kept as nonlinear constraint.']); - disp(c); + disp(['Constraint ', num2str(ii), ' is kept as a nonlinear constraint.']); + disp('Constraint expression: '); + disp(c) disp(' ') else % c is linear in x and u Jc_fun = Function('Jc_fun', {x(1)}, {jacobian(c, [x;u])}); @@ -124,9 +125,10 @@ Jbx(end, idb) = 1; lbx = [lbx; LB(ii)/Jc(idb)]; ubx = [ubx; UB(ii)/Jc(idb)]; - disp(['constraint ', num2str(ii),... - ' is reformulated as bound on x.']); - disp(c); + disp(['Constraint ', num2str(ii),... + ' is reformulated as a bound on x.']); + disp('Constraint expression: '); + disp(c) disp(' ') else % bound on u; @@ -134,9 +136,10 @@ Jbu(end, idb-nx) = 1; lbu = [lbu; LB(ii)/Jc(idb)]; ubu = [ubu; UB(ii)/Jc(idb)]; - disp(['constraint ', num2str(ii),... - ' is reformulated as bound on u.']); - disp(c); + disp(['Constraint ', num2str(ii),... + ' is reformulated as a bound on u.']); + disp('Constraint expression: '); + disp(c) disp(' ') end else @@ -145,9 +148,10 @@ D = [D; Jc(nx+1:end)]; lg = [ lg; LB(ii)]; ug = [ ug; UB(ii)]; - disp(['constraint ', num2str(ii),... - ' is reformulated as general linear constraint.']); - disp(c); + disp(['Constraint ', num2str(ii),... + ' is reformulated as a general linear constraint.']); + disp('Constraint expression: '); + disp(c) disp(' ') end end @@ -157,7 +161,7 @@ if strcmp(stage_type, 'terminal') % checks if any(expr_constr.which_depends(u)) || ~isempty(lbu) || (~isempty(D) && any(D)) - error('terminal constraint may not depend on control input.'); + error('Terminal constraint may not depend on control input.'); end % h constraints.constr_type_e = 'BGH'; From fbb8e5c6978a6963e1dea06787bbc01e21bb980b Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Mon, 14 Jul 2025 17:37:16 +0200 Subject: [PATCH 103/164] Added `qp_out` as function input to `config->step_update` (#1590) The `config->step_update` takes `qp_out` as input such that it is more modular. This enables Second-Order Corrections to use the same function for a step update while using different data structures to store the data. --- acados/ocp_nlp/ocp_nlp_common.c | 19 +++++++---------- acados/ocp_nlp/ocp_nlp_common.h | 4 ++-- acados/ocp_nlp/ocp_nlp_ddp.c | 21 ++++++++++--------- acados/ocp_nlp/ocp_nlp_ddp.h | 2 +- .../ocp_nlp_globalization_fixed_step.c | 2 +- acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 9 ++------ ...ocp_nlp_globalization_merit_backtracking.c | 6 +++--- 7 files changed, 28 insertions(+), 35 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index cca5e135b4..1017de037c 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -3213,11 +3213,12 @@ calculates new iterate or trial iterate in 'out_destination' with step 'mem->qp_ step size 'alpha', and current iterate 'out_start'. */ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, - void *in_, void *out_, void *opts_, void *mem_, + void *in_, void *out_, void *qp_out_, void *opts_, void *mem_, void *work_, void *out_destination_, void *solver_mem, double alpha, bool full_step_dual) { ocp_nlp_dims *dims = dims_; + ocp_qp_out *qp_out = qp_out_; ocp_nlp_out *out_start = out_; ocp_nlp_memory *mem = mem_; ocp_nlp_out *out_destination = out_destination_; @@ -3236,28 +3237,24 @@ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, for (int i = 0; i <= N; i++) { // step in primal variables - blasfeo_daxpy(nv[i], alpha, mem->qp_out->ux + i, 0, out_start->ux + i, 0, out_destination->ux + i, 0); + blasfeo_daxpy(nv[i], alpha, qp_out->ux + i, 0, out_start->ux + i, 0, out_destination->ux + i, 0); // update dual variables if (full_step_dual) { - blasfeo_dveccp(2*ni[i], mem->qp_out->lam+i, 0, out_destination->lam+i, 0); + blasfeo_dveccp(2*ni[i], qp_out->lam+i, 0, out_destination->lam+i, 0); if (i < N) { - blasfeo_dveccp(nx[i+1], mem->qp_out->pi+i, 0, out_destination->pi+i, 0); + blasfeo_dveccp(nx[i+1], qp_out->pi+i, 0, out_destination->pi+i, 0); } } else { // update duals with alpha step - blasfeo_daxpby(2*ni[i], 1.0-alpha, out_start->lam+i, 0, alpha, mem->qp_out->lam+i, 0, out_destination->lam+i, 0); - // blasfeo_dvecsc(2*ni[i], 1.0-alpha, out->lam+i, 0); - // blasfeo_daxpy(2*ni[i], alpha, mem->qp_out->lam+i, 0, out->lam+i, 0, out->lam+i, 0); + blasfeo_daxpby(2*ni[i], 1.0-alpha, out_start->lam+i, 0, alpha, qp_out->lam+i, 0, out_destination->lam+i, 0); if (i < N) { - // blasfeo_dvecsc(nx[i+1], 1.0-alpha, out->pi+i, 0); - // blasfeo_daxpy(nx[i+1], alpha, mem->qp_out->pi+i, 0, out->pi+i, 0, out->pi+i, 0); - blasfeo_daxpby(nx[i+1], 1.0-alpha, out_start->pi+i, 0, alpha, mem->qp_out->pi+i, 0, out_destination->pi+i, 0); + blasfeo_daxpby(nx[i+1], 1.0-alpha, out_start->pi+i, 0, alpha, qp_out->pi+i, 0, out_destination->pi+i, 0); } } @@ -3266,7 +3263,7 @@ void ocp_nlp_update_variables_sqp(void *config_, void *dims_, { // out->z = mem->z_alg + alpha * dzdux * qp_out->ux blasfeo_dgemv_t(nu[i]+nx[i], nz[i], alpha, mem->dzduxt+i, 0, 0, - mem->qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); + qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); } } #if defined(ACADOS_DEVELOPER_DEBUG_CHECKS) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 988aa5462e..3337edd575 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -99,7 +99,7 @@ typedef struct ocp_nlp_config void *opts_, void *mem_, void *work_, void *sens_nlp_out, const char *field, int stage, void *grad_p); void (*step_update)(void *config, void *dims, void *in, - void *out_start, void *opts, void *mem, void *work, + void *out_start, void *qp_out, void *opts, void *mem, void *work, void *out_destination, void* solver_mem, double alpha, bool full_step_dual); // prepare memory int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); @@ -580,7 +580,7 @@ void ocp_nlp_level_c_update(ocp_nlp_config *config, ocp_nlp_memory *mem, ocp_nlp_workspace *work); // void ocp_nlp_update_variables_sqp(void *config_, void *dims_, - void *in_, void *out_, void *opts_, void *mem_, void *work_, + void *in_, void *out_, void *qp_out_, void *opts_, void *mem_, void *work_, void *out_destination_, void *solver_mem, double alpha, bool full_step_dual); // void ocp_nlp_convert_primaldelta_absdual_step_to_delta_step(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 index 1cd68cb1c0..ebc30b0e23 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -332,7 +332,7 @@ static void ocp_nlp_ddp_cast_workspace(ocp_nlp_config *config, ocp_nlp_dims *dim ************************************************/ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, - void *in_, void *out_, void *opts_, void *mem_, + void *in_, void *out_, void *qp_out_, void *opts_, void *mem_, void *work_, void *out_destination_, void *solver_mem, double alpha, bool full_step_dual) { @@ -340,6 +340,7 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, ocp_nlp_dims *dims = dims_; ocp_nlp_in *in = in_; ocp_nlp_out *out = out_; + ocp_qp_out *qp_out = qp_out_; ocp_nlp_opts *opts = opts_; ocp_nlp_memory *mem = mem_; ocp_nlp_workspace *work = work_; @@ -359,7 +360,7 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, // compute x_0 int i = 0; - blasfeo_daxpy(nx[i], alpha, mem->qp_out->ux + i, nu[i], + blasfeo_daxpy(nx[i], alpha, qp_out->ux + i, nu[i], out->ux + i, nu[i], out_destination->ux + i, nu[i]); // compute u_i, x_{i+1} @@ -369,11 +370,11 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, // compute u // (if i < N?) /* u_i = \bar{u}_i + alpha * k_i + K_i * (x_i - \bar{x}_i) */ // get K - xcond_solver_config->solver_get(xcond_solver_config, mem->qp_in, mem->qp_out, opts->qp_solver_opts, mem->qp_solver_mem, "K", i, ddp_mem->tmp_nu_times_nx, nu[i], nx[i]); + xcond_solver_config->solver_get(xcond_solver_config, mem->qp_in, qp_out, opts->qp_solver_opts, mem->qp_solver_mem, "K", i, ddp_mem->tmp_nu_times_nx, nu[i], nx[i]); blasfeo_pack_dmat(nu[i], nx[i], ddp_mem->tmp_nu_times_nx, nu[i], &ddp_mem->K_mat, 0, 0); // get k = tmp_nv; - xcond_solver_config->solver_get(xcond_solver_config, mem->qp_in, mem->qp_out, opts->qp_solver_opts, mem->qp_solver_mem, "k", i, work->tmp_nv_double, nu[i], 1); + xcond_solver_config->solver_get(xcond_solver_config, mem->qp_in, qp_out, opts->qp_solver_opts, mem->qp_solver_mem, "k", i, work->tmp_nv_double, nu[i], 1); blasfeo_pack_dvec(nu[i], work->tmp_nv_double, 1, &work->tmp_nv, 0); // compute delta_u = alpha * k_i + K_i * (x_i - \bar{x}_i) @@ -400,21 +401,21 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, // update dual variables if (globalization_opts->full_step_dual) { - blasfeo_dveccp(2*ni[i], mem->qp_out->lam+i, 0, out_destination->lam+i, 0); + blasfeo_dveccp(2*ni[i], qp_out->lam+i, 0, out_destination->lam+i, 0); if (i < N) { - blasfeo_dveccp(nx[i+1], mem->qp_out->pi+i, 0, out_destination->pi+i, 0); + blasfeo_dveccp(nx[i+1], qp_out->pi+i, 0, out_destination->pi+i, 0); } } else { // update duals with alpha step blasfeo_dvecsc(2*ni[i], 1.0-alpha, out_destination->lam+i, 0); - blasfeo_daxpy(2*ni[i], alpha, mem->qp_out->lam+i, 0, out_destination->lam+i, 0, out_destination->lam+i, 0); + blasfeo_daxpy(2*ni[i], alpha, qp_out->lam+i, 0, out_destination->lam+i, 0, out_destination->lam+i, 0); if (i < N) { blasfeo_dvecsc(nx[i+1], 1.0-alpha, out_destination->pi+i, 0); - blasfeo_daxpy(nx[i+1], alpha, mem->qp_out->pi+i, 0, out_destination->pi+i, 0, out_destination->pi+i, 0); + blasfeo_daxpy(nx[i+1], alpha, qp_out->pi+i, 0, out_destination->pi+i, 0, out_destination->pi+i, 0); } } @@ -423,7 +424,7 @@ void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, { // out_destination->z = mem->z_alg + alpha * dzdux * qp_out->ux blasfeo_dgemv_t(nu[i]+nx[i], nz[i], alpha, mem->dzduxt+i, 0, 0, - mem->qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); + qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out_destination->z+i, 0); } } return; @@ -781,7 +782,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { // Accept the forward simulation to get feasible initial guess mem->alpha = 1.0; // full step to obtain feasible initial guess - ocp_nlp_ddp_compute_trial_iterate(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_work->tmp_nlp_out, mem, mem->alpha, 1.0); + ocp_nlp_ddp_compute_trial_iterate(config, dims, nlp_in, nlp_out, qp_out, nlp_opts, nlp_mem, nlp_work, nlp_work->tmp_nlp_out, mem, mem->alpha, 1.0); copy_ocp_nlp_out(dims, work->nlp_work->tmp_nlp_out, nlp_out); infeasible_initial_guess = false; } diff --git a/acados/ocp_nlp/ocp_nlp_ddp.h b/acados/ocp_nlp/ocp_nlp_ddp.h index 332428c9f8..c9c86cefed 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.h +++ b/acados/ocp_nlp/ocp_nlp_ddp.h @@ -143,7 +143,7 @@ void ocp_nlp_ddp_eval_lagr_grad_p(void *config_, void *dims_, void *nlp_in_, voi void ocp_nlp_ddp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_); // void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_, - void *in_, void *out_, void *opts_, void *mem_, + void *in_, void *out_, void *qp_out_, void *opts_, void *mem_, void *work_, void *out_destination_, void *solver_mem, double alpha, bool full_step_dual); diff --git a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c index 64d7ba0324..e0f0ee5998 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c @@ -210,7 +210,7 @@ int ocp_nlp_globalization_fixed_step_find_acceptable_iterate(void *nlp_config_, } else { - config->step_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, alpha, opts->globalization_opts->full_step_dual); + config->step_update(config, dims, nlp_in, nlp_out, qp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, alpha, opts->globalization_opts->full_step_dual); } *step_size = alpha; diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index b685bb95bc..0dd2041861 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -341,11 +341,6 @@ bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory * print_debug_output_double("predicted_reduction_infeasibility", predicted_reduction_infeasibility, nlp_opts->print_level, 2); print_debug_output_double("predicted_reduction_merit", predicted_reduction_merit, nlp_opts->print_level, 2); - if (alpha < 1.0 && trial_infeasibility > current_infeasibility) - { - printf("IPOPT would trigger SOC!\n"); - } - if (opts->use_merit_fun_only) // We only check the penalty method but not the funnel! { mem->funnel_penalty_mode = true; @@ -453,8 +448,8 @@ int backtracking_line_search(ocp_nlp_config *config, while (true) { // Calculate trial iterate: trial_iterate = current_iterate + alpha * direction - config->step_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, - nlp_work, nlp_work->tmp_nlp_out, solver_mem, alpha, globalization_opts->full_step_dual); + config->step_update(config, dims, nlp_in, nlp_out, nlp_mem->qp_out, nlp_opts, nlp_mem, + nlp_work, nlp_work->tmp_nlp_out, solver_mem, alpha, globalization_opts->full_step_dual); /////////////////////////////////////////////////////////////////////// // Evaluate cost function at trial iterate diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index e9611e9966..350cba03fd 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -773,8 +773,8 @@ static int ocp_nlp_ddp_backtracking_line_search(ocp_nlp_config *config, ocp_nlp_ while (true) { // Do the DDP forward sweep to get the trial iterate - config->step_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, - nlp_work, nlp_work->tmp_nlp_out, solver_mem, alpha, globalization_opts->full_step_dual); + config->step_update(config, dims, nlp_in, nlp_out, nlp_mem->qp_out, nlp_opts, nlp_mem, + nlp_work, nlp_work->tmp_nlp_out, solver_mem, alpha, globalization_opts->full_step_dual); /////////////////////////////////////////////////////////////////////// // Evaluate cost function at trial iterate @@ -890,7 +890,7 @@ int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_c } // update variables - nlp_config->step_update(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, mem->alpha, globalization_opts->full_step_dual); + nlp_config->step_update(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_mem->qp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, mem->alpha, globalization_opts->full_step_dual); *step_size = mem->alpha; return ACADOS_SUCCESS; } From 20e5146b3108e4cb89e89bb19150adb06996f00c Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Wed, 16 Jul 2025 10:44:51 +0200 Subject: [PATCH 104/164] Change of default `globalization_alpha_reduction` factor (#1592) The default alpha reduction factor was changed. The `FUNNEL_L1PEN_LINESEARCH` method used `0.5` and the `MERIT_BACKTRACKING` used `0.7`. Now both methods use `0.7` and it works very well for the funnel. --- interfaces/acados_matlab_octave/AcadosOcp.m | 18 ------------------ .../acados_matlab_octave/AcadosOcpOptions.m | 2 +- .../acados_template/acados_ocp.py | 6 ------ .../acados_template/acados_ocp_options.py | 12 ++++-------- 4 files changed, 5 insertions(+), 33 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 3ecb327241..32a07a5700 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -1019,15 +1019,6 @@ function make_consistent(self, is_mocp_phase) end end - if isempty(opts.globalization_alpha_reduction) - % if strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') - if ddp_with_merit_or_funnel - opts.globalization_alpha_reduction = 0.5; - else - opts.globalization_alpha_reduction = 0.7; - end - end - if isempty(opts.globalization_eps_sufficient_descent) % if strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') if ddp_with_merit_or_funnel @@ -1093,15 +1084,6 @@ function make_consistent(self, is_mocp_phase) end end - if isempty(opts.globalization_alpha_reduction) - % if strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') - if ddp_with_merit_or_funnel - opts.globalization_alpha_reduction = 0.5; - else - opts.globalization_alpha_reduction = 0.7; - end - end - if isempty(opts.globalization_eps_sufficient_descent) % if strcmp(opts.globalization, 'FUNNEL_L1PEN_LINESEARCH') if ddp_with_merit_or_funnel diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 9e5081da03..b981d07819 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -210,7 +210,7 @@ obj.fixed_hess = 0; obj.ext_cost_num_hess = 0; obj.globalization_alpha_min = []; - obj.globalization_alpha_reduction = []; + obj.globalization_alpha_reduction = 0.7; obj.globalization_line_search_use_sufficient_descent = 0; obj.globalization_use_SOC = 0; obj.globalization_full_step_dual = []; diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 4afb4a296a..059321f4de 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1047,12 +1047,6 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None else: opts.globalization_alpha_min = 0.05 - if opts.globalization_alpha_reduction is None: - if ddp_with_merit_or_funnel: - opts.globalization_alpha_reduction = 0.5 - else: - opts.globalization_alpha_reduction = 0.7 - if opts.globalization_eps_sufficient_descent is None: if ddp_with_merit_or_funnel: opts.globalization_eps_sufficient_descent = 1e-6 diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 578114bf81..909c7bfb65 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -115,7 +115,7 @@ def __init__(self): self.__ext_cost_num_hess = 0 self.__globalization_use_SOC = 0 self.__globalization_alpha_min = None - self.__globalization_alpha_reduction = None + self.__globalization_alpha_reduction = 0.7 self.__globalization_line_search_use_sufficient_descent = 0 self.__globalization_full_step_dual = None self.__globalization_eps_sufficient_descent = None @@ -893,15 +893,11 @@ def reg_min_epsilon(self): @property def globalization_alpha_reduction(self): - """Step size reduction factor for globalization MERIT_BACKTRACKING, + """Step size reduction factor for globalization MERIT_BACKTRACKING and + FUNNEL_L1PEN_LINESEARCH Type: float - Default: None. - - If None is given: - - in case of FUNNEL_L1PEN_LINESEARCH, value is set to 0.5. - - in case of MERIT_BACKTRACKING, value is set to 0.7. - default: 0.7. + Default: 0.7. """ return self.__globalization_alpha_reduction From d268e226bc6bf575b899e588e8c81f0cf6f935b4 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 16 Jul 2025 10:46:53 +0200 Subject: [PATCH 105/164] Implement QP solver tolerance strategies (#1583) Add option `nlp_qp_tol_strategy`: Strategy for setting the QP tolerances in the NLP solver. ``` String in ["ADAPTIVE_CURRENT_RES_JOINT", "ADAPTIVE_QPSCALING", "FIXED_QP_TOL"] - FIXED_QP_TOL: uses the fixed QP solver tolerances set by the properties `qp_solver_tol_stat`, `qp_solver_tol_eq`, `qp_solver_tol_ineq`, `qp_solver_tol_comp`, this was implemented in acados <= v0.5.0. - ADAPTIVE_CURRENT_RES_JOINT: uses the current NLP residuals to set the QP tolerances in a joint manner. The QP tolerances are set as follows: 1) `tmp_tol_* = MIN(nlp_qp_tol_reduction_factor * inf_norm_res_*, 1e-2)` 2) `joint_tol = MAX(tmp_tol_* for all * in ['stat', 'eq', 'ineq', 'comp'])` 3) `tol_* = MAX(joint_tol, nlp_qp_tol_safety_factor * nlp_solver_tol_*)` - ADAPTIVE_QPSCALING: adapts the QP tolerances based on the QP scaling factors to make NLP residuals converge to desired tolerances, if it can be achieved. The QP tolerances are set as follows: 1) `qp_tol_stat = nlp_qp_tol_safety_factor * nlp_solver_tol_stat * MIN(objective_scaling_factor, min_constraint_scaling);` 2) `qp_tol_eq = nlp_qp_tol_safety_factor * nlp_solver_tol_eq` 3) `qp_tol_ineq = nlp_qp_tol_safety_factor * nlp_solver_tol_ineq * min_constraint_scaling` 4) `qp_tol_comp = nlp_qp_tol_safety_factor * nlp_solver_tol_comp * min_constraint_scaling` 5) cap all QP tolerances to a minimum of `nlp_qp_tol_min_*`. Default: "FIXED_QP_TOL". ``` Added option: `nlp_qp_tol_safety_factor` ``` Safety factor for the QP tolerances. Used to ensure qp_tol* = nlp_qp_tol_safety_factor * nlp_solver_tol_* when approaching the NLP solution. Often QPs should be solved to a higher accuracy than the NLP solver tolerances to ensure convergence of the NLP solver. Used in the ADAPTIVE_CURRENT_RES_JOINT, ADAPTIVE_QPSCALING strategies. Type: float in [0, 1]. Default: 0.1. ``` Closes https://github.com/acados/acados/issues/1553 --- .github/workflows/full_build.yml | 2 +- acados/ocp_nlp/ocp_nlp_common.c | 105 +++++++++- acados/ocp_nlp/ocp_nlp_common.h | 7 + acados/ocp_nlp/ocp_nlp_qpscaling.c | 19 +- acados/ocp_qp/ocp_qp_osqp.c | 2 +- acados/sim/sim_irk_integrator.c | 2 +- acados/utils/types.h | 9 + .../example_ocp_qp_tol.m | 168 +++++++++++++++ .../test/test_all_examples.m | 19 +- .../hs074_constraint_scaling.py | 20 +- .../acados_python/hock_schittkowsky/hs099.py | 196 ++++++++++++++++++ .../inconsistent_qp_linearization_test.py | 5 +- .../linear_mass_model/sqp_wfqp_test.py | 4 +- .../test_qpscaling_slacked.py | 96 +++++---- .../non_ocp_nlp/qpscaling_test.py | 114 ++++++---- .../ocp/minimal_example_ocp_cmake.py | 19 +- .../ocp/time_optimal_swing_up.py | 1 - interfaces/CMakeLists.txt | 5 + interfaces/acados_matlab_octave/AcadosOcp.m | 27 +++ .../acados_matlab_octave/AcadosOcpOptions.m | 14 ++ .../acados_matlab_octave/AcadosOcpSolver.m | 15 ++ .../acados_template/acados_ocp.py | 9 + .../acados_template/acados_ocp_options.py | 114 ++++++++++ .../acados_template/acados_ocp_solver.py | 2 + .../c_templates_tera/acados_multi_solver.in.c | 24 ++- .../c_templates_tera/acados_solver.in.c | 24 ++- 26 files changed, 904 insertions(+), 118 deletions(-) create mode 100644 examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_qp_tol.m create mode 100644 examples/acados_python/hock_schittkowsky/hs099.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index d4c9a2a525..7ade1e3075 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -262,7 +262,7 @@ jobs: working-directory: ${{runner.workspace}}/acados/external shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh + ${{runner.workspace}}/acados/.github/linux/install_new_casadi_matlab.sh - name: Export Paths working-directory: ${{runner.workspace}}/acados diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1017de037c..9b18166463 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -606,7 +606,6 @@ void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_, const char *field, { config->dynamics[i]->dims_set(config->dynamics[i], dims->dynamics[i], "np", &int_array[i]); } - // TODO: implement np for constraints // // constraints // for (int i = 0; i <= N; i++) // { @@ -1219,6 +1218,13 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) opts->log_primal_step_norm = 0; opts->log_dual_step_norm = 0; opts->max_iter = 1; + opts->nlp_qp_tol_strategy = FIXED_QP_TOL; + opts->nlp_qp_tol_reduction_factor = 1e-1; + opts->nlp_qp_tol_safety_factor = 0.1; + opts->nlp_qp_tol_min_stat = 1e-9; + opts->nlp_qp_tol_min_eq = 1e-10; + opts->nlp_qp_tol_min_ineq = 1e-10; + opts->nlp_qp_tol_min_comp = 1e-11; /* submodules opts */ // qp solver @@ -1382,6 +1388,41 @@ 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, "nlp_qp_tol_strategy")) + { + ocp_nlp_qp_tol_strategy_t* nlp_qp_tol_strategy = (ocp_nlp_qp_tol_strategy_t *) value; + opts->nlp_qp_tol_strategy = *nlp_qp_tol_strategy; + } + else if (!strcmp(field, "nlp_qp_tol_reduction_factor")) + { + double* nlp_qp_tol_reduction_factor = (double *) value; + opts->nlp_qp_tol_reduction_factor = *nlp_qp_tol_reduction_factor; + } + else if (!strcmp(field, "nlp_qp_tol_safety_factor")) + { + double* nlp_qp_tol_safety_factor = (double *) value; + opts->nlp_qp_tol_safety_factor = *nlp_qp_tol_safety_factor; + } + else if (!strcmp(field, "nlp_qp_tol_min_stat")) + { + double* nlp_qp_tol_min_stat = (double *) value; + opts->nlp_qp_tol_min_stat = *nlp_qp_tol_min_stat; + } + else if (!strcmp(field, "nlp_qp_tol_min_eq")) + { + double* nlp_qp_tol_min_eq = (double *) value; + opts->nlp_qp_tol_min_eq = *nlp_qp_tol_min_eq; + } + else if (!strcmp(field, "nlp_qp_tol_min_ineq")) + { + double* nlp_qp_tol_min_ineq = (double *) value; + opts->nlp_qp_tol_min_ineq = *nlp_qp_tol_min_ineq; + } + else if (!strcmp(field, "nlp_qp_tol_min_comp")) + { + double* nlp_qp_tol_min_comp = (double *) value; + opts->nlp_qp_tol_min_comp = *nlp_qp_tol_min_comp; + } else if (!strcmp(field, "store_iterates")) { bool* store_iterates = (bool *) value; @@ -1564,30 +1605,30 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value } else if (!strcmp(field, "tol_stat")) { + // NOTE: NLP solver tolerances should be set before QP tolerances! double* tol_stat = (double *) value; opts->tol_stat = *tol_stat; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_stat", value); } else if (!strcmp(field, "tol_eq")) { + // NOTE: NLP solver tolerances should be set before QP tolerances! double* tol_eq = (double *) value; opts->tol_eq = *tol_eq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_eq", value); } else if (!strcmp(field, "tol_ineq")) { + // NOTE: NLP solver tolerances should be set before QP tolerances! double* tol_ineq = (double *) value; opts->tol_ineq = *tol_ineq; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_ineq", value); } else if (!strcmp(field, "tol_comp")) { + // NOTE: NLP solver tolerances should be set before QP tolerances! double* tol_comp = (double *) value; opts->tol_comp = *tol_comp; - // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_comp", value); } else if (!strcmp(field, "tol_min_step_norm")) @@ -4374,6 +4415,60 @@ int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims double tmp_time; int qp_status; + // update QP solver tolerances + if (nlp_opts->nlp_qp_tol_strategy == ADAPTIVE_CURRENT_RES_JOINT) + { + // printf("ocp_nlp_solve_qp_and_correct_dual: nlp_qp_tol_reduction_factor = %e\n", nlp_opts->nlp_qp_tol_reduction_factor); + double reduction_factor = nlp_opts->nlp_qp_tol_reduction_factor; + // double max_log_diff = (nlp_mem->nlp_res->inf_norm_res_stat / nlp_opts->tol_stat); + + double tmp_tol_stat = MIN(reduction_factor * nlp_mem->nlp_res->inf_norm_res_stat, 1e-2); + double tmp_tol_eq = MIN(reduction_factor * nlp_mem->nlp_res->inf_norm_res_eq, 1e-2); + double tmp_tol_ineq = MIN(reduction_factor * nlp_mem->nlp_res->inf_norm_res_ineq, 1e-2); + double tmp_tol_comp = MIN(reduction_factor * nlp_mem->nlp_res->inf_norm_res_comp, 1e-2); + + double joint_tol = MAX(tmp_tol_stat, MAX(tmp_tol_eq, MAX(tmp_tol_ineq, tmp_tol_comp))); + + tmp_tol_stat = MAX(nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_stat, joint_tol); + tmp_tol_eq = MAX(nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_eq, joint_tol); + tmp_tol_ineq = MAX(nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_ineq, joint_tol); + tmp_tol_comp = MAX(nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_comp, joint_tol); + + qp_solver->opts_set(qp_solver, qp_opts, "tol_stat", &tmp_tol_stat); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_stat to %e\n", tmp_tol_stat); + qp_solver->opts_set(qp_solver, qp_opts, "tol_eq", &tmp_tol_eq); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_eq to %e\n", tmp_tol_eq); + qp_solver->opts_set(qp_solver, qp_opts, "tol_ineq", &tmp_tol_ineq); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_ineq to %e\n", tmp_tol_ineq); + qp_solver->opts_set(qp_solver, qp_opts, "tol_comp", &tmp_tol_comp); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_comp to %e\n", tmp_tol_comp); + } + else if (nlp_opts->nlp_qp_tol_strategy == ADAPTIVE_QPSCALING) + { + double min_constraint_scaling, objective_scaling_factor; + ocp_nlp_qpscaling_memory_get(dims->qpscaling, nlp_mem->qpscaling, "min_constraint_scaling", 0, &min_constraint_scaling); + ocp_nlp_qpscaling_memory_get(dims->qpscaling, nlp_mem->qpscaling, "obj", 0, &objective_scaling_factor); + + // + double stat_factor = MIN(objective_scaling_factor, min_constraint_scaling); + double tmp_tol_stat = nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_stat * stat_factor; + double tmp_tol_eq = nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_eq * 1.0; // equalities are not scaled + double tmp_tol_ineq = nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_ineq * min_constraint_scaling; + double tmp_tol_comp = nlp_opts->nlp_qp_tol_safety_factor * nlp_opts->tol_comp * min_constraint_scaling; + tmp_tol_stat = MAX(tmp_tol_stat, nlp_opts->nlp_qp_tol_min_stat); + tmp_tol_eq = MAX(tmp_tol_eq, nlp_opts->nlp_qp_tol_min_eq); + tmp_tol_ineq = MAX(tmp_tol_ineq, nlp_opts->nlp_qp_tol_min_ineq); + tmp_tol_comp = MAX(tmp_tol_comp, nlp_opts->nlp_qp_tol_min_comp); + qp_solver->opts_set(qp_solver, qp_opts, "tol_stat", &tmp_tol_stat); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_stat to %e\n", tmp_tol_stat); + qp_solver->opts_set(qp_solver, qp_opts, "tol_eq", &tmp_tol_eq); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_eq to %e\n", tmp_tol_eq); + qp_solver->opts_set(qp_solver, qp_opts, "tol_ineq", &tmp_tol_ineq); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_ineq to %e\n", tmp_tol_ineq); + qp_solver->opts_set(qp_solver, qp_opts, "tol_comp", &tmp_tol_comp); + // printf("ocp_nlp_solve_qp_and_correct_dual: setting tol_comp to %e\n", tmp_tol_comp); + } + // solve qp acados_tic(&timer); if (precondensed_lhs) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 3337edd575..df28631faf 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -335,6 +335,13 @@ typedef struct ocp_nlp_opts int ext_qp_res; int qp_warm_start; + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy; // strategy for setting the QP tolerances + double nlp_qp_tol_reduction_factor; + double nlp_qp_tol_safety_factor; + double nlp_qp_tol_min_stat; + double nlp_qp_tol_min_eq; + double nlp_qp_tol_min_ineq; + double nlp_qp_tol_min_comp; 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. diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c index 92be82cf09..05cc9e3caa 100644 --- a/acados/ocp_nlp/ocp_nlp_qpscaling.c +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -273,6 +273,22 @@ void ocp_nlp_qpscaling_memory_get(ocp_nlp_qpscaling_dims *dims, void *mem_, cons struct blasfeo_dvec **ptr = value; *ptr = mem->constraints_scaling_vec + stage; } + else if (!strcmp(field, "min_constraint_scaling")) + { + double min = 1.0; + double tmp; + for (int i = 0; i < dims->qp_dim->N; i++) + { + for (int j = 0; j < dims->qp_dim->ng[i]; j++) + { + tmp = BLASFEO_DVECEL(mem->constraints_scaling_vec+i, j); + if (tmp < min) + min = tmp; + } + } + double *ptr = value; + *ptr = min; + } else if (!strcmp(field, "status")) { int *ptr = value; @@ -292,9 +308,10 @@ void ocp_nlp_qpscaling_memory_get(ocp_nlp_qpscaling_dims *dims, void *mem_, cons static double norm_inf_matrix_col(int col_idx, int col_length, struct blasfeo_dmat *At) { double norm = 0.0; + double tmp; for (int j = 0; j < col_length; ++j) { - double tmp = BLASFEO_DMATEL(At, j, col_idx); + tmp = BLASFEO_DMATEL(At, j, col_idx); norm = MAX(norm, fabs(tmp)); } return norm; diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index e16b55712b..54e9ca5ae0 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -1737,7 +1737,7 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m osqp_update_P_A(mem->osqp_work, mem->P_x, NULL, mem->P_nnzmax, mem->A_x, NULL, mem->A_nnzmax); osqp_update_bounds(mem->osqp_work, mem->l, mem->u); - // TODO(oj): update OSQP options here if they were updated? + cpy_osqp_settings(opts->osqp_opts, mem->osqp_work->settings); } else { diff --git a/acados/sim/sim_irk_integrator.c b/acados/sim/sim_irk_integrator.c index 74a19173e8..f3fcce6dba 100644 --- a/acados/sim/sim_irk_integrator.c +++ b/acados/sim/sim_irk_integrator.c @@ -1203,7 +1203,7 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo struct blasfeo_dvec *cost_grad = mem->cost_grad; struct blasfeo_dvec *nls_res = workspace->nls_res; struct blasfeo_dvec *tmp_ny = workspace->tmp_ny; - double cost_scaling; + double cost_scaling = 0.0; struct blasfeo_dmat *cost_hess = mem->cost_hess; struct blasfeo_dmat *J_y_tilde = workspace->J_y_tilde; diff --git a/acados/utils/types.h b/acados/utils/types.h index 802ad93e43..60f933c74c 100644 --- a/acados/utils/types.h +++ b/acados/utils/types.h @@ -96,6 +96,15 @@ typedef enum } ocp_nlp_cost_t; +/// Types of the cost function. +typedef enum +{ + FIXED_QP_TOL, + ADAPTIVE_CURRENT_RES_JOINT, + ADAPTIVE_QPSCALING, +} ocp_nlp_qp_tol_strategy_t; + + /// Types of the timeout heuristic. typedef enum { diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_qp_tol.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_qp_tol.m new file mode 100644 index 0000000000..a72d93b07f --- /dev/null +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_qp_tol.m @@ -0,0 +1,168 @@ +% +% 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.; + +import casadi.* + +check_acados_requirements() + +for itest = 1:2 + + %% solver settings + N = 20; % number of discretization steps + T = 1; % [s] prediction horizon length + x0 = [0; pi; 0; 0]; % initial state + + %% model dynamics + model = get_pendulum_on_cart_model(); + model.name = ['pendulum_on_cart_model_', num2str(itest)]; + nx = length(model.x); % state size + nu = length(model.u); % input size + + %% OCP formulation object + ocp = AcadosOcp(); + ocp.model = model; + + %% cost in nonlinear least squares form + W_x = diag([1e3, 1e3, 1e-2, 1e-2]); + W_u = 1e-2; + + % initial cost term + ny_0 = nu; + ocp.cost.cost_type_0 = 'NONLINEAR_LS'; + ocp.cost.W_0 = W_u; + ocp.cost.yref_0 = zeros(ny_0, 1); + ocp.model.cost_y_expr_0 = model.u; + + % path cost term + ny = nx + nu; + ocp.cost.cost_type = 'NONLINEAR_LS'; + ocp.cost.W = blkdiag(W_x, W_u); + ocp.cost.yref = zeros(ny, 1); + ocp.model.cost_y_expr = vertcat(model.x, model.u); + + % terminal cost term + ny_e = nx; + ocp.cost.cost_type_e = 'NONLINEAR_LS'; + ocp.model.cost_y_expr_e = model.x; + ocp.cost.yref_e = zeros(ny_e, 1); + ocp.cost.W_e = W_x; + + %% define constraints + % only bound on u on initial stage and path + ocp.model.con_h_expr = model.u; + ocp.model.con_h_expr_0 = model.u; + + U_max = 80; + ocp.constraints.lh = -U_max; + ocp.constraints.lh_0 = -U_max; + ocp.constraints.uh = U_max; + ocp.constraints.uh_0 = U_max; + ocp.constraints.x0 = x0; + + % define solver options + ocp.solver_options.N_horizon = N; + ocp.solver_options.tf = T; + ocp.solver_options.nlp_solver_type = 'SQP'; + ocp.solver_options.integrator_type = 'ERK'; + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + ocp.solver_options.qp_solver_mu0 = 1e3; + ocp.solver_options.qp_solver_cond_N = 5; + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; + ocp.solver_options.ext_fun_compile_flags = '-O2'; + ocp.solver_options.globalization = 'MERIT_BACKTRACKING'; + % ocp.solver_options.qp_solver_iter_max = 100 + + if itest == 1 + disp('testing fixed qp tol') + ocp.solver_options.nlp_qp_tol_strategy = 'FIXED_QP_TOL'; + else + disp('testing adaptive current res joint') + qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT'; + ocp.solver_options.nlp_qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT'; + ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-2; + ocp.solver_options.nlp_qp_tol_safety_factor = 0.1; + end + + %% create solver + ocp_solver = AcadosOcpSolver(ocp); + + %% call ocp solver + % update initial state + ocp_solver.set('constr_x0', x0); + + % solve + ocp_solver.solve(); + ocp_solver.print_statistics(); + + qp_iter_all = ocp_solver.get('qp_iter_all'); + sum_qp_iter(itest) = sum(qp_iter_all); + nlp_iter(itest) = ocp_solver.get('nlp_iter'); + + % get solution + utraj = ocp_solver.get('u'); + xtraj = ocp_solver.get('x'); + + status = ocp_solver.get('status'); % 0 - success + + disp(['Solver took ', num2str(nlp_iter(itest)), ' NLP iterations and ', ... + num2str(sum_qp_iter(itest)), ' QP iterations.']); + clear ocp_solver; +end + +%% checks +if sum_qp_iter(1) <= sum_qp_iter(2) + error('number of QP iterations with fixed tol should be larger than with adaptive tol'); +end +if nlp_iter(1) > nlp_iter(2) + error('fixed small QP tolerance should not lead to more nlp iterations than adaptive QP tolerance'); +end + +%% plots +ts = linspace(0, T, N+1); +figure; hold on; +states = {'p', 'theta', 'v', 'dtheta'}; +for i=1:length(states) + subplot(length(states), 1, i); + plot(ts, xtraj(i,:)); grid on; + ylabel(states{i}); + xlabel('t [s]') +end + +figure +stairs(ts, [utraj'; utraj(end)]) +ylabel('F [N]') +xlabel('t [s]') +grid on + + + + +if is_octave() + waitforbuttonpress; +end diff --git a/examples/acados_matlab_octave/test/test_all_examples.m b/examples/acados_matlab_octave/test/test_all_examples.m index 6554806461..1d90694e0e 100644 --- a/examples/acados_matlab_octave/test/test_all_examples.m +++ b/examples/acados_matlab_octave/test/test_all_examples.m @@ -50,6 +50,7 @@ '../pendulum_dae/example_sim.m'; '../pendulum_on_cart_model/example_ocp.m'; '../pendulum_on_cart_model/zoro_example.m'; + '../pendulum_on_cart_model/example_ocp_qp_tol.m'; '../race_cars/main.m'; '../simple_dae_model/example_ocp.m'; '../swarming/example_ocp.m'; @@ -144,18 +145,20 @@ for idx = 1:length(targets) if strcmp(messages{idx},"") disp(targets{idx}) - end -end -disp(' ') -disp('Failed tests: ') -for idx = 1:length(targets) - if ~strcmp(messages{idx},"") - disp(targets{idx}) - disp(['message: ',messages{idx}]) + else fail = true; end end +disp(' ') + if fail==true + disp('Failed tests: ') + for idx = 1:length(targets) + if ~strcmp(messages{idx},"") + disp(targets{idx}) + disp(['message: ',messages{idx}]) + end + end error('Test failure'); end clearvars \ No newline at end of file diff --git a/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py b/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py index ed88f63006..86422ac5de 100644 --- a/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py +++ b/examples/acados_python/hock_schittkowsky/hs074_constraint_scaling.py @@ -33,7 +33,7 @@ import casadi as ca -def solve_problem_with_constraint_scaling(scale_constraints): +def solve_problem_with_constraint_scaling(scale_constraints, nlp_solver_type): # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -79,7 +79,8 @@ def solve_problem_with_constraint_scaling(scale_constraints): ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_max_iter = 1000 ocp.solver_options.qp_solver_iter_max = 1000 - ocp.solver_options.nlp_solver_type = 'SQP_WITH_FEASIBLE_QP' + ocp.solver_options.nlp_solver_type = nlp_solver_type + ocp.solver_options.nlp_solver_ext_qp_res = 1 # Globalization ocp.solver_options.globalization = 'FUNNEL_L1PEN_LINESEARCH' @@ -101,6 +102,8 @@ def solve_problem_with_constraint_scaling(scale_constraints): # solve status = ocp_solver.solve() + ocp_solver.print_statistics() + # checks obj_scale = ocp_solver.get_qp_scaling_objective() print(f"Objective scaling: {obj_scale:.4e}") @@ -116,12 +119,13 @@ def solve_problem_with_constraint_scaling(scale_constraints): def main(): # run test cases - print("\nTest standard unscaled version, HPIPM should fail:") - solve_problem_with_constraint_scaling(scale_constraints=False) - print("\n\n----------------------------------------------") - print("\nTest constraint scaling version, HPIPM should fail:") - solve_problem_with_constraint_scaling(scale_constraints=True) - print("\n\n----------------------------------------------") + for nlp_solver_type in ['SQP_WITH_FEASIBLE_QP', 'SQP']: + print("\nTest standard unscaled version, HPIPM should fail:") + solve_problem_with_constraint_scaling(scale_constraints=False, nlp_solver_type=nlp_solver_type) + print("\n\n----------------------------------------------") + print("\nTest constraint scaling version, HPIPM should succeed:") + solve_problem_with_constraint_scaling(scale_constraints=True, nlp_solver_type=nlp_solver_type) + print("\n\n----------------------------------------------") if __name__ == '__main__': main() diff --git a/examples/acados_python/hock_schittkowsky/hs099.py b/examples/acados_python/hock_schittkowsky/hs099.py new file mode 100644 index 0000000000..201068706d --- /dev/null +++ b/examples/acados_python/hock_schittkowsky/hs099.py @@ -0,0 +1,196 @@ +# +# 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.; +# + + +import casadi as ca +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY +import numpy as np + +def get_hs099_definition(): + # The optimal objective is (if given in): + f_opt = -0.831079892e+9 + x_opt = ca.DM([0.542468, 0.529021, 0.508449, 0.480269, 0.451236, 0.409183, 0.352788]) + x = ca.MX.sym('x', 7) + x0 = np.zeros((7, 1)) + lbx = -ACADOS_INFTY*np.ones((7, 1)) + ubx = ACADOS_INFTY*np.ones((7, 1)) + a = ca.DM.zeros(8, 1) + t = ca.DM.zeros(8, 1) + r_tmp = ca.MX.zeros(8, 1) + q_tmp = ca.MX.zeros(8, 1) + s_tmp = ca.MX.zeros(8, 1) + g = ca.MX.zeros(2, 1) + lbg = -ACADOS_INFTY*np.ones((2, 1)) + ubg = ACADOS_INFTY*np.ones((2, 1)) + + a[0] = 0 + a[1] = 50 + a[2] = 50 + a[3] = 75 + a[4] = 75 + a[5] = 75 + a[6] = 100 + a[7] = 100 + b = 32 + t[0] = 0 + t[1] = 25 + t[2] = 50 + t[3] = 100 + t[4] = 150 + t[5] = 200 + t[6] = 290 + t[7] = 380 + + x0[0:7] = 0.5 + lbx[0:7] = 0 + ubx[0:7] = 1.58 + + r_tmp[0] = 0 + for i in range(1, 8): + r_tmp[i] = a[i]*(t[i]-t[i-1]) * ca.cos(x[i-1]) + r_tmp[i-1] + + s_tmp[0] = 0 + for i in range(1, 8): + s_tmp[i] = (t[i]-t[i-1])*(a[i]*ca.sin(x[i-1]) - b) + s_tmp[i-1] + q_tmp[0] = 0 + for i in range(1, 8): + q_tmp[i] = 0.5*(t[i]-t[i-1])**2*(a[i]*ca.sin(x[i-1]) - b) + (t[i]-t[i-1])*s_tmp[i-1] + q_tmp[i-1] + + lbg[0] = 1.0e+5 + ubg[0] = 1.0e+5 + g[0] = q_tmp[7] + lbg[1] = 1.0e+3 + ubg[1] = 1.0e+3 + g[1] = s_tmp[7] + + f = -r_tmp[7]**2 + + return (x_opt, f_opt, x, f, g, lbg, ubg, lbx, ubx, x0) + + +def create_hs099_ocp(x_opt, f_opt, x, f, g, lbg, ubg, lbx, ubx, x0): + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + ocp.code_export_directory = 'c_generated_code_hs099' + + # set model + model = ocp.model + model.x = x + model.name = 'hs099' + ocp.cost.cost_type_e = 'EXTERNAL' + model.cost_expr_ext_cost_e = f + model.con_h_expr_e = g + + ocp.constraints.lh_e = lbg + ocp.constraints.uh_e = ubg + + ocp.constraints.idxbx_e = np.arange(7) + ocp.constraints.lbx_e = lbx + ocp.constraints.ubx_e = ubx + + ocp.solver_options.N_horizon = 0 + + return ocp + + +def set_ocp_options(ocp: AcadosOcp, use_qp_scaling: bool = True, qp_tol_strategy: str = 'NAIVE'): + opts = ocp.solver_options + opts.hessian_approx = 'EXACT' + opts.nlp_solver_max_iter = 20 + opts.qp_solver_iter_max = 100 + + opts.nlp_solver_ext_qp_res = 1 + opts.nlp_solver_type = 'SQP' + + opts.globalization = 'FIXED_STEP' + + # Scaling + if use_qp_scaling: + opts.qpscaling_scale_objective = 'OBJECTIVE_GERSHGORIN' + opts.qpscaling_scale_constraints = 'INF_NORM' + opts.qpscaling_lb_norm_inf_grad_obj = 1e-4 + opts.qpscaling_ub_max_abs_eig = 1e5 + if qp_tol_strategy == 'NAIVE': + pass + elif qp_tol_strategy == 'SUFFICIENTLY_SMALL': + opts.qp_tol = 1e-9 + elif qp_tol_strategy == 'ADAPTIVE': + opts.nlp_qp_tol_strategy = 'ADAPTIVE_QPSCALING' + return + +def test_solver(use_qp_scaling: bool = True, qp_tol_strategy: str = 'NAIVE'): + + x_opt, f_opt, x, f, g, lbg, ubg, lbx, ubx, x0 = get_hs099_definition() + + ocp = create_hs099_ocp(x_opt, f_opt, x, f, g, lbg, ubg, lbx, ubx, x0) + set_ocp_options(ocp, use_qp_scaling, qp_tol_strategy) + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + # initialize + ocp_solver.set(0, 'x', x0) + + # solve + status = ocp_solver.solve() + + # evaluate + ocp_solver.print_statistics() + + x_sol = ocp_solver.get(0, 'x') + + diff_x = np.linalg.norm(x_sol - x_opt) + if diff_x > 1e-6: + raise ValueError(f"Solution x does not match optimal x: {diff_x}") + + cost = ocp_solver.get_cost() + if not np.isclose(cost, f_opt, atol=1e-6): + raise ValueError(f"Cost does not match optimal cost: {cost} vs {f_opt}") + + if status != 0: + raise ValueError(f"Solver failed with status: {status}") + + +def main(): + try: + test_solver(use_qp_scaling=True, qp_tol_strategy='NAIVE') + except Exception as e: + if 'status' in str(e): + print(f"Test failed with status error as expected: {e}") + else: + raise ValueError(f"Test with QP scaling and NAIVE strategy should fail") + else: + raise ValueError("Test with QP scaling should not fail!") + + test_solver(use_qp_scaling=True, qp_tol_strategy='SUFFICIENTLY_SMALL') + test_solver(use_qp_scaling=True, qp_tol_strategy='ADAPTIVE') + test_solver(use_qp_scaling=False) + +if __name__ == "__main__": + main() diff --git a/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py b/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py index 69b9a139e0..f93246e2c6 100644 --- a/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py +++ b/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py @@ -28,10 +28,9 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, ACADOS_INFTY +from acados_template import AcadosOcp, AcadosOcpOptions, AcadosOcpSolver, AcadosModel, ACADOS_INFTY import numpy as np from casadi import * -from matplotlib import pyplot as plt from itertools import product # Problem with infeasible linearization @@ -118,7 +117,7 @@ def create_solver_opts(N=1, max_iter: int = 20, search_direction_mode='NOMINAL_QP'): - solver_options = AcadosOcp().solver_options + solver_options = AcadosOcpOptions() # set options solver_options.N_horizon = N diff --git a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py index e0ca527d66..78cc79821d 100644 --- a/examples/acados_python/linear_mass_model/sqp_wfqp_test.py +++ b/examples/acados_python/linear_mass_model/sqp_wfqp_test.py @@ -28,7 +28,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY +from acados_template import AcadosOcp, AcadosOcpOptions, AcadosOcpSolver, ACADOS_INFTY import numpy as np import scipy.linalg from linear_mass_model import export_linear_mass_model, plot_linear_mass_system_X_state_space @@ -103,7 +103,7 @@ def feasible_qp_index_test(soften_obstacle, soften_terminal, soften_controls, N, def create_solver_opts(N=4, Tf=2, nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', allow_switching_modes=True): - solver_options = AcadosOcp().solver_options + solver_options = AcadosOcpOptions() # set options solver_options.N_horizon = N diff --git a/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py index c271ee843b..49161e0150 100644 --- a/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py +++ b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py @@ -28,7 +28,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY, AcadosOcpIterate +from acados_template import AcadosOcp, AcadosOcpOptions, AcadosOcpSolver, ACADOS_INFTY, AcadosOcpIterate, AcadosOcpFlattenedIterate import numpy as np import scipy.linalg from linear_mass_model import export_linear_mass_model @@ -36,7 +36,7 @@ def create_solver_opts(N=4, Tf=2, nlp_solver_type = 'SQP_WITH_FEASIBLE_QP', allow_switching_modes=True, globalization= 'FUNNEL_L1PEN_LINESEARCH'): - solver_options = AcadosOcp().solver_options + solver_options = AcadosOcpOptions() # set options solver_options.N_horizon = N @@ -138,10 +138,11 @@ def create_solver(solver_name: str, soften_obstacle: bool, soften_terminal: bool # add obstacle obs_rad = 1.0 + prescaling = 1.0 ocp.constraints.lh = -np.array([ACADOS_INFTY]) - ocp.constraints.uh = -np.array([obs_rad**2]) + ocp.constraints.uh = -prescaling * np.array([obs_rad**2]) x_square = model.x[0] ** 2 + model.x[1] ** 2 - ocp.model.con_h_expr = -x_square + ocp.model.con_h_expr = prescaling * (-x_square) # copy for terminal ocp.constraints.uh_e = ocp.constraints.uh ocp.constraints.lh_e = ocp.constraints.lh @@ -217,21 +218,52 @@ def call_solver(ocp: AcadosOcp, ocp_solver: AcadosOcpSolver, soften_obstacle: bo print(f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations") print(f"solved sqp_wfqp problem with settings soften_obstacle = {soften_obstacle},soften_terminal = {soften_terminal}, SOFTEN_CONTROL = {soften_controls}") -def check_residual_solutions(stat1: np.ndarray, stat2: np.ndarray): - n_rows1 = len(stat1[0]) - n_rows2 = len(stat2[0]) - - assert n_rows1 == n_rows2, f"Both solvers should take the same number of iterations!, got {n_rows1} for solver 1, and {n_rows2} for solver 2" - - for jj in range(n_rows1): - # res_stat - assert np.allclose(stat1[1][jj], stat2[1][jj]), f"res_stat differs in iter {jj}" - # res_eq - assert np.allclose(stat1[2][jj], stat2[2][jj]), f"res_eq differs in iter {jj}" - # res_ineq - assert np.allclose(stat1[3][jj], stat2[3][jj]), f"res_ineq differs in iter {jj}" - # res_comp - assert np.allclose(stat1[4][jj], stat2[4][jj]), f"res_comp differs in iter {jj}" +def check_iteration_residual(stat_list = list[np.ndarray]): + + if len(stat_list) < 2: + raise ValueError("At least two statistics are required for comparison.") + stats_ref = stat_list[0] + + n_iter_ref = stats_ref.shape[1] + for stat in stat_list[1:]: + n_iter = stat.shape[1] + if n_iter != n_iter_ref: + raise ValueError(f"Statistics have different number of rows: {n_iter} vs {n_iter_ref}") + for jj in range(n_iter_ref): + # res_stat + if not np.allclose(stats_ref[1, jj], stat[1, jj]): + raise ValueError(f"res_stat differs in iter {jj}") + # res_eq + if not np.allclose(stats_ref[2, jj], stat[2, jj]): + raise ValueError(f"res_eq differs in iter {jj}, got {stat[2, jj]} vs {stats_ref[2, jj]}") + # res_ineq + if not np.allclose(stats_ref[3, jj], stat[3, jj]): + raise ValueError(f"res_ineq differs in iter {jj}") + # res_comp + if not np.allclose(stats_ref[4, jj], stat[4, jj]): + raise ValueError(f"res_comp differs in iter {jj}") + +def check_solutions(sol_list: list[AcadosOcpFlattenedIterate]): + # check solutions + ref_sol = sol_list[0] + if len(sol_list) < 2: + raise ValueError("At least two solutions are required for comparison.") + # print(f"{ref_sol}") + + for sol in sol_list[1:]: + for field in ["x", "u", "sl", "su", "lam", "pi"]: + v1 = getattr(ref_sol, field) + v2 = getattr(sol, field) + if not np.allclose(v1, v2, atol=1e-6): + print(f"Field {field} differs: max diff = {np.max(np.abs(v1 - v2))}") + print(f"got difference {v1 - v2}") + else: + # print(f"Solutions match in field {field}.") + pass + if ref_sol.allclose(sol): + print("Both solvers have the same solution.") + else: + raise ValueError("Solutions of solvers differ!") def test_qp_scaling(nlp_solver_type = 'SQP', globalization = 'FUNNEL_L1PEN_LINESEARCH'): print(f"\n\nTesting solver={nlp_solver_type} with globalization={globalization}") @@ -244,38 +276,22 @@ def test_qp_scaling(nlp_solver_type = 'SQP', globalization = 'FUNNEL_L1PEN_LINES ocp_1, ocp_solver_1 = create_solver("1", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type=nlp_solver_type, globalization=globalization, allow_switching_modes=False, use_qp_scaling=False) sol_1 = call_solver(ocp_1, ocp_solver_1, soften_obstacle, soften_terminal, soften_controls, plot=False) check_qp_scaling(ocp_solver_1) - sol_1 = ocp_solver_1.store_iterate_to_obj() + sol_1 = ocp_solver_1.store_iterate_to_flat_obj() stats_1 = ocp_solver_1.get_stats("statistics") # test QP scaling ocp_2, ocp_solver_2 = create_solver("2", soften_obstacle, soften_terminal, soften_controls, nlp_solver_type=nlp_solver_type, allow_switching_modes=False, use_qp_scaling=True) sol_2 = call_solver(ocp_2, ocp_solver_2, soften_obstacle, soften_terminal, soften_controls, plot=False) check_qp_scaling(ocp_solver_2) - sol_2 = ocp_solver_2.store_iterate_to_obj() + sol_2 = ocp_solver_2.store_iterate_to_flat_obj() ocp_solver_2.get_from_qp_in(1, "idxs_rev") stats_2 = ocp_solver_2.get_stats("statistics") - check_residual_solutions(stats_1, stats_2) + check_iteration_residual([stats_1, stats_2]) # check solutions - for field in ["x_traj", "u_traj", "sl_traj", "su_traj", "lam_traj", "pi_traj"]: - v1 = getattr(sol_1, field) - v2 = getattr(sol_2, field) - for i in range(len(v1)): - if not np.allclose(v1[i], v2[i], atol=1e-6): - print(f"Field {field} differs at index {i}: max diff = {np.max(np.abs(v1[i] - v2[i]))}") - print(f"got difference {v1[i] - v2[i]}") - else: - pass - # print(f"Field {field} is the same at index {i}.") - - # equivalent check - if sol_1.allclose(sol_2, atol=1e-6): - print("Both solvers have the same solution.") - else: - raise ValueError("Solutions of solvers differ!") - - print("\n\n---------------------------------------------------------") + check_solutions([sol_1, sol_2]) + print("\n-----------------------------------------\n") if __name__ == '__main__': test_qp_scaling(nlp_solver_type = 'SQP', globalization = 'FUNNEL_L1PEN_LINESEARCH') diff --git a/examples/acados_python/non_ocp_nlp/qpscaling_test.py b/examples/acados_python/non_ocp_nlp/qpscaling_test.py index 577bb7f542..49e9e8c431 100644 --- a/examples/acados_python/non_ocp_nlp/qpscaling_test.py +++ b/examples/acados_python/non_ocp_nlp/qpscaling_test.py @@ -36,7 +36,8 @@ def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP', allow_switching_modes: bool = True, use_qp_scaling: bool = False, - soft_h: bool = True): + soft_h: bool = True, + qp_tol_scheme: str = "SUFFICIENTLY_SMALL"): ocp = AcadosOcp() @@ -91,8 +92,15 @@ def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP solver_options.N_horizon = N solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' - qp_tol = 5e-9 - solver_options.qp_tol = qp_tol + if qp_tol_scheme == "SUFFICIENTLY_SMALL": + qp_tol = 5e-9 + solver_options.qp_tol = qp_tol + solver_options.nlp_qp_tol_strategy = "FIXED_QP_TOL" + elif qp_tol_scheme == "ADAPTIVE_QPSCALING": + solver_options.nlp_qp_tol_strategy = "ADAPTIVE_QPSCALING" + elif qp_tol_scheme == "NAIVE": + pass + solver_options.qp_solver_ric_alg = 1 solver_options.qp_solver_mu0 = 1e4 solver_options.qp_solver_iter_max = 400 @@ -103,7 +111,7 @@ def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP solver_options.print_level = 1 solver_options.nlp_solver_max_iter = 6 solver_options.use_constraint_hessian_in_feas_qp = False - solver_options.nlp_solver_ext_qp_res = 0 + solver_options.nlp_solver_ext_qp_res = 1 if not allow_switching_modes: solver_options.search_direction_mode = 'BYRD_OMOJOKUN' @@ -114,7 +122,7 @@ def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP ocp.solver_options.qpscaling_scale_objective = "OBJECTIVE_GERSHGORIN" # create ocp solver - ocp_solver = AcadosOcpSolver(ocp, verbose=False) + ocp_solver = AcadosOcpSolver(ocp) return ocp, ocp_solver @@ -155,49 +163,62 @@ def call_solver(ocp_solver: AcadosOcpSolver) -> AcadosOcpFlattenedIterate: sol = ocp_solver.store_iterate_to_flat_obj() return sol -def check_solutions(sol_1: AcadosOcpFlattenedIterate, sol_2: AcadosOcpFlattenedIterate, soft_h: bool): +def check_solutions(sol_list: list[AcadosOcpFlattenedIterate], soft_h: bool): # check solutions - for field in ["x", "u", "sl", "su", "lam", "pi"]: - v1 = getattr(sol_1, field) - v2 = getattr(sol_2, field) - if not np.allclose(v1, v2, atol=1e-6): - print(f"Field {field} differs: max diff = {np.max(np.abs(v1 - v2))}") - print(f"got difference {v1 - v2}") + ref_sol = sol_list[0] + if len(sol_list) < 2: + raise ValueError("At least two solutions are required for comparison.") + # print(f"{ref_sol}") + + for sol in sol_list[1:]: + for field in ["x", "u", "sl", "su", "lam", "pi"]: + v1 = getattr(ref_sol, field) + v2 = getattr(sol, field) + if not np.allclose(v1, v2, atol=1e-6): + print(f"Field {field} differs: max diff = {np.max(np.abs(v1 - v2))}") + print(f"got difference {v1 - v2}") + else: + # print(f"Solutions match in field {field}.") + pass + if ref_sol.allclose(sol): + print("Both solvers have the same solution.") else: - print(f"Solutions match in field {field}.") - pass - print(f"{sol_1}") + raise ValueError("Solutions of solvers differ!") if soft_h: - if np.any(sol_1.su > 1e-1): + if np.any(ref_sol.su > 1e-1): print("checked with active soft constraints.") else: raise ValueError("Soft constraints should be active, but are not.") - if sol_1.allclose(sol_2): - print("Both solvers have the same solution.") - else: - raise ValueError("Solutions of solvers differ!") - -def check_residual_solutions(stat1: np.ndarray, stat2: np.ndarray): - n_rows1 = len(stat1[0]) - n_rows2 = len(stat2[0]) - assert n_rows1 == n_rows2, f"Both solvers should take the same number of iterations!, got {n_rows1} for solver 1, and {n_rows2} for solver 2" - - for jj in range(n_rows1): - # res_stat - assert np.allclose(stat1[1][jj], stat2[1][jj]), f"res_stat differs in iter {jj}" - # res_eq - assert np.allclose(stat1[2][jj], stat2[2][jj]), f"res_eq differs in iter {jj}" - # res_ineq - assert np.allclose(stat1[3][jj], stat2[3][jj]), f"res_ineq differs in iter {jj}" - # res_comp - assert np.allclose(stat1[4][jj], stat2[4][jj]), f"res_comp differs in iter {jj}" +def check_iteration_residual(stat_list = list[np.ndarray]): + + if len(stat_list) < 2: + raise ValueError("At least two statistics are required for comparison.") + stats_ref = stat_list[0] + + n_iter_ref = stats_ref.shape[1] + for stat in stat_list[1:]: + n_iter = stat.shape[1] + if n_iter != n_iter_ref: + raise ValueError(f"Statistics have different number of rows: {n_iter} vs {n_iter_ref}") + for jj in range(n_iter_ref): + # res_stat + if not np.allclose(stats_ref[1, jj], stat[1, jj]): + raise ValueError(f"res_stat differs in iter {jj}") + # res_eq + if not np.allclose(stats_ref[2, jj], stat[2, jj]): + raise ValueError(f"res_eq differs in iter {jj}, got {stat[2, jj]} vs {stats_ref[2, jj]}") + # res_ineq + if not np.allclose(stats_ref[3, jj], stat[3, jj]): + raise ValueError(f"res_ineq differs in iter {jj}") + # res_comp + if not np.allclose(stats_ref[4, jj], stat[4, jj]): + raise ValueError(f"res_comp differs in iter {jj}") def test_qp_scaling(soft_h: bool = True): nlp_solver_type = "SQP" - # nlp_solver_type = "SQP_WITH_FEASIBLE_QP" # test without QP scaling print("Reference ...") @@ -215,8 +236,21 @@ def test_qp_scaling(soft_h: bool = True): check_qp_scaling(ocp_solver_1) stats1 = ocp_solver_1.get_stats("statistics") - check_residual_solutions(stats1, stats2) - check_solutions(sol_1, sol_2, soft_h) + print("Testing QP scaling with SQP solver...") + _, ocp_solver_3 = create_solver("3", nlp_solver_type=nlp_solver_type, allow_switching_modes=True, use_qp_scaling=True, soft_h=soft_h, qp_tol_scheme="NAIVE") + sol_3 = call_solver(ocp_solver_3) + check_qp_scaling(ocp_solver_3) + stats3 = ocp_solver_3.get_stats("statistics") + + check_solutions([sol_1, sol_2, sol_3], soft_h) + check_iteration_residual([stats1, stats2]) + + try: + check_iteration_residual([stats1, stats3]) + except ValueError as e: + print(f"Got different iterations when using different QP tolerances, as expected.") + else: + raise ValueError("Iteration residuals should differ when using different QP tolerances, but they do not.") def test_sanity_check(soft_h: bool = True, use_qp_scaling: bool = True): print("Sanity Check SQP and SQP_WITH_FEASIBLE_QP solver...") @@ -236,8 +270,8 @@ def test_sanity_check(soft_h: bool = True, use_qp_scaling: bool = True): stats1 = ocp_solver_1.get_stats("statistics") check_qp_scaling(ocp_solver_1) - check_residual_solutions(stats1, stats2) - check_solutions(sol_1, sol_2, soft_h) + check_iteration_residual([stats1, stats2]) + check_solutions([sol_1, sol_2], soft_h) print("\n") diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py index 32d709d1da..c90a2dd74c 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py @@ -86,13 +86,15 @@ ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) # set options -ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # FULL_CONDENSING_QPOASES -# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, -# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP +ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' +# ocp.solver_options.qp_solver = '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.nlp_solver_type = 'SQP' +ocp.solver_options.nlp_solver_ext_qp_res = 1 +ocp.solver_options.nlp_qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT' +ocp.solver_options.qp_solver_iter_max = 1000 +ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-2 # set prediction horizon ocp.solver_options.tf = Tf @@ -107,6 +109,13 @@ status = ocp_solver.solve() +sum_qp_iter = sum(ocp_solver.get_stats("qp_iter")) +nlp_iter = ocp_solver.get_stats("nlp_iter") +print(f'nlp_iter: {nlp_iter}, total qp_iter: {sum_qp_iter}') + +if sum_qp_iter > 66: + raise Exception(f'number of qp iterations {sum_qp_iter} is too high, expected <= 66.') + if status != 0: ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") raise Exception(f'acados returned status {status}.') diff --git a/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py b/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py index 39832615d0..7dbfce9ed6 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py +++ b/examples/acados_python/pendulum_on_cart/ocp/time_optimal_swing_up.py @@ -37,7 +37,6 @@ import numpy as np from utils import plot_pendulum import casadi as ca -from casadi.tools import entry, struct_symSX def formulate_ocp(opts: AcadosOcpOptions) -> AcadosOcp: # create ocp object to formulate the OCP diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 70a295c047..ddaff34852 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -245,6 +245,11 @@ add_test(NAME py_sqp_wfqp_problem_hs074_constraint_scaling COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky python hs074_constraint_scaling.py) +add_test(NAME py_hs099_tol_test + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky + python hs099.py) + + # CMake test add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 32a07a5700..cadf31e4ec 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -829,6 +829,33 @@ function make_consistent(self, is_mocp_phase) if ~ismember(opts.qpscaling_scale_objective, qpscaling_scale_objective_types) error(['Invalid qpscaling_scale_objective: ', opts.qpscaling_scale_objective, '. Available options are: ', strjoin(qpscaling_scale_objective_types, ', ')]); end + + nlp_qp_tol_strategy_types = {'ADAPTIVE_CURRENT_RES_JOINT', 'ADAPTIVE_QPSCALING', 'FIXED_QP_TOL'}; + if ~ismember(opts.nlp_qp_tol_strategy, nlp_qp_tol_strategy_types) + error(['Invalid nlp_qp_tol_strategy: ', opts.nlp_qp_tol_strategy, '. Available options are: ', strjoin(nlp_qp_tol_strategy_types, ', ')]); + end + + % checks on values + if opts.nlp_qp_tol_reduction_factor < 0.0 || opts.nlp_qp_tol_reduction_factor > 1.0 + error(['nlp_qp_tol_reduction_factor must be in [0, 1], got: ', num2str(opts.nlp_qp_tol_reduction_factor)]); + end + + if opts.nlp_qp_tol_safety_factor < 0.0 || opts.nlp_qp_tol_safety_factor > 1.0 + error(['nlp_qp_tol_safety_factor must be in [0, 1], got: ', num2str(opts.nlp_qp_tol_safety_factor)]); + end + + if strcmp(opts.nlp_qp_tol_strategy, "ADAPTIVE_QPSCALING") + if strcmp(opts.qpscaling_scale_constraints, "NO_CONSTRAINT_SCALING") && strcmp(opts.qpscaling_scale_objective, "NO_OBJECTIVE_SCALING") + error('ADAPTIVE_QPSCALING only makes sense if QP scaling is used.'); + end + end + + % RTI checks + if strcmp(opts.nlp_solver_type, 'SQP_RTI') + if ~strcmp(opts.nlp_qp_tol_strategy, 'FIXED_QP_TOL') + error('SQP_RTI only supports FIXED_QP_TOL nlp_qp_tol_strategy.'); + end + end % OCP name self.name = model.name; diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index b981d07819..45c203caba 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -87,6 +87,13 @@ qpscaling_lb_norm_inf_grad_obj qpscaling_scale_objective qpscaling_scale_constraints + nlp_qp_tol_strategy + nlp_qp_tol_reduction_factor + nlp_qp_tol_safety_factor + nlp_qp_tol_min_stat + nlp_qp_tol_min_eq + nlp_qp_tol_min_ineq + nlp_qp_tol_min_comp exact_hess_cost exact_hess_dyn exact_hess_constr @@ -198,6 +205,13 @@ obj.qpscaling_lb_norm_inf_grad_obj = 1e-4; obj.qpscaling_scale_objective = 'NO_OBJECTIVE_SCALING'; obj.qpscaling_scale_constraints = 'NO_CONSTRAINT_SCALING'; + obj.nlp_qp_tol_strategy = 'FIXED_QP_TOL'; + obj.nlp_qp_tol_reduction_factor = 1e-1; + obj.nlp_qp_tol_safety_factor = 0.1; + obj.nlp_qp_tol_min_stat = 1e-9; + obj.nlp_qp_tol_min_eq = 1e-10; + obj.nlp_qp_tol_min_ineq = 1e-10; + obj.nlp_qp_tol_min_comp = 1e-11; obj.reg_epsilon = 1e-4; obj.reg_adaptive_eps = false; obj.reg_max_cond_block = 1e7; diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index fe3af9a56c..1260dce5be 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -213,6 +213,18 @@ function set(obj, field, value, varargin) end function value = get(obj, field, varargin) + if strcmp('qp_iter_all', field) + full_stats = obj.t_ocp.get('stat'); + if strcmp(obj.solver_options.nlp_solver_type, 'SQP') + value = full_stats(:, 7); + return; + elseif strcmp(obj.solver_options.nlp_solver_type, 'SQP_RTI') + value = full_stats(:, 3); + return; + else + error("qp_iter is not available for nlp_solver_type %s.", obj.solver_options.nlp_solver_type); + end + end if strcmp('hess_block', field) @@ -368,6 +380,9 @@ function print(obj, varargin) obj.t_ocp.print(varargin{:}); end + function print_statistics(obj) + obj.t_ocp.print(); + end function reset(obj) obj.t_ocp.reset(); diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 059321f4de..b8b28892db 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1039,6 +1039,10 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None if opts.nlp_solver_type == "SQP_RTI": raise NotImplementedError('qpscaling_scale_constraints and qpscaling_scale_objective not supported for SQP_RTI solver.') + if opts.nlp_qp_tol_strategy == "ADAPTIVE_QPSCALING": + if opts.qpscaling_scale_constraints == "NO_CONSTRAINT_SCALING" and opts.qpscaling_scale_objective == "NO_OBJECTIVE_SCALING": + raise NotImplementedError('ADAPTIVE_QPSCALING only makes sense if QP scaling is used.') + # Set default parameters for globalization ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') if opts.globalization_alpha_min is None: @@ -1073,6 +1077,11 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None if opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' and opts.nlp_solver_type not in ['SQP', 'SQP_WITH_FEASIBLE_QP']: raise NotImplementedError('FUNNEL_L1PEN_LINESEARCH only supports SQP.') + # RTI checks + if opts.nlp_solver_type == "SQP_RTI": + if opts.nlp_qp_tol_strategy != "FIXED_QP_TOL": + raise NotImplementedError('SQP_RTI only supports FIXED_QP_TOL nlp_qp_tol_strategy.') + # termination if opts.nlp_solver_tol_min_step_norm is None: if ddp_with_merit_or_funnel: diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 909c7bfb65..77bfcfeada 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -112,6 +112,15 @@ def __init__(self): self.__qpscaling_lb_norm_inf_grad_obj = 1e-4 self.__qpscaling_scale_objective = "NO_OBJECTIVE_SCALING" self.__qpscaling_scale_constraints = "NO_CONSTRAINT_SCALING" + + self.__nlp_qp_tol_strategy = "FIXED_QP_TOL" + self.__nlp_qp_tol_reduction_factor = 1e-1 + self.__nlp_qp_tol_safety_factor = 0.1 + self.__nlp_qp_tol_min_stat = 1e-9 + self.__nlp_qp_tol_min_eq = 1e-10 + self.__nlp_qp_tol_min_ineq = 1e-10 + self.__nlp_qp_tol_min_comp = 1e-11 + self.__ext_cost_num_hess = 0 self.__globalization_use_SOC = 0 self.__globalization_alpha_min = None @@ -401,6 +410,88 @@ def qpscaling_scale_constraints(self): """ return self.__qpscaling_scale_constraints + @property + def nlp_qp_tol_strategy(self): + """ + Strategy for setting the QP tolerances in the NLP solver. + String in ["ADAPTIVE_CURRENT_RES_JOINT", "ADAPTIVE_QPSCALING", "FIXED_QP_TOL"] + + - FIXED_QP_TOL: uses the fixed QP solver tolerances set by the properties `qp_solver_tol_stat`, `qp_solver_tol_eq`, `qp_solver_tol_ineq`, `qp_solver_tol_comp`, only this was implemented in acados <= v0.5.0. + + - ADAPTIVE_CURRENT_RES_JOINT: uses the current NLP residuals to set the QP tolerances in a joint manner. + The QP tolerances are set as follows: + 1) `tmp_tol_* = MIN(nlp_qp_tol_reduction_factor * inf_norm_res_*, 1e-2)` + 2) `joint_tol = MAX(tmp_tol_* for all * in ['stat', 'eq', 'ineq', 'comp'])` + 3) `tol_* = MAX(joint_tol, nlp_qp_tol_safety_factor * nlp_solver_tol_*)` + + - ADAPTIVE_QPSCALING: adapts the QP tolerances based on the QP scaling factors, to make NLP residuals converge to desired tolerances, if it can be achieved. + The QP tolerances are set as follows: + 1) `qp_tol_stat = nlp_qp_tol_safety_factor * nlp_solver_tol_stat * MIN(objective_scaling_factor, min_constraint_scaling);` + 2) `qp_tol_eq = nlp_qp_tol_safety_factor * nlp_solver_tol_eq` + 3) `qp_tol_ineq = nlp_qp_tol_safety_factor * nlp_solver_tol_ineq * min_constraint_scaling` + 4) `qp_tol_comp = nlp_qp_tol_safety_factor * nlp_solver_tol_comp * min_constraint_scaling` + 5) cap all QP tolerances to a minimum of `nlp_qp_tol_min_*`. + + Default: "FIXED_QP_TOL". + """ + return self.__nlp_qp_tol_strategy + + @property + def nlp_qp_tol_reduction_factor(self): + """ + Factor by which the QP tolerance is smaller compared to the NLP residuals when using the ADAPTIVE_CURRENT_RES_JOINT strategy. + Default: 1e-1. + """ + return self.__nlp_qp_tol_reduction_factor + + @property + def nlp_qp_tol_safety_factor(self): + """ + Safety factor for the QP tolerances. + Used to ensure qp_tol* = nlp_qp_tol_safety_factor * nlp_solver_tol_* when approaching the NLP solution. + Often QPs should be solved to a higher accuracy than the NLP solver tolerances, to ensure convergence of the NLP solver. + Used in the ADAPTIVE_CURRENT_RES_JOINT, ADAPTIVE_QPSCALING strategies. + Type: float in [0, 1]. + Default: 0.1. + """ + return self.__nlp_qp_tol_safety_factor + + @property + def nlp_qp_tol_min_stat(self): + """ + Minimum value to be set in the QP solver stationarity tolerance by `nlp_qp_tol_strategy`, used in `ADAPTIVE_QPSCALING`. + Type: float > 0. + Default: 1e-9. + """ + return self.__nlp_qp_tol_min_stat + + @property + def nlp_qp_tol_min_eq(self): + """ + Minimum value to be set in the QP solver equality tolerance by `nlp_qp_tol_strategy`, used in `ADAPTIVE_QPSCALING`. + Type: float > 0. + Default: 1e-10. + """ + return self.__nlp_qp_tol_min_eq + + @property + def nlp_qp_tol_min_ineq(self): + """ + Minimum value to be set in the QP solver inequality tolerance by `nlp_qp_tol_strategy`, used in `ADAPTIVE_QPSCALING`. + Type: float > 0. + Default: 1e-10. + """ + return self.__nlp_qp_tol_min_ineq + + @property + def nlp_qp_tol_min_comp(self): + """ + Minimum value to be set in the QP solver complementarity tolerance by `nlp_qp_tol_strategy`, used in `ADAPTIVE_QPSCALING`. + Type: float > 0. + Default: 1e-11. + """ + return self.__nlp_qp_tol_min_comp + @property def nlp_solver_step_length(self): """ @@ -494,6 +585,7 @@ def sim_method_jac_reuse(self): def qp_solver_tol_stat(self): """ QP solver stationarity tolerance. + Used if nlp_qp_tol_strategy == "FIXED_QP_TOL". Default: :code:`None` """ return self.__qp_solver_tol_stat @@ -502,6 +594,7 @@ def qp_solver_tol_stat(self): def qp_solver_tol_eq(self): """ QP solver equality tolerance. + Used if nlp_qp_tol_strategy == "FIXED_QP_TOL". Default: :code:`None` """ return self.__qp_solver_tol_eq @@ -510,6 +603,7 @@ def qp_solver_tol_eq(self): def qp_solver_tol_ineq(self): """ QP solver inequality. + Used if nlp_qp_tol_strategy == "FIXED_QP_TOL". Default: :code:`None` """ return self.__qp_solver_tol_ineq @@ -518,6 +612,7 @@ def qp_solver_tol_ineq(self): def qp_solver_tol_comp(self): """ QP solver complementarity. + Used if nlp_qp_tol_strategy == "FIXED_QP_TOL". Default: :code:`None` """ return self.__qp_solver_tol_comp @@ -1755,6 +1850,25 @@ def qpscaling_scale_constraints(self, qpscaling_scale_constraints): raise ValueError(f'Invalid qpscaling_scale_constraints value. Must be in {qpscaling_scale_constraints_types}, got {qpscaling_scale_constraints}.') self.__qpscaling_scale_constraints = qpscaling_scale_constraints + @nlp_qp_tol_strategy.setter + def nlp_qp_tol_strategy(self, nlp_qp_tol_strategy): + nlp_qp_tol_strategy_types = ["ADAPTIVE_CURRENT_RES_JOINT", "ADAPTIVE_QPSCALING", "FIXED_QP_TOL"] + if not nlp_qp_tol_strategy in nlp_qp_tol_strategy_types: + raise ValueError(f'Invalid nlp_qp_tol_strategy value. Must be in {nlp_qp_tol_strategy_types}, got {nlp_qp_tol_strategy}.') + self.__nlp_qp_tol_strategy = nlp_qp_tol_strategy + + @nlp_qp_tol_reduction_factor.setter + def nlp_qp_tol_reduction_factor(self, nlp_qp_tol_reduction_factor): + if not isinstance(nlp_qp_tol_reduction_factor, float) or nlp_qp_tol_reduction_factor < 0.0 or nlp_qp_tol_reduction_factor > 1.0: + raise ValueError(f'Invalid nlp_qp_tol_reduction_factor value. Must be in [0, 1], got {nlp_qp_tol_reduction_factor}.') + self.__nlp_qp_tol_reduction_factor = nlp_qp_tol_reduction_factor + + @nlp_qp_tol_safety_factor.setter + def nlp_qp_tol_safety_factor(self, nlp_qp_tol_safety_factor): + if not isinstance(nlp_qp_tol_safety_factor, float) or nlp_qp_tol_safety_factor < 0.0 or nlp_qp_tol_safety_factor > 1.0: + raise ValueError(f'Invalid nlp_qp_tol_safety_factor value. Must be in [0, 1], got {nlp_qp_tol_safety_factor}.') + self.__nlp_qp_tol_safety_factor = nlp_qp_tol_safety_factor + @nlp_solver_step_length.setter def nlp_solver_step_length(self, nlp_solver_step_length): print("The option nlp_solver_step_length is deprecated and has new name: globalization_fixed_step_length") diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 4d7d2c4153..4343fbee78 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1601,6 +1601,8 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: return full_stats[6, :] elif self.__solver_options['nlp_solver_type'] == 'SQP_RTI': return full_stats[2, :] + else: + raise ValueError(f"qp_iter is not available for nlp_solver_type {self.__solver_options['nlp_solver_type']}.") elif field_ == "qp_res_ineq": if not self.__solver_options['nlp_solver_ext_qp_res']: 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 118b9379ac..6ccc11bac6 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 @@ -2386,6 +2386,28 @@ ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "allow_direction_mode_switch_to_no ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = {{ solver_options.qpscaling_scale_constraints }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + // NLP QP tol strategy + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = {{ solver_options.nlp_qp_tol_strategy }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); + + double nlp_qp_tol_reduction_factor = {{ solver_options.nlp_qp_tol_reduction_factor }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); + + double nlp_qp_tol_safety_factor = {{ solver_options.nlp_qp_tol_safety_factor }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); + + double nlp_qp_tol_min_stat = {{ solver_options.nlp_qp_tol_min_stat }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); + + double nlp_qp_tol_min_eq = {{ solver_options.nlp_qp_tol_min_eq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); + + double nlp_qp_tol_min_ineq = {{ solver_options.nlp_qp_tol_min_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); + + double nlp_qp_tol_min_comp = {{ solver_options.nlp_qp_tol_min_comp }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); + {%- if solver_options.nlp_solver_type == "SQP" and solver_options.timeout_max_time > 0 %} double timeout_max_time = {{ solver_options.timeout_max_time }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "timeout_max_time", &timeout_max_time); @@ -2414,7 +2436,7 @@ ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "allow_direction_mode_switch_to_no int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); -{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #} +{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the same values. #} {%- if solver_options.qp_solver_tol_stat %} double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat); 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 b50f9578fa..62cd866e34 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 @@ -2527,6 +2527,28 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = {{ solver_options.qpscaling_scale_constraints }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + // NLP QP tol strategy + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = {{ solver_options.nlp_qp_tol_strategy }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); + + double nlp_qp_tol_reduction_factor = {{ solver_options.nlp_qp_tol_reduction_factor }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); + + double nlp_qp_tol_safety_factor = {{ solver_options.nlp_qp_tol_safety_factor }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); + + double nlp_qp_tol_min_stat = {{ solver_options.nlp_qp_tol_min_stat }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); + + double nlp_qp_tol_min_eq = {{ solver_options.nlp_qp_tol_min_eq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); + + double nlp_qp_tol_min_ineq = {{ solver_options.nlp_qp_tol_min_ineq }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); + + double nlp_qp_tol_min_comp = {{ solver_options.nlp_qp_tol_min_comp }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); + {%- if solver_options.nlp_solver_type == "SQP" and solver_options.timeout_max_time > 0 %} double timeout_max_time = {{ solver_options.timeout_max_time }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "timeout_max_time", &timeout_max_time); @@ -2555,7 +2577,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); -{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #} +{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the same values. #} {%- if solver_options.qp_solver_tol_stat %} double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat); From 967bb86bf4110665ff08d1341819956bd6abe183 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 16 Jul 2025 14:24:41 +0200 Subject: [PATCH 106/164] MATLAB: Compute max abs hess val, add comments to investigate partial condensing (#1593) --- acados/ocp_qp/ocp_qp_partial_condensing.c | 16 +++++++++++----- .../acados_matlab_octave/AcadosOcpSolver.m | 16 ++++++++++++---- 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index a8b8cc6cbd..d28fc64b38 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -371,6 +371,12 @@ acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims_, void d_part_cond_qp_compute_dim(dims->red_dims, dims->block_size, dims->pcond_dims); + // printf("\n\ndimensions of partially condensed QP:\n"); + // print_ocp_qp_dims(dims->pcond_dims); + + // printf("\n\ndimensions of original QP:\n"); + // print_ocp_qp_dims(dims->orig_dims); + acados_size_t size = 0; size += sizeof(ocp_qp_partial_condensing_memory); @@ -532,13 +538,13 @@ int ocp_qp_partial_condensing(void *qp_in_, void *pcond_qp_in_, void *opts_, voi // start timer acados_tic(&timer); -//d_ocp_qp_dim_print(qp_in->dim); -//d_ocp_qp_dim_print(mem->red_qp->dim); // reduce eq constr DOF d_ocp_qp_reduce_eq_dof(qp_in, mem->red_qp, opts->hpipm_red_opts, mem->hpipm_red_work); -//d_ocp_qp_print(qp_in->dim, qp_in); -//d_ocp_qp_print(mem->red_qp->dim, mem->red_qp); -//exit(1); + + // d_ocp_qp_codegen("orig_ocp_qp.c", "w", qp_in->dim, qp_in); + // d_ocp_qp_codegen("pcond_ocp_qp.c", "w", pcond_qp_in->dim, pcond_qp_in); + // d_ocp_qp_print(qp_in->dim, qp_in); + // d_ocp_qp_print(pcond_qp_in->dim, pcond_qp_in); // convert to partially condensed qp structure d_part_cond_qp_cond(mem->red_qp, pcond_qp_in, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 1260dce5be..9683c4bb8c 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -419,9 +419,10 @@ function reset(obj) result.max_eigv_stage = zeros(num_blocks, 1); result.condition_number_stage = zeros(num_blocks, 1); min_abs_val = inf; - max_abs_val = -inf; + max_abs_eig_val = -inf; max_ev = -inf; min_ev = inf; + max_abs_hess_val = -inf; for n=1:num_blocks if partially_condensed_qp @@ -431,18 +432,25 @@ function reset(obj) end eigvals = eig(hess_block); max_ev = max(max_ev, max(eigvals)); - max_abs_val = max(max_abs_val, max(abs(eigvals))); + max_abs_eig_val = max(max_abs_eig_val, max(abs(eigvals))); min_ev = min(min_ev, min(eigvals)); min_abs_val = min(min_abs_val, min(abs(eigvals))); + max_abs_hess_val = max(max_abs_hess_val, max(max(abs(hess_block)))); result.min_eigv_stage(n) = min(eigvals); result.max_eigv_stage(n) = max(eigvals); result.condition_number_stage(n) = max(eigvals) / min(eigvals); end - result.condition_number_global = max_abs_val / min_abs_val; + % Zl, Zu don't change in partial condensing. + for n=1:obj.N_horizon+1 + max_abs_hess_val = max(max_abs_hess_val, max(abs(obj.get('qp_Zl', n-1)))); + max_abs_hess_val = max(max_abs_hess_val, max(abs(obj.get('qp_Zu', n-1)))); + end + result.condition_number_global = max_abs_eig_val / min_abs_val; result.max_eigv_global = max_ev; - result.max_abs_eigv_global = max_abs_val; + result.max_abs_eigv_global = max_abs_eig_val; result.min_eigv_global = min_ev; result.min_abs_eigv_global = min_abs_val; + result.max_abs_hess_val = max_abs_hess_val; end From e1b9e2f4ae1ccd7865889adbcbf5c61eae117fe1 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 16 Jul 2025 15:09:24 +0200 Subject: [PATCH 107/164] `AcadosCasadiOCP`: Add support for soft nonlinear constraints (#1585) - Add support for soft nonlinear constraint in `AcadosCasadiOCP` - Preliminarily complete `get()` and `set()`for slack variables in `AcadosCasadiOcpSolver` - Add a draft test for testing soft nonlinear constraint - Initially write some helper functions for simplification --------- Co-authored-by: Jonathan Frey --- .../casadi_tests/test_casadi_slack_in_h.py | 158 ++++ interfaces/CMakeLists.txt | 5 + .../acados_casadi_ocp_solver.py | 706 +++++++++++------- 3 files changed, 614 insertions(+), 255 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_slack_in_h.py diff --git a/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py new file mode 100644 index 0000000000..26e03f6e65 --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py @@ -0,0 +1,158 @@ +# +# 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.; +# + +import sys +sys.path.insert(0, '../getting_started') +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model +import numpy as np +import casadi as ca + + +N_horizon = 20 +Tf = 1.0 + +def formulate_ocp(using_soft_constraints=True): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + + ocp.solver_options.N_horizon = N_horizon + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + if using_soft_constraints: + xmax = 2 + vmax = 3 + Fmax = 50 + # soft bound on x, using constraint h + v1 = ocp.model.x[2] + x1 = ocp.model.x[0] + u = ocp.model.u[0] + + # initial soft constraint on h + ocp.model.con_h_expr_0 = ca.vertcat(u,v1) + ocp.constraints.lh_0 = np.array([-Fmax, -vmax]) + ocp.constraints.uh_0 = np.array([+Fmax, +vmax]) + ocp.constraints.idxsh_0 = np.array([1]) # indices of slacked constraints within h + #set initial penalty weight for slack variables + ocp.cost.zl_0 = np.ones((1,)) + ocp.cost.Zl_0 = np.ones((1,)) + ocp.cost.zu_0 = np.ones((1,)) + ocp.cost.Zu_0 = np.ones((1,)) + + # intermidiate soft constraints on h + ocp.model.con_h_expr = ca.vertcat(x1, v1, u) + ocp.constraints.lh = np.array([-xmax, -vmax, -Fmax]) + ocp.constraints.uh = np.array([+xmax, +vmax, +Fmax]) + ocp.constraints.idxsh = np.array([1]) # indices of slacked constraints within h + # set penalty weight for slack variables + ocp.cost.zl = np.ones((1,)) + ocp.cost.Zl = np.ones((1,)) + ocp.cost.zu = np.ones((1,)) + ocp.cost.Zu = np.ones((1,)) + + # terminal soft constraint on h + ocp.model.con_h_expr_e = ca.vertcat(v1, x1) + ocp.constraints.lh_e = np.array([-vmax, -xmax]) + ocp.constraints.uh_e = np.array([+vmax, +xmax]) + ocp.constraints.idxsh_e = np.array([0,1]) # indices of slacked constraints within h + # set penalty weight for terminal slack variable + ocp.cost.zl_e = np.ones((2,)) + ocp.cost.Zl_e = np.ones((2,)) + ocp.cost.zu_e = np.ones((2,)) + ocp.cost.Zu_e = np.ones((2,)) + + else: + # hard constraints on u + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + ocp = formulate_ocp() + N = ocp.solver_options.N_horizon + + # create solver + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + # solve OCP + status = ocp_solver.solve() + if status != 0: + raise Exception(f'acados OCP solver returned status {status}') + result = ocp_solver.store_iterate_to_obj() + print('acados_cost:', ocp_solver.get_cost()) + + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + result_casadi = casadi_ocp_solver.store_iterate_to_obj() + print('casadi_cost:', casadi_ocp_solver.get_cost()) + + result.flatten().allclose(other=result_casadi.flatten()) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index ddaff34852..cfbd0f0049 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -363,6 +363,7 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_ocp_with_onesided_constraints python main_convex_onesided.py) + # casadi_examples add_test(NAME python_casadi_get_set_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_get_set.py) @@ -375,6 +376,9 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME test_casadi_constraint COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_constraint.py) + add_test(NAME test_casadi_slack_in_h + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_slack_in_h.py) # Sim add_test(NAME python_pendulum_ext_sim_example @@ -458,6 +462,7 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS test_casadi_p_in_constraint_and_cost) set_tests_properties(test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS test_casadi_parametric) set_tests_properties(test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) + set_tests_properties(test_casadi_slack_in_h PROPERTIES DEPENDS test_casadi_constraint) # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 67ac4ad8f6..0cb60ff5bc 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -51,44 +51,57 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): ocp.make_consistent() # create index map for variables - index_map = { + self._index_map = { # indices of variables within w 'x_in_w': [], 'u_in_w': [], + # indices of slack variables within w + 'sl_in_w': [], + 'su_in_w': [], # indices of parameters within p_nlp 'p_in_p_nlp': [], 'p_global_in_p_nlp': [], - # indices of state bounds within lam_x(lam_w) in casadi formulation + # indices of state bounds and control bounds within lam_x(lam_w) in casadi formulation 'lam_bx_in_lam_w':[], - # indices of control bounds within lam_x(lam_w) in casadi formulation 'lam_bu_in_lam_w': [], # indices of dynamic constraints within g in casadi formulation 'pi_in_lam_g': [], # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation - 'lam_gnl_in_lam_g': [] + 'lam_gnl_in_lam_g': [], + # indices of slack variables within lam_g in casadi formulation + 'lam_sl_in_lam_g': [], + 'lam_su_in_lam_g': [], } + self.offset_w = 0 # offset for the indices in index_map + self.offset_gnl = 0 + self.offset_lam = 0 # unpack model = ocp.model dims = ocp.dims constraints = ocp.constraints + cost = ocp.cost solver_options = ocp.solver_options N_horizon = solver_options.N_horizon # check what is not supported yet - if any([dims.ns_0, dims.ns, dims.ns_e]): - raise NotImplementedError("AcadosCasadiOcpSolver does not support soft constraints yet.") + if any([dims.nsbx, dims.nsbx_e, dims.nsbu]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for variables (x and u) yet.") + if any([dims.nsg, dims.nsg_e, dims.nsphi, dims.nsphi_e]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for general linear and convex-over-nonlinear constraints (g, phi).") if dims.nz > 0: raise NotImplementedError("AcadosCasadiOcpSolver does not support algebraic variables (z) yet.") if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") - # create primal variables indexed by shooting nodes + # create primal variables and slack variables ca_symbol = model.get_casadi_symbol() - xtraj_node = [ca_symbol(f'x{i}', dims.nx, 1) for i in range(N_horizon+1)] - utraj_node = [ca_symbol(f'u{i}', dims.nu, 1) for i in range(N_horizon)] - if dims.nz > 0: - raise NotImplementedError("CasADi NLP formulation not implemented for models with algebraic variables (z).") + xtraj_node = [] + utraj_node = [] + sl_node = [] + su_node = [] + for i in range(N_horizon+1): + self._append_node(ca_symbol, xtraj_node, utraj_node, sl_node, su_node, i, dims) # parameters ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon+1)] @@ -98,75 +111,54 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - offset = 0 + # setup slack variables + # TODO: speicify different bounds for lsbu, lsbx, lsg, lsh ,lsphi + lb_slack_node = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ + ([0* ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ + ([0 * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) + ub_slack_node = ([np.inf * ca.DM.ones((dims.ns, 1))] if dims.ns_0 else []) + \ + ([np.inf * ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ + ([np.inf * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) for i in range(0, N_horizon+1): - if i == 0: - lb_xtraj_node[i][constraints.idxbx_0] = constraints.lbx_0 - ub_xtraj_node[i][constraints.idxbx_0] = constraints.ubx_0 - index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_0)) - offset += dims.nx - elif i < N_horizon: - lb_xtraj_node[i][constraints.idxbx] = constraints.lbx - ub_xtraj_node[i][constraints.idxbx] = constraints.ubx - index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx)) - offset += dims.nx - elif i == N_horizon: - lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e - ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e - index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_e)) - offset += dims.nx - if i < N_horizon: - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu - index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) - offset += dims.nu + self._set_bounds_indices(i, lb_xtraj_node, ub_xtraj_node, lb_utraj_node, ub_utraj_node, constraints, dims) ### Concatenate primal variables and bounds - # w = [x0, u0, x1, u1, ...] + # w = [x0, u0, sl0, su0, x1, u1, ...] w_sym_list = [] lbw_list = [] ubw_list = [] w0_list = [] p_list = [] - offset = 0 offset_p = 0 x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) - for i in range(N_horizon): - # add x - w_sym_list.append(xtraj_node[i]) - lbw_list.append(lb_xtraj_node[i]) - ubw_list.append(ub_xtraj_node[i]) - w0_list.append(x_guess) - index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) - offset += dims.nx - # add u - w_sym_list.append(utraj_node[i]) - lbw_list.append(lb_utraj_node[i]) - ubw_list.append(ub_utraj_node[i]) - w0_list.append(np.zeros((dims.nu,))) - index_map['u_in_w'].append(list(range(offset, offset + dims.nu))) - offset += dims.nu - # add parameters - p_list.append(ocp.parameter_values) - index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) - offset_p += dims.np - ## terminal stage - # add x - w_sym_list.append(xtraj_node[-1]) - lbw_list.append(lb_xtraj_node[-1]) - ubw_list.append(ub_xtraj_node[-1]) - w0_list.append(x_guess) - index_map['x_in_w'].append(list(range(offset, offset + dims.nx))) - offset += dims.nx - # add parameters - p_list.append(ocp.parameter_values) - index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) - offset_p += dims.np - p_list.append(ocp.p_global_values) - index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) - offset_p += dims.np_global - - nw = offset # number of primal variables + for i in range(N_horizon+1): + if i < N_horizon: + # add x + self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_node, lb_xtraj_node, ub_xtraj_node, i, dims, x_guess) + # add u + self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_node, lb_utraj_node, ub_utraj_node, i, dims, x_guess) + # add slack variables + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_node, su_node], lb_slack_node, ub_slack_node, i, dims, x_guess) + # add parameters + p_list.append(ocp.parameter_values) + self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) + offset_p += dims.np + else: + ## terminal stage + # add x + self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_node, lb_xtraj_node, ub_xtraj_node, i, dims, x_guess) + # add slack variables + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_node, su_node], lb_slack_node, ub_slack_node, i, dims, x_guess) + # add parameters + p_list.append(ocp.parameter_values) + self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) + offset_p += dims.np + # add global parameters + p_list.append(ocp.p_global_values) + self._index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) + offset_p += dims.np_global + + nw = self.offset_w # number of primal variables # vectorize w = ca.vertcat(*w_sym_list) @@ -174,8 +166,14 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): ubw = ca.vertcat(*ubw_list) p_nlp = ca.vertcat(*ptraj_node, model.p_global) - ### Nonlinear constraints - # dynamics + ### Create Constraints + g = [] + lbg = [] + ubg = [] + if with_hessian: + lam_g = [] + hess_l = ca.DM.zeros((nw, nw)) + # dynamics constraints if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) elif solver_options.integrator_type == "ERK": @@ -185,30 +183,6 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): else: raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") - # initial - h_0_fun = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) - - # intermediate - h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) - if dims.nphi > 0: - conl_constr_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) - conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr]) - - # terminal - h_e_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) - if dims.nphi_e > 0: - conl_constr_expr_e = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) - conl_constr_e_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_constr_expr_e]) - - # create nonlinear constraints - g = [] - lbg = [] - ubg = [] - offset = 0 - if with_hessian: - lam_g = [] - hess_l = ca.DM.zeros((nw, nw)) - for i in range(N_horizon+1): # add dynamics constraints if i < N_horizon: @@ -217,12 +191,11 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): elif solver_options.integrator_type == "ERK": para = ca.vertcat(utraj_node[i], ptraj_node[i], model.p_global) dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], para, solver_options.time_steps[i]) - g.append(dyn_equality) - lbg.append(np.zeros((dims.nx, 1))) - ubg.append(np.zeros((dims.nx, 1))) - index_map['pi_in_lam_g'].append(list(range(offset, offset+dims.nx))) - offset += dims.nx - + self._append_constraints(i, 'dyn', g, lbg, ubg, + g_expr = dyn_equality, + lbg_expr = np.zeros((dims.nx, 1)), + ubg_expr = np.zeros((dims.nx, 1)), + cons_dim=dims.nx) if with_hessian: # add hessian of dynamics constraints lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) @@ -231,146 +204,91 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - # nonlinear constraints + # Nonlinear Constraints # initial stage - if i == 0 and N_horizon > 0: - if dims.ng > 0: - C = constraints.C - D = constraints.D - linear_constr_expr = ca.mtimes(C, xtraj_node[0]) + ca.mtimes(D, utraj_node[0]) - g.append(linear_constr_expr) - lbg.append(constraints.lg) - ubg.append(constraints.ug) - - if dims.nh_0 > 0: - # h_0 - h_0_nlp_expr = h_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) - g.append(h_0_nlp_expr) - lbg.append(constraints.lh_0) - ubg.append(constraints.uh_0) - if with_hessian: - lam_h_0 = ca_symbol(f'lam_h_0', dims.nh_0, 1) - lam_g.append(lam_h_0) - # add hessian contribution - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_0_nlp_expr, w, lam_h_0, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - - if dims.nphi_0 > 0: - conl_constr_expr_0 = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) - conl_constr_0_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_constr_expr_0]) - g.append(conl_constr_0_fun(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global)) - lbg.append(constraints.lphi_0) - ubg.append(constraints.uphi_0) - if with_hessian: - lam_phi_0 = ca_symbol(f'lam_phi_0', dims.nphi_0, 1) - lam_g.append(lam_phi_0) - # always use CONL Hessian approximation here, disregarding inner second derivative - outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr_0[i], model.con_r_in_phi_0)[0] for i in range(dims.nphi_0)]) - outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi_0, model.con_r_expr_0) - r_in_nlp = ca.substitute(model.con_r_expr_0, model.x, xtraj_node[-1]) - dr_dw = ca.jacobian(r_in_nlp, w) - hess_l += dr_dw.T @ outer_hess_r @ dr_dw - - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng + dims.nh_0 + dims.nphi_0))) - offset += dims.ng + dims.nh_0 + dims.nphi_0 - - # intermediate stages - elif i < N_horizon: - if dims.ng > 0: - C = constraints.C - D = constraints.D - linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) - g.append(linear_constr_expr) - lbg.append(constraints.lg) - ubg.append(constraints.ug) - - if dims.nh > 0: - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - g.append(h_i_nlp_expr) - lbg.append(constraints.lh) - ubg.append(constraints.uh) - if with_hessian and dims.nh > 0: - # add hessian contribution - lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) - lam_g.append(lam_h) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - - if dims.nphi > 0: - g.append(conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global)) - lbg.append(constraints.lphi) - ubg.append(constraints.uphi) - if with_hessian: - lam_phi = ca_symbol(f'lam_phi', dims.nphi, 1) - lam_g.append(lam_phi) - # always use CONL Hessian approximation here, disregarding inner second derivative - outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) - outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi, model.con_r_expr) - r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_node[-1]) - dr_dw = ca.jacobian(r_in_nlp, w) - hess_l += dr_dw.T @ outer_hess_r @ dr_dw - - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng + dims.nh + dims.nphi))) - offset += dims.ng + dims.nphi + dims.nh - - # terminal stage - else: - if dims.ng_e > 0: - C_e = constraints.C_e - linear_constr_expr_e = ca.mtimes(C_e, xtraj_node[-1]) - g.append(linear_constr_expr_e) - lbg.append(constraints.lg_e) - ubg.append(constraints.ug_e) - - if dims.nh_e > 0: - h_e_nlp_expr = h_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global) - g.append(h_e_nlp_expr) - lbg.append(constraints.lh_e) - ubg.append(constraints.uh_e) - if with_hessian and dims.nh_e > 0: - # add hessian contribution - lam_h_e = ca_symbol(f'lam_h_e', dims.nh_e, 1) - lam_g.append(lam_h_e) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_e_nlp_expr, w, lam_h_e, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - - if dims.nphi_e > 0: - g.append(conl_constr_e_fun(xtraj_node[-1], ptraj_node[-1], model.p_global)) - lbg.append(constraints.lphi_e) - ubg.append(constraints.uphi_e) - if with_hessian: - lam_phi_e = ca_symbol(f'lam_phi_e', dims.nphi_e, 1) - lam_g.append(lam_phi_e) - # always use CONL Hessian approximation here, disregarding inner second derivative - outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr_e[i], model.con_r_in_phi_e)[0] for i in range(dims.nphi_e)]) - outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi_e, model.con_r_expr_e) - r_in_nlp = ca.substitute(model.con_r_expr_e, model.x, xtraj_node[-1]) - dr_dw = ca.jacobian(r_in_nlp, w) - hess_l += dr_dw.T @ outer_hess_r @ dr_dw - - index_map['lam_gnl_in_lam_g'].append(list(range(offset, offset + dims.ng_e + dims.nh_e + dims.nphi_e))) - offset += dims.ng_e + dims.nh_e + dims.nphi_e + lg, ug, lh, uh, lphi, uphi, ng, nh, nphi, nsg, nsh, nsphi, idxsh, linear_constr_expr, h_i_nlp_expr, conl_constr_fun =\ + self._get_constraint_node(i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims) + + # add linear constraints + if ng > 0: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = linear_constr_expr, + lbg_expr = lg, + ubg_expr = ug, + cons_dim=ng) + + # add nonlinear constraints + if nh > 0: + if nsh > 0: + # h_fun with slack variables + soft_h_indices = idxsh + hard_h_indices = np.array([h for h in range(len(lh)) if h not in idxsh]) + for index_in_nh in range(nh): + if index_in_nh in soft_h_indices: + index_in_soft = soft_h_indices.tolist().index(index_in_nh) + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = h_i_nlp_expr[index_in_nh] + sl_node[i][index_in_soft], + lbg_expr = lh[index_in_nh], + ubg_expr = np.inf * ca.DM.ones((1, 1)), + cons_dim=1, + sl=True) + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = h_i_nlp_expr[index_in_nh] - su_node[i][index_in_soft], + lbg_expr = -np.inf * ca.DM.ones((1, 1)), + ubg_expr = uh[index_in_nh], + cons_dim=1, + su=True) + elif index_in_nh in hard_h_indices: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = h_i_nlp_expr[index_in_nh], + lbg_expr = lh[index_in_nh], + ubg_expr = uh[index_in_nh], + cons_dim=1) + else: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = h_i_nlp_expr, + lbg_expr = lh, + ubg_expr = uh, + cons_dim=nh) + if with_hessian: + # add hessian contribution + lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) + lam_g.append(lam_h) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + + # add compound nonlinear constraints + if nphi > 0: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global), + lbg_expr = lphi, + ubg_expr = uphi, + cons_dim=nphi) + if with_hessian: + lam_phi = ca_symbol(f'lam_phi', nphi, 1) + lam_g.append(lam_phi) + # always use CONL Hessian approximation here, disregarding inner second derivative + outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) + outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi, model.con_r_expr) + r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_node[-1]) + dr_dw = ca.jacobian(r_in_nlp, w) + hess_l += dr_dw.T @ outer_hess_r @ dr_dw ### Cost - # initial cost term nlp_cost = 0 - cost_expr_0 = ocp.get_initial_cost_expression() - cost_fun_0 = ca.Function('cost_fun_0', [model.x, model.u, model.p, model.p_global], [cost_expr_0]) - nlp_cost += solver_options.cost_scaling[0] * cost_fun_0(xtraj_node[0], utraj_node[0], ptraj_node[0], model.p_global) - - # intermediate cost term - cost_expr = ocp.get_path_cost_expression() - cost_fun = ca.Function('cost_fun', [model.x, model.u, model.p, model.p_global], [cost_expr]) - for i in range(1, N_horizon): - nlp_cost += solver_options.cost_scaling[i] * cost_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - - # terminal cost term - cost_expr_e = ocp.get_terminal_cost_expression() - cost_fun_e = ca.Function('cost_fun_e', [model.x, model.p, model.p_global], [cost_expr_e]) - nlp_cost += solver_options.cost_scaling[-1] * cost_fun_e(xtraj_node[-1], ptraj_node[-1], model.p_global) + for i in range(N_horizon+1): + xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, ns, zl, Zl, zu, Zu = \ + self._get_cost_node(i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_node, su_node, ocp, dims, cost) + + cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, model.p, model.p_global], [cost_expr_i]) + nlp_cost += solver_options.cost_scaling[i] * cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) + if ns: + penalty_expr_i = 0.5 * ca.mtimes(sl_node_i.T, ca.mtimes(np.diag(Zl), sl_node_i)) + \ + ca.mtimes(zl.reshape(-1, 1).T, sl_node_i) + \ + 0.5 * ca.mtimes(su_node_i.T, ca.mtimes(np.diag(Zu), su_node_i)) + \ + ca.mtimes(zu.reshape(-1, 1).T, su_node_i) + nlp_cost += solver_options.cost_scaling[i] * penalty_expr_i if with_hessian: lam_f = ca_symbol('lam_f', 1, 1) @@ -386,8 +304,6 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): nlp_hess_l_custom = None hess_l = None - # sanity check - # create NLP nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} @@ -398,10 +314,238 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): self.__bounds = bounds self.__w0 = w0 self.__p = p - self.__index_map = index_map + self.__index_map = self._index_map self.__nlp_hess_l_custom = nlp_hess_l_custom self.__hess_approx_expr = hess_l + def _append_node(self, ca_symbol, xtraj_node:list, utraj_node:list, sl_node:list, su_node:list, i, dims): + """ + Helper function to append a node to the NLP formulation. + """ + if i == 0: + ns = dims.ns_0 + elif i < dims.N: + ns = dims.ns + else: + ns = dims.ns_e + xtraj_node.append(ca_symbol(f'x{i}', dims.nx, 1)) + utraj_node.append(ca_symbol(f'u{i}', dims.nu, 1)) + if ns > 0: + sl_node.append(ca_symbol(f'sl_0', ns, 1)) + su_node.append(ca_symbol(f'su_0', ns, 1)) + else: + sl_node.append([]) + su_node.append([]) + + def _set_bounds_indices(self, i, lb_xtraj_node, ub_xtraj_node, lb_utraj_node, ub_utraj_node, constraints, dims): + """ + Helper function to set bounds and indices for the primal variables. + """ + if i == 0: + lb_xtraj_node[i][constraints.idxbx_0] = constraints.lbx_0 + ub_xtraj_node[i][constraints.idxbx_0] = constraints.ubx_0 + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx_0)) + self.offset_lam += dims.nx + elif i < dims.N: + lb_xtraj_node[i][constraints.idxbx] = constraints.lbx + ub_xtraj_node[i][constraints.idxbx] = constraints.ubx + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx)) + self.offset_lam += dims.nx + elif i == dims.N: + lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e + ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx_e)) + self.offset_lam += dims.nx + if i < dims.N: + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu + self._index_map['lam_bu_in_lam_w'].append(list(self.offset_lam + constraints.idxbu)) + self.offset_lam += dims.nu + self.offset_lam += 2*dims.ns_0 if i == 0 else 2*dims.ns + + def _append_variables_and_bounds(self, _field, w_sym_list, lbw_list, ubw_list, w0_list, + node_list, lb_node_list, ub_node_list, i, dims, x_guess): + """ + Unified helper function to add a primal or slack variable to the NLP formulation. + """ + if _field == "x": + # Add state variable + w_sym_list.append(node_list[i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(x_guess) + self._index_map['x_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nx))) + self.offset_w += dims.nx + + elif _field == "u": + # Add control variable + w_sym_list.append(node_list[i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((dims.nu,))) + self._index_map['u_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nu))) + self.offset_w += dims.nu + + elif _field == "slack": + # Add slack variables (sl and su) + if i == 0 and dims.ns_0: + ns = dims.ns_0 + elif i < dims.N and dims.ns: + ns = dims.ns + elif i == dims.N and dims.ns_e: + ns = dims.ns_e + else: + self._index_map['sl_in_w'].append([]) + self._index_map['su_in_w'].append([]) + return + + # Add sl + w_sym_list.append(node_list[0][i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((ns,))) + # Add su + w_sym_list.append(node_list[1][i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((ns,))) + + self._index_map['sl_in_w'].append(list(range(self.offset_w, self.offset_w + ns))) + self._index_map['su_in_w'].append(list(range(self.offset_w + ns, self.offset_w + 2 * ns))) + self.offset_w += 2 * ns + + else: + raise ValueError(f"Unsupported for: {_field}") + + def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr, cons_dim, sl=False, su=False): + """ + Helper function to append constraints to the NLP formulation. + """ + g.append(g_expr) + lbg.append(lbg_expr) + ubg.append(ubg_expr) + if _field == 'dyn': + self._index_map['pi_in_lam_g'].append(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) + self.offset_gnl += cons_dim + elif _field == 'gnl': + if not sl and not su: + self._index_map['lam_gnl_in_lam_g'][i].extend(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) + self.offset_gnl += cons_dim + elif sl: + self._index_map['lam_sl_in_lam_g'][i].append(self.offset_gnl) + self.offset_gnl += 1 + elif su: + self._index_map['lam_su_in_lam_g'][i].append(self.offset_gnl) + self.offset_gnl += 1 + + def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_node, su_node, ocp, dims, cost): + """ + Helper function to get the cost node for a given stage. + """ + if i == 0: + return (xtraj_node[0], + utraj_node[0], + ptraj_node[0], + sl_node[0], + su_node[0], + ocp.get_initial_cost_expression(), + dims.ns_0, cost.zl_0, cost.Zl_0, cost.zu_0, cost.Zu_0) + elif i < N_horizon: + return (xtraj_node[i], + utraj_node[i], + ptraj_node[i], + sl_node[i], + su_node[i], + ocp.get_path_cost_expression(), + dims.ns, cost.zl, cost.Zl, cost.zu, cost.Zu) + else: + return (xtraj_node[-1], + [], + ptraj_node[-1], + sl_node[-1], + su_node[-1], + ocp.get_terminal_cost_expression(), + dims.ns_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) + + def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims): + """ + Helper function to get the constraint node for a given stage. + """ + if i == 0 and N_horizon > 0: + lg, ug = constraints.lg, constraints.ug + lh, uh = constraints.lh_0, constraints.uh_0 + lphi, uphi = constraints.lphi_0, constraints.uphi_0 + ng, nh, nphi = dims.ng, dims.nh_0, dims.nphi_0 + nsg, nsh, nsphi, idxsh = dims.nsg, dims.nsh_0, dims.nsphi_0, constraints.idxsh_0 + + # linear function + linear_constr_expr = None + if dims.ng > 0: + C = constraints.C + D = constraints.D + linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) + # nonlinear function + h_fun = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + # compound nonlinear constraint + conl_constr_fun = None + if dims.nphi_0 > 0: + conl_expr = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) + conl_constr_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) + + elif i < N_horizon: + lg, ug = constraints.lg, constraints.ug + lh, uh = constraints.lh, constraints.uh + lphi, uphi = constraints.lphi, constraints.uphi + ng, nh, nphi = dims.ng, dims.nh, dims.nphi + nsg, nsh, nsphi, idxsh = dims.nsg, dims.nsh, dims.nsphi, constraints.idxsh + + linear_constr_expr = None + if dims.ng > 0: + C = constraints.C + D = constraints.D + linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) + h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + conl_constr_fun = None + if dims.nphi > 0: + conl_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) + conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) + + else: + lg, ug = constraints.lg_e, constraints.ug_e + lh, uh = constraints.lh_e, constraints.uh_e + lphi, uphi = constraints.lphi_e, constraints.uphi_e + ng, nh, nphi = dims.ng_e, dims.nh_e, dims.nphi_e + nsg, nsh, nsphi, idxsh = dims.nsg_e, dims.nsh_e, dims.nsphi_e, constraints.idxsh_e + + linear_constr_expr = None + if dims.ng_e > 0: + C = constraints.C_e + linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + h_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) + h_i_nlp_expr = h_fun(xtraj_node[i], ptraj_node[i], model.p_global) + conl_constr_fun = None + if dims.nphi_e > 0: + conl_expr = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) + conl_constr_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_expr]) + + self._index_map['lam_gnl_in_lam_g'].append([]) + self._index_map['lam_sl_in_lam_g'].append([]) + self._index_map['lam_su_in_lam_g'].append([]) + + return ( + lg, ug, + lh, uh, + lphi, uphi, + ng, nh, nphi, + nsg, nsh, nsphi, + idxsh, + linear_constr_expr, + h_i_nlp_expr, + conl_constr_fun + ) + @property def nlp(self): """ @@ -541,7 +685,12 @@ def get(self, stage: int, field: str): Get the last solution of the solver. :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'pi', 'p', 'lam'] + :param field: string in ['x', 'u', 'pi', 'p', 'lam', 'sl', 'su'] + + .. note:: regarding lam: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] """ if not isinstance(stage, int): @@ -557,6 +706,10 @@ def get(self, stage: int, field: str): return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() elif field == 'p': return self.p[self.index_map['p_in_p_nlp'][stage]].flatten() + elif field == 'sl': + return self.nlp_sol_w[self.index_map['sl_in_w'][stage]].flatten() + elif field == 'su': + return self.nlp_sol_w[self.index_map['su_in_w'][stage]].flatten() elif field == 'lam': if stage == 0: bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] @@ -572,14 +725,33 @@ def get(self, stage: int, field: str): g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] lbx_lam = np.maximum(0, -bx_lam) - lbu_lam = np.maximum(0, -bu_lam) - lg_lam = np.maximum(0, -g_lam) ubx_lam = np.maximum(0, bx_lam) + lbu_lam = np.maximum(0, -bu_lam) ubu_lam = np.maximum(0, bu_lam) - ug_lam = np.maximum(0, g_lam) - lam = np.concatenate((lbu_lam, lbx_lam, lg_lam, ubu_lam, ubx_lam, ug_lam)) + if any([dims.ns_0, dims.ns, dims.ns_e]): + lw_soft_lam = self.nlp_sol_lam_x[self.index_map['sl_in_w'][stage]] + uw_soft_lam = self.nlp_sol_lam_x[self.index_map['su_in_w'][stage]] + lg_soft_lam = self.nlp_sol_lam_g[self.index_map['lam_sl_in_lam_g'][stage]] + ug_soft_lam = self.nlp_sol_lam_g[self.index_map['lam_su_in_lam_g'][stage]] + if self.index_map['lam_su_in_lam_g'][stage]: + g_indices = np.array(self.index_map['lam_gnl_in_lam_g'][stage]+\ + self.index_map['lam_sl_in_lam_g'][stage]) + sorted_indices = np.argsort(g_indices) + g_lam_lower = np.concatenate((np.maximum(0, -g_lam), -lg_soft_lam)) + lbg_lam = g_lam_lower[sorted_indices] + g_lam_upper = np.concatenate((np.maximum(0, g_lam), ug_soft_lam)) + ubg_lam = g_lam_upper[sorted_indices] + else: + lbg_lam = np.abs(lg_soft_lam) + ubg_lam = np.abs(ug_soft_lam) + lam_soft = np.concatenate((-lw_soft_lam, -uw_soft_lam)) + else: + lbg_lam = np.maximum(0, -g_lam) + ubg_lam = np.maximum(0, g_lam) + lam_soft = np.empty((0, 1)) + lam = np.concatenate((lbu_lam, lbx_lam, lbg_lam, ubu_lam, ubx_lam, ubg_lam, lam_soft)) return lam.flatten() - elif field in ['sl', 'su', 'z']: + elif field in ['z']: return np.empty((0, 1)) # Only empty is supported for now. TODO: extend. else: raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") @@ -671,12 +843,11 @@ def load_iterate_from_obj(self, iterate: AcadosOcpIterate) -> None: Loads the provided iterate into the OCP solver. Note: The iterate object does not contain the the parameters. """ - # TODO: add slacks for key, traj in iterate.__dict__.items(): field = key.replace('_traj', '') for n, val in enumerate(traj): - if field in ['x', 'u', 'pi', 'lam']: + if field in ['x', 'u', 'pi', 'lam', 'sl', 'su']: self.set(n, field, val) def store_iterate_to_flat_obj(self) -> AcadosOcpFlattenedIterate: @@ -700,6 +871,8 @@ def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedIterate) -> None self.set_flat("u", iterate.u) self.set_flat("pi", iterate.pi) self.set_flat("lam", iterate.lam) + self.set_flat("sl", iterate.sl) + self.set_flat("su", iterate.su) def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: @@ -716,7 +889,7 @@ def set(self, stage: int, field: str, value_: np.ndarray): Set solver initialization to stages. :param stage: integer corresponding to shooting node - :param field: string in ['x', 'u', 'pi', 'lam'] + :param field: string in ['x', 'u', 'pi', 'lam', 'p', 'sl', 'su'] :value_: """ dims = self.ocp.dims @@ -729,19 +902,26 @@ def set(self, stage: int, field: str, value_: np.ndarray): self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = -value_.flatten() elif field == 'p': self.p[self.index_map['p_in_p_nlp'][stage]] = value_.flatten() + elif field == 'sl': + self.w0[self.index_map['sl_in_w'][stage]] = value_.flatten() + elif field == 'su': + self.w0[self.index_map['su_in_w'][stage]] = value_.flatten() elif field == 'lam': if stage == 0: nbx = dims.nbx_0 nbu = dims.nbu n_ghphi = dims.ng + dims.nh_0 + dims.nphi_0 + ns = dims.ns_0 elif stage < dims.N: nbx = dims.nbx nbu = dims.nbu n_ghphi = dims.ng + dims.nh + dims.nphi + ns = dims.ns elif stage == dims.N: nbx = dims.nbx_e nbu = 0 n_ghphi = dims.ng_e + dims.nh_e + dims.nphi_e + ns = dims.ns_e offset_u = (nbx+nbu+n_ghphi) lbu_lam = value_[:nbu] @@ -750,15 +930,31 @@ def set(self, stage: int, field: str, value_: np.ndarray): ubu_lam = value_[offset_u:offset_u+nbu] ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] ug_lam = value_[offset_u+nbu+nbx:offset_u+nbu+nbx+n_ghphi] + offset_soft = 2*offset_u + soft_lam = value_[offset_soft:offset_soft + 2 * ns] + + g_indices = np.array(self.index_map['lam_gnl_in_lam_g'][stage]+\ + self.index_map['lam_sl_in_lam_g'][stage]) + sorted = np.sort(g_indices) + gnl_indices = [i for i, x in enumerate(sorted) if x in self.index_map['lam_gnl_in_lam_g'][stage]] + sl_indices = [i for i, x in enumerate(sorted) if x in self.index_map['lam_sl_in_lam_g'][stage]] + lg_lam_hard = lg_lam[gnl_indices] + lg_lam_soft = lg_lam[sl_indices] + ug_lam_hard = ug_lam[gnl_indices] + ug_lam_soft = ug_lam[sl_indices] + if stage != dims.N: self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) - self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam + self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam_hard-lg_lam_hard + self.lam_g0[self.index_map['lam_sl_in_lam_g'][stage]] = -lg_lam_soft + self.lam_g0[self.index_map['lam_su_in_lam_g'][stage]] = ug_lam_soft + self.lam_x0[self.index_map['sl_in_w'][stage]+self.index_map['su_in_w'][stage]] = -soft_lam else: self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]] = ubx_lam-lbx_lam - self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam - elif field in ['sl', 'su']: - # do nothing for now, only empty is supported - pass + self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam_hard-lg_lam_hard + self.lam_g0[self.index_map['lam_sl_in_lam_g'][stage]] = -lg_lam_soft + self.lam_g0[self.index_map['lam_su_in_lam_g'][stage]] = ug_lam_soft + self.lam_x0[self.index_map['sl_in_w'][stage]+self.index_map['su_in_w'][stage]] = -soft_lam else: raise NotImplementedError(f"Field '{field}' is not yet implemented in set().") From d93ef4fb6d8839beb37dc4f735cb84f2b3d0361e Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Wed, 16 Jul 2025 15:09:45 +0200 Subject: [PATCH 108/164] `Python`: Fix compatibility `translate_to_feasibility_problem` and `AcadosCasadiOcpSolver` (#1594) In the formulation of the feasibility problem some expressions needed to be formulated as `[]` instead of `None` such that it is compatible with the `AcadosCasadiOcpSolver`. --- .../acados_template/acados_ocp.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index b8b28892db..beccd442f5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1898,10 +1898,10 @@ def translate_to_feasibility_problem(self, for i in range(casadi_length(constr_expr)): self.formulate_constraint_as_L2_penalty(constr_expr[i], weight=1.0, upper_bound=upper_bound[i], lower_bound=lower_bound[i]) - model.con_h_expr = None - model.con_phi_expr = None - model.con_r_expr = None - model.con_r_in_phi = None + model.con_h_expr = [] + model.con_phi_expr = [] + model.con_r_expr = [] + model.con_r_in_phi = [] # formulate **terminal** constraints as L2 penalties expr_bound_list_e = [ @@ -1918,10 +1918,10 @@ def translate_to_feasibility_problem(self, for i in range(casadi_length(constr_expr)): self.formulate_constraint_as_L2_penalty(constr_expr[i], weight=1.0, upper_bound=upper_bound[i], lower_bound=lower_bound[i], constraint_type="terminal") - model.con_h_expr_e = None - model.con_phi_expr_e = None - model.con_r_expr_e = None - model.con_r_in_phi_e = None + model.con_h_expr_e = [] + model.con_phi_expr_e = [] + model.con_r_expr_e = [] + model.con_r_in_phi_e = [] # Convert initial conditions to l2 penalty # Expressions for control constraints on u @@ -1956,10 +1956,10 @@ def translate_to_feasibility_problem(self, for i in range(casadi_length(constr_expr)): self.formulate_constraint_as_L2_penalty(constr_expr[i], weight=1.0, upper_bound=upper_bound[i], lower_bound=lower_bound[i], constraint_type="initial") - model.con_h_expr_0 = None - model.con_phi_expr_0 = None - model.con_r_expr_0 = None - model.con_r_in_phi_0 = None + model.con_h_expr_0 = [] + model.con_phi_expr_0 = [] + model.con_r_expr_0 = [] + model.con_r_in_phi_0 = [] # delete constraint fromulation from constraints object self.constraints = new_constraints From e880303c1246d27eb4df0f079bbd269af920ad1d Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Wed, 16 Jul 2025 15:58:04 +0200 Subject: [PATCH 109/164] `AcadosCasadiOcpSolver`: added option for fatrop structure exploitation (#1595) The option for the structure exploitation of fatrop is now correctly set. Fatrop is much faster. --- .../acados_casadi_ocp_solver.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 0cb60ff5bc..fee0935b74 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -111,7 +111,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - # setup slack variables + # setup slack variables # TODO: speicify different bounds for lsbu, lsbx, lsg, lsh ,lsphi lb_slack_node = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ ([0* ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ @@ -327,7 +327,7 @@ def _append_node(self, ca_symbol, xtraj_node:list, utraj_node:list, sl_node:list elif i < dims.N: ns = dims.ns else: - ns = dims.ns_e + ns = dims.ns_e xtraj_node.append(ca_symbol(f'x{i}', dims.nx, 1)) utraj_node.append(ca_symbol(f'u{i}', dims.nu, 1)) if ns > 0: @@ -439,7 +439,7 @@ def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr self.offset_gnl += 1 def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_node, su_node, ocp, dims, cost): - """ + """ Helper function to get the cost node for a given stage. """ if i == 0: @@ -459,10 +459,10 @@ def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_no ocp.get_path_cost_expression(), dims.ns, cost.zl, cost.Zl, cost.zu, cost.Zu) else: - return (xtraj_node[-1], - [], - ptraj_node[-1], - sl_node[-1], + return (xtraj_node[-1], + [], + ptraj_node[-1], + sl_node[-1], su_node[-1], ocp.get_terminal_cost_expression(), dims.ns_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) @@ -529,7 +529,7 @@ def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, if dims.nphi_e > 0: conl_expr = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) conl_constr_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_expr]) - + self._index_map['lam_gnl_in_lam_g'].append([]) self._index_map['lam_sl_in_lam_g'].append([]) self._index_map['lam_su_in_lam_g'].append([]) @@ -631,6 +631,7 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, if solver == "fatrop": pi_in_lam_g_flat = [idx for sublist in self.index_map['pi_in_lam_g'] for idx in sublist] is_equality_array = [True if i in pi_in_lam_g_flat else False for i in range(casadi_length(self.casadi_nlp['g']))] + casadi_solver_opts['structure_detection'] = 'auto' casadi_solver_opts['equality'] = is_equality_array if use_acados_hessian: @@ -934,7 +935,7 @@ def set(self, stage: int, field: str, value_: np.ndarray): soft_lam = value_[offset_soft:offset_soft + 2 * ns] g_indices = np.array(self.index_map['lam_gnl_in_lam_g'][stage]+\ - self.index_map['lam_sl_in_lam_g'][stage]) + self.index_map['lam_sl_in_lam_g'][stage]) sorted = np.sort(g_indices) gnl_indices = [i for i, x in enumerate(sorted) if x in self.index_map['lam_gnl_in_lam_g'][stage]] sl_indices = [i for i, x in enumerate(sorted) if x in self.index_map['lam_sl_in_lam_g'][stage]] @@ -942,7 +943,7 @@ def set(self, stage: int, field: str, value_: np.ndarray): lg_lam_soft = lg_lam[sl_indices] ug_lam_hard = ug_lam[gnl_indices] ug_lam_soft = ug_lam[sl_indices] - + if stage != dims.N: self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam_hard-lg_lam_hard From 2990cc924db7cf33391cda085760c6a9035aa443 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Thu, 17 Jul 2025 11:13:02 +0200 Subject: [PATCH 110/164] Add warm start test in `test_casadi_slack_in_h` (#1596) - Add a test using `sqpmethod` as the solver in CasADi for warm-start evaluation. --- .../acados_python/casadi_tests/test_casadi_slack_in_h.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py index 26e03f6e65..49f708e0d2 100644 --- a/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py +++ b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py @@ -154,5 +154,12 @@ def main(): result.flatten().allclose(other=result_casadi.flatten()) + casadi_sqp_ocp_solver = AcadosCasadiOcpSolver(ocp, solver="sqpmethod", verbose=False) + casadi_sqp_ocp_solver.load_iterate_from_obj(result) + casadi_sqp_ocp_solver.solve() + iteration = casadi_sqp_ocp_solver.get_stats('nlp_iter') + if iteration > 1: + raise Exception(f'casadi SQP solver returned {iteration} iterations, expected less than 1.') + if __name__ == '__main__': main() \ No newline at end of file From 48e223e85f0408ebfd1d8c6d6fb0589e9c41b3aa Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 18 Jul 2025 12:24:16 +0200 Subject: [PATCH 111/164] Get timing from casadi, store stats (#1598) --- .../acados_template/acados_casadi_ocp_solver.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index fee0935b74..52a462dbd7 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -668,6 +668,8 @@ def solve(self) -> int: # timing = solver_stats['t_proc_total'] self.status = solver_stats['return_status'] self.nlp_iter = solver_stats['iter_count'] + self.time_total = solver_stats['t_wall_total'] + self.solver_stats = solver_stats # nlp_res = ca.norm_inf(sol['g']).full()[0][0] # cost_val = ca.norm_inf(sol['f']).full()[0][0] return self.status @@ -879,6 +881,8 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: if field_ == "nlp_iter": return self.nlp_iter + elif field_ == "time_tot": + return self.time_total else: raise NotImplementedError() From 448bd0b0cad7822505d778d5e01213f671e30a79 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Tue, 22 Jul 2025 11:28:14 +0200 Subject: [PATCH 112/164] Simulink: automatically create the S-function block with port info (#1599) This PR adds the option to automatically generate the Simulink block with port information when `make_sfun.m` is called. Also, I added a missing Simulink input to the docs, please correct me if I'm wrong. --- docs/matlab_octave_interface/index.md | 5 +-- .../matlab_templates/make_sfun.in.m | 32 +++++++++++++++++++ .../matlab_templates/make_sfun_sim.in.m | 30 ++++++++++++++++- .../simulink_default_opts.json | 4 ++- 4 files changed, 67 insertions(+), 4 deletions(-) diff --git a/docs/matlab_octave_interface/index.md b/docs/matlab_octave_interface/index.md index f244dae5c2..7df9682613 100644 --- a/docs/matlab_octave_interface/index.md +++ b/docs/matlab_octave_interface/index.md @@ -92,13 +92,14 @@ This is a list of possible inputs to the Simulink block of an OCP solver which c | `cost_zu` | Cost `zu` for all nodes 0 to N | `sum(ns_i) for i = 0,..., N` | Yes | | `cost_Zl` | Cost `Zl` for all nodes 0 to N | `sum(ns_i) for i = 0,..., N` | Yes | | `cost_Zu` | Cost `Zu` for all nodes 0 to N | `sum(ns_i) for i = 0,..., N` | Yes | -| `reset_solver` | Determines if the solver's iterate is set to all zeros before other initializations | `1` | Yes | -| `ignore_inits` | Determines if initialization (`x_init`, `u_init`, `pi_init`, `slacks_init`) is set (0) or ignored (1) | `1` | Yes | +| `reset_solver` | Determines if the solver's iterate is set to all zeros before other initializations | `1` | Yes | +| `ignore_inits` | Determines if initialization (`x_init`, `u_init`, `pi_init`, `slacks_init`) is set (0) or ignored (1) | `1` | Yes | | `x_init` | Initialization of x for all stages | `sum(nx_i), i=0,..., N` | Yes | | `u_init` | Initialization of u for stages 0 to N-1 | `sum(nu_i), i=0,..., N-1` | Yes | | `pi_init` | Initialization of pi for stages 0 to N-1 | `sum(npi_i), i=0,..., N-1` | Yes | | `slacks_init` | Initialization of slack values for all stages (0 to N) | `2 * ns_total` | Yes | | `rti_phase` | Real-time iteration phase | `1` | Yes | +| `levenberg_marquardt` | Factor for LM regularization | `1` | Yes | ### List of possible outputs diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m index d5b9b6336f..d48238d093 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m @@ -569,6 +569,38 @@ fprintf(output_note) +{%- if simulink_opts.generate_simulink_block == 1 %} +modelName = '{{ name }}_ocp_solver_simulink_block'; +new_system(modelName); +open_system(modelName); + +blockPath = [modelName '/{{ name }}_ocp_solver']; +add_block('simulink/User-Defined Functions/S-Function', blockPath); +set_param(blockPath, 'FunctionName', 'acados_solver_sfunction_{{ name }}'); + +Simulink.Mask.create(blockPath); +{%- if simulink_opts.show_port_info == 1 %} +mask_str = sprintf([ ... + 'global sfun_input_names sfun_output_names\n' ... + 'for i = 1:length(sfun_input_names)\n' ... + ' port_label(''input'', i, sfun_input_names{i})\n' ... + 'end\n' ... + 'for i = 1:length(sfun_output_names)\n' ... + ' port_label(''output'', i, sfun_output_names{i})\n' ... + 'end\n' ... + 'disp("acados OCP")' ... +]); +{%- else %} +mask_str = sprintf('disp("acados OCP")'); +{%- endif %} +mask = Simulink.Mask.get(blockPath); +mask.Display = mask_str; + +save_system(modelName); +close_system(modelName); +disp([newline, 'Created the OCP solver Simulink block in: ', modelName]) +{%- endif %} + % The mask drawing command is: % --- % global sfun_input_names sfun_output_names diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m index af80408ff1..52479753ec 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m @@ -111,6 +111,34 @@ fprintf(output_note) +% create the Simulink block for the integrator +modelName = '{{ model.name }}_sim_solver_simulink_block'; +new_system(modelName); +open_system(modelName); + +blockPath = [modelName '/{{ model.name }}_sim_solver']; +add_block('simulink/User-Defined Functions/S-Function', blockPath); +set_param(blockPath, 'FunctionName', 'acados_sim_solver_sfunction_{{ model.name }}'); + +Simulink.Mask.create(blockPath); +mask_str = sprintf([ ... + 'global sfun_sim_input_names sfun_sim_output_names\n' ... + 'for i = 1:length(sfun_sim_input_names)\n' ... + ' port_label(''input'', i, sfun_sim_input_names{i})\n' ... + 'end\n' ... + 'for i = 1:length(sfun_sim_output_names)\n' ... + ' port_label(''output'', i, sfun_sim_output_names{i})\n' ... + 'end\n' ... + 'disp("acados sim")' ... +]); +mask = Simulink.Mask.get(blockPath); +mask.Display = mask_str; + +save_system(modelName); +close_system(modelName); +disp([newline, 'Created the sim solver Simulink block in: ', modelName]) + + % The mask drawing command is: % --- % global sfun_sim_input_names sfun_sim_output_names @@ -122,4 +150,4 @@ % end % --- % It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands -% (you can access it wirth ctrl+M on the s-function) \ No newline at end of file +% (you can access it with ctrl+M on the s-function) diff --git a/interfaces/acados_template/acados_template/simulink_default_opts.json b/interfaces/acados_template/acados_template/simulink_default_opts.json index 852229681b..2205d8c501 100644 --- a/interfaces/acados_template/acados_template/simulink_default_opts.json +++ b/interfaces/acados_template/acados_template/simulink_default_opts.json @@ -56,5 +56,7 @@ "rti_phase": 0, "levenberg_marquardt": 0 }, - "samplingtime": "t0" + "samplingtime": "t0", + "generate_simulink_block": 1, + "show_port_info": 1 } From db152aa68ac008721add35e55a7cd16383370330 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Jul 2025 10:38:52 +0200 Subject: [PATCH 113/164] Matlab sanity checks in QP scaling getter (#1600) --- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 2 +- interfaces/acados_matlab_octave/AcadosOcpSolver.m | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 0f3425a3b0..dc3a51b9c1 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -252,7 +252,7 @@ void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *fi } else if (!strcmp(field, "nge")) { - *value =dims->nge; + *value = dims->nge; } else if (!strcmp(field, "nhe")) { diff --git a/interfaces/acados_matlab_octave/AcadosOcpSolver.m b/interfaces/acados_matlab_octave/AcadosOcpSolver.m index 9683c4bb8c..9a9c99b799 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpSolver.m +++ b/interfaces/acados_matlab_octave/AcadosOcpSolver.m @@ -203,12 +203,18 @@ function set(obj, field, value, varargin) end function value = get_qp_scaling_constraints(obj, stage) + if strcmp(obj.solver_options.qpscaling_scale_constraints, 'NO_CONSTRAINT_SCALING') + error("QP scaling constraints are not enabled."); + end % returns the qp scaling constraints for the given stage value = obj.t_ocp.get('qpscaling_constr', stage); end function value = get_qp_scaling_objective(obj) % returns the qp scaling objective + if strcmp(obj.solver_options.qpscaling_scale_objective, 'NO_OBJECTIVE_SCALING') + error("QP scaling objective is not enabled."); + end value = obj.t_ocp.get('qpscaling_obj'); end @@ -649,6 +655,7 @@ function compile_ocp_shared_lib(self, export_dir) end end cd(return_dir); + disp('problem specific shared library compiled successfully'); end end % private methods From 62c577074da5a013f0887535dcf05d3ee621c4f4 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 23 Jul 2025 15:15:29 +0200 Subject: [PATCH 114/164] Modify test dependencies for `octave_test_ocp_pendulum_code_reuse` in `CMakeList` (#1601) Recent CI runs sometimes failed in running `octave_test_ocp_pendulum_code_reuse `. I suspect the issue is related to the test execution order. please close it if the modification is not suitable. --- interfaces/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index cfbd0f0049..043cb6f4e2 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -132,7 +132,7 @@ if(ACADOS_OCTAVE) set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS octave_test_OSQP) endif() if(ACADOS_WITH_QPDUNES) - set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS octave_test_qpDUNES) + set_tests_properties(octave_test_ocp_pendulum_code_reuse PROPERTIES DEPENDS "octave_test_qpDUNES;octave_test_OSQP") set_tests_properties(octave_test_OSQP PROPERTIES DEPENDS octave_test_qpDUNES) endif() endif() From e4ef754a1c5958c2220bf5ba15372a05617812ef Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Jul 2025 10:52:05 +0200 Subject: [PATCH 115/164] Improve order of constraint definition in solver templates (#1603) --- acados/ocp_nlp/ocp_nlp_common.c | 2 +- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 4 - .../c_templates_tera/acados_multi_solver.in.c | 286 +++++------ .../c_templates_tera/acados_solver.in.c | 459 +++++++++--------- 4 files changed, 379 insertions(+), 372 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 9b18166463..69edff4f27 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -3418,7 +3418,7 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl config->constraints[ii]->dims_get(config->constraints[ii], dims->constraints[ii], "ns", &module_val); if (dims->ns[ii] != module_val) { - printf("ocp_nlp_sqp_precompute: inconsistent dimension ns for stage %d with constraint module, got %d, module: %d.", + printf("ocp_nlp_precompute_common: inconsistent dimension ns for stage %d with constraint module, got %d, module: %d.", ii, dims->ns[ii], module_val); exit(1); } diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index dc3a51b9c1..3d4d0895fb 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -463,7 +463,6 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, offset = 0; blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_lower(model, nbu, offset); - } else if (!strcmp(field, "ubu")) { @@ -484,14 +483,12 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, offset = nb; blasfeo_pack_dvec(ng, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_lower(model, ng, offset); - } else if (!strcmp(field, "ug")) { offset = 2*nb+ng+nh; blasfeo_pack_dvec(ng, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_upper(model, ng, offset); - } else if (!strcmp(field, "nl_constr_h_fun")) { @@ -560,7 +557,6 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, offset = 2*nb+2*ng+2*nh+ns+nsbu; blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_lower(model, nsbx, offset); - } else if (!strcmp(field, "idxsg")) { 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 6ccc11bac6..3b9c8733b1 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 @@ -1498,43 +1498,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i /**** Constraints phase {{ jj }} ****/ - /* constraints that are the same for initial and intermediate */ -{%- if phases_dims[jj].nsbx > 0 %} -{# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); - - // soft bounds on x - idxsbx = malloc({{ phases_dims[jj].nsbx }} * sizeof(int)); - {%- for i in range(end=phases_dims[jj].nsbx) %} - idxsbx[{{ i }}] = {{ constraints[jj].idxsbx[i] }}; - {%- endfor %} - - lusbx = calloc(2*{{ phases_dims[jj].nsbx }}, sizeof(double)); - lsbx = lusbx; - usbx = lusbx + {{ phases_dims[jj].nsbx }}; - {%- for i in range(end=phases_dims[jj].nsbx) %} - {%- if constraints[jj].lsbx[i] != 0 %} - lsbx[{{ i }}] = {{ constraints[jj].lsbx[i] }}; - {%- endif %} - {%- if constraints[jj].usbx[i] != 0 %} - usbx[{{ i }}] = {{ constraints[jj].usbx[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); - } - free(idxsbx); - free(lusbx); -{%- endif %} - - {%- if phases_dims[jj].nbu > 0 %} // u idxbu = malloc({{ phases_dims[jj].nbu }} * sizeof(int)); @@ -1563,6 +1527,56 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(lubu); {%- endif %} + +{% if phases_dims[jj].ng > 0 %} + // set up general constraints for stage 0 to N-1 + D = calloc({{ phases_dims[jj].ng }}*{{ phases_dims[jj].nu }}, sizeof(double)); + C = calloc({{ phases_dims[jj].ng }}*{{ phases_dims[jj].nx }}, sizeof(double)); + lug = calloc(2*{{ phases_dims[jj].ng }}, sizeof(double)); + lg = lug; + ug = lug + {{ phases_dims[jj].ng }}; + + {%- for j in range(end=phases_dims[jj].ng) -%} + {% for k in range(end=phases_dims[jj].nu) %} + {%- if constraints[jj].D[j][k] != 0 %} + D[{{ j }}+{{ phases_dims[jj].ng }} * {{ k }}] = {{ constraints[jj].D[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {%- for j in range(end=phases_dims[jj].ng) -%} + {% for k in range(end=phases_dims[jj].nx) %} + {%- if constraints[jj].C[j][k] != 0 %} + C[{{ j }}+{{ phases_dims[jj].ng }} * {{ k }}] = {{ constraints[jj].C[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {%- for i in range(end=phases_dims[jj].ng) %} + {%- if constraints[jj].lg[i] != 0 %} + lg[{{ i }}] = {{ constraints[jj].lg[i] }}; + {%- endif %} + {%- endfor %} + + {%- for i in range(end=phases_dims[jj].ng) %} + {%- if constraints[jj].ug[i] != 0 %} + ug[{{ i }}] = {{ constraints[jj].ug[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); + } + free(D); + free(C); + free(lug); +{%- endif %} + + {%- if phases_dims[jj].nsbu > 0 %} // set up soft bounds for u idxsbu = malloc({{ phases_dims[jj].nsbu }} * sizeof(int)); @@ -1618,62 +1632,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(lusg); {%- endif %} -{% if phases_dims[jj].nsh > 0 %} - // set up soft bounds for nonlinear constraints - idxsh = malloc({{ phases_dims[jj].nsh }} * sizeof(int)); - {%- for i in range(end=phases_dims[jj].nsh) %} - idxsh[{{ i }}] = {{ constraints[jj].idxsh[i] }}; - {%- endfor %} - lush = calloc(2*{{ phases_dims[jj].nsh }}, sizeof(double)); - lsh = lush; - ush = lush + {{ phases_dims[jj].nsh }}; - {%- for i in range(end=phases_dims[jj].nsh) %} - {%- if constraints[jj].lsh[i] != 0 %} - lsh[{{ i }}] = {{ constraints[jj].lsh[i] }}; - {%- endif %} - {%- if constraints[jj].ush[i] != 0 %} - ush[{{ i }}] = {{ constraints[jj].ush[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); - } - free(idxsh); - free(lush); -{%- endif %} - -{% if phases_dims[jj].nsphi > 0 %} - // set up soft bounds for convex-over-nonlinear constraints - idxsphi = malloc({{ phases_dims[jj].nsphi }} * sizeof(int)); - {%- for i in range(end=phases_dims[jj].nsphi) %} - idxsphi[{{ i }}] = {{ constraints[jj].idxsphi[i] }}; - {%- endfor %} - lusphi = calloc(2*{{ phases_dims[jj].nsphi }}, sizeof(double)); - lsphi = lusphi; - usphi = lusphi + {{ phases_dims[jj].nsphi }}; - {%- for i in range(end=phases_dims[jj].nsphi) %} - {%- if constraints[jj].lsphi[i] != 0 %} - lsphi[{{ i }}] = {{ constraints[jj].lsphi[i] }}; - {%- endif %} - {%- if constraints[jj].usphi[i] != 0 %} - usphi[{{ i }}] = {{ constraints[jj].usphi[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); - } - free(idxsphi); - free(lusphi); -{%- endif %} - + /* Path constraints */ {% if phases_dims[jj].nbx > 0 %} // x idxbx = malloc({{ phases_dims[jj].nbx }} * sizeof(int)); @@ -1702,54 +1661,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(lubx); {%- endif %} -{% if phases_dims[jj].ng > 0 %} - // set up general constraints for stage 0 to N-1 - D = calloc({{ phases_dims[jj].ng }}*{{ phases_dims[jj].nu }}, sizeof(double)); - C = calloc({{ phases_dims[jj].ng }}*{{ phases_dims[jj].nx }}, sizeof(double)); - lug = calloc(2*{{ phases_dims[jj].ng }}, sizeof(double)); - lg = lug; - ug = lug + {{ phases_dims[jj].ng }}; - - {%- for j in range(end=phases_dims[jj].ng) -%} - {% for k in range(end=phases_dims[jj].nu) %} - {%- if constraints[jj].D[j][k] != 0 %} - D[{{ j }}+{{ phases_dims[jj].ng }} * {{ k }}] = {{ constraints[jj].D[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - - {%- for j in range(end=phases_dims[jj].ng) -%} - {% for k in range(end=phases_dims[jj].nx) %} - {%- if constraints[jj].C[j][k] != 0 %} - C[{{ j }}+{{ phases_dims[jj].ng }} * {{ k }}] = {{ constraints[jj].C[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - - {%- for i in range(end=phases_dims[jj].ng) %} - {%- if constraints[jj].lg[i] != 0 %} - lg[{{ i }}] = {{ constraints[jj].lg[i] }}; - {%- endif %} - {%- endfor %} - - {%- for i in range(end=phases_dims[jj].ng) %} - {%- if constraints[jj].ug[i] != 0 %} - ug[{{ i }}] = {{ constraints[jj].ug[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); - } - free(D); - free(C); - free(lug); -{%- endif %} - {% if phases_dims[jj].nh > 0 %} // set up nonlinear constraints for stage 1 to N-1 luh = calloc(2*{{ phases_dims[jj].nh }}, sizeof(double)); @@ -1821,7 +1732,100 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i } free(luphi); {%- endif %} -{%- endfor %} + + +{%- if phases_dims[jj].nsbx > 0 %} +{# TODO: introduce nsbx0 #} + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); + + // soft bounds on x + idxsbx = malloc({{ phases_dims[jj].nsbx }} * sizeof(int)); + {%- for i in range(end=phases_dims[jj].nsbx) %} + idxsbx[{{ i }}] = {{ constraints[jj].idxsbx[i] }}; + {%- endfor %} + + lusbx = calloc(2*{{ phases_dims[jj].nsbx }}, sizeof(double)); + lsbx = lusbx; + usbx = lusbx + {{ phases_dims[jj].nsbx }}; + {%- for i in range(end=phases_dims[jj].nsbx) %} + {%- if constraints[jj].lsbx[i] != 0 %} + lsbx[{{ i }}] = {{ constraints[jj].lsbx[i] }}; + {%- endif %} + {%- if constraints[jj].usbx[i] != 0 %} + usbx[{{ i }}] = {{ constraints[jj].usbx[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); + } + free(idxsbx); + free(lusbx); +{%- endif %} + + +{% if phases_dims[jj].nsh > 0 %} + // set up soft bounds for nonlinear constraints + idxsh = malloc({{ phases_dims[jj].nsh }} * sizeof(int)); + {%- for i in range(end=phases_dims[jj].nsh) %} + idxsh[{{ i }}] = {{ constraints[jj].idxsh[i] }}; + {%- endfor %} + lush = calloc(2*{{ phases_dims[jj].nsh }}, sizeof(double)); + lsh = lush; + ush = lush + {{ phases_dims[jj].nsh }}; + {%- for i in range(end=phases_dims[jj].nsh) %} + {%- if constraints[jj].lsh[i] != 0 %} + lsh[{{ i }}] = {{ constraints[jj].lsh[i] }}; + {%- endif %} + {%- if constraints[jj].ush[i] != 0 %} + ush[{{ i }}] = {{ constraints[jj].ush[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); + } + free(idxsh); + free(lush); +{%- endif %} + +{% if phases_dims[jj].nsphi > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + idxsphi = malloc({{ phases_dims[jj].nsphi }} * sizeof(int)); + {%- for i in range(end=phases_dims[jj].nsphi) %} + idxsphi[{{ i }}] = {{ constraints[jj].idxsphi[i] }}; + {%- endfor %} + lusphi = calloc(2*{{ phases_dims[jj].nsphi }}, sizeof(double)); + lsphi = lusphi; + usphi = lusphi + {{ phases_dims[jj].nsphi }}; + {%- for i in range(end=phases_dims[jj].nsphi) %} + {%- if constraints[jj].lsphi[i] != 0 %} + lsphi[{{ i }}] = {{ constraints[jj].lsphi[i] }}; + {%- endif %} + {%- if constraints[jj].usphi[i] != 0 %} + usphi[{{ i }}] = {{ constraints[jj].usphi[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); + } + free(idxsphi); + free(lusphi); +{%- endif %} + +{%- endfor %}{# phases loop !#} 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 62cd866e34..b699fcfe22 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 @@ -1642,41 +1642,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} /* constraints that are the same for initial and intermediate */ -{%- if dims.nsbx > 0 %} -{# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); - - // soft bounds on x - int* idxsbx = malloc(NSBX * sizeof(int)); - {%- for i in range(end=dims.nsbx) %} - idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; - {%- endfor %} - - double* lusbx = calloc(2*NSBX, sizeof(double)); - double* lsbx = lusbx; - double* usbx = lusbx + NSBX; - {%- for i in range(end=dims.nsbx) %} - {%- if constraints.lsbx[i] != 0 %} - lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; - {%- endif %} - {%- if constraints.usbx[i] != 0 %} - usbx[{{ i }}] = {{ constraints.usbx[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = 1; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); - } - free(idxsbx); - free(lusbx); -{%- endif %} - - {%- if dims.nbu > 0 %} // u int* idxbu = malloc(NBU * sizeof(int)); @@ -1705,6 +1670,55 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(lubu); {%- endif %} +{% if dims.ng > 0 %} + // set up general constraints for stage 0 to N-1 + double* D = calloc(NG*NU, sizeof(double)); + double* C = calloc(NG*NX, sizeof(double)); + double* lug = calloc(2*NG, sizeof(double)); + double* lg = lug; + double* ug = lug + NG; + + {%- for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nu) %} + {%- if constraints.D[j][k] != 0 %} + D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {%- for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nx) %} + {%- if constraints.C[j][k] != 0 %} + C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {%- for i in range(end=dims.ng) %} + {%- if constraints.lg[i] != 0 %} + lg[{{ i }}] = {{ constraints.lg[i] }}; + {%- endif %} + {%- endfor %} + + {%- for i in range(end=dims.ng) %} + {%- if constraints.ug[i] != 0 %} + ug[{{ i }}] = {{ constraints.ug[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); + } + free(D); + free(C); + free(lug); +{%- endif %} + + {%- if dims.nsbu > 0 %} // set up soft bounds for u int* idxsbu = malloc(NSBU * sizeof(int)); @@ -1760,62 +1774,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(lusg); {%- endif %} -{% if dims.nsh > 0 %} - // set up soft bounds for nonlinear constraints - int* idxsh = malloc(NSH * sizeof(int)); - {%- for i in range(end=dims.nsh) %} - idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; - {%- endfor %} - double* lush = calloc(2*NSH, sizeof(double)); - double* lsh = lush; - double* ush = lush + NSH; - {%- for i in range(end=dims.nsh) %} - {%- if constraints.lsh[i] != 0 %} - lsh[{{ i }}] = {{ constraints.lsh[i] }}; - {%- endif %} - {%- if constraints.ush[i] != 0 %} - ush[{{ i }}] = {{ constraints.ush[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = 1; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); - } - free(idxsh); - free(lush); -{%- endif %} - -{% if dims.nsphi > 0 %} - // set up soft bounds for convex-over-nonlinear constraints - int* idxsphi = malloc(NSPHI * sizeof(int)); - {%- for i in range(end=dims.nsphi) %} - idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; - {%- endfor %} - double* lusphi = calloc(2*NSPHI, sizeof(double)); - double* lsphi = lusphi; - double* usphi = lusphi + NSPHI; - {%- for i in range(end=dims.nsphi) %} - {%- if constraints.lsphi[i] != 0 %} - lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; - {%- endif %} - {%- if constraints.usphi[i] != 0 %} - usphi[{{ i }}] = {{ constraints.usphi[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = 1; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); - } - free(idxsphi); - free(lusphi); -{%- endif %} + /* Path constraints */ {% if dims.nbx > 0 %} // x int* idxbx = malloc(NBX * sizeof(int)); @@ -1844,54 +1804,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(lubx); {%- endif %} -{% if dims.ng > 0 %} - // set up general constraints for stage 0 to N-1 - double* D = calloc(NG*NU, sizeof(double)); - double* C = calloc(NG*NX, sizeof(double)); - double* lug = calloc(2*NG, sizeof(double)); - double* lg = lug; - double* ug = lug + NG; - - {%- for j in range(end=dims.ng) -%} - {% for k in range(end=dims.nu) %} - {%- if constraints.D[j][k] != 0 %} - D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - - {%- for j in range(end=dims.ng) -%} - {% for k in range(end=dims.nx) %} - {%- if constraints.C[j][k] != 0 %} - C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - - {%- for i in range(end=dims.ng) %} - {%- if constraints.lg[i] != 0 %} - lg[{{ i }}] = {{ constraints.lg[i] }}; - {%- endif %} - {%- endfor %} - - {%- for i in range(end=dims.ng) %} - {%- if constraints.ug[i] != 0 %} - ug[{{ i }}] = {{ constraints.ug[i] }}; - {%- endif %} - {%- endfor %} - - for (int i = 0; i < N; i++) - { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); - } - free(D); - free(C); - free(lug); -{%- endif %} - {% if dims.nh > 0 %} // set up nonlinear constraints for stage 1 to N-1 double* luh = calloc(2*NH, sizeof(double)); @@ -1962,6 +1874,96 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(luphi); {%- endif %} + +{%- if dims.nsbx > 0 %} +{# TODO: introduce nsbx0 #} + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); + + // soft bounds on x + int* idxsbx = malloc(NSBX * sizeof(int)); + {%- for i in range(end=dims.nsbx) %} + idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; + {%- endfor %} + + double* lusbx = calloc(2*NSBX, sizeof(double)); + double* lsbx = lusbx; + double* usbx = lusbx + NSBX; + {%- for i in range(end=dims.nsbx) %} + {%- if constraints.lsbx[i] != 0 %} + lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; + {%- endif %} + {%- if constraints.usbx[i] != 0 %} + usbx[{{ i }}] = {{ constraints.usbx[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); + } + free(idxsbx); + free(lusbx); +{%- endif %} + +{% if dims.nsh > 0 %} + // set up soft bounds for nonlinear constraints + int* idxsh = malloc(NSH * sizeof(int)); + {%- for i in range(end=dims.nsh) %} + idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; + {%- endfor %} + double* lush = calloc(2*NSH, sizeof(double)); + double* lsh = lush; + double* ush = lush + NSH; + {%- for i in range(end=dims.nsh) %} + {%- if constraints.lsh[i] != 0 %} + lsh[{{ i }}] = {{ constraints.lsh[i] }}; + {%- endif %} + {%- if constraints.ush[i] != 0 %} + ush[{{ i }}] = {{ constraints.ush[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); + } + free(idxsh); + free(lush); +{%- endif %} + +{% if dims.nsphi > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int* idxsphi = malloc(NSPHI * sizeof(int)); + {%- for i in range(end=dims.nsphi) %} + idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; + {%- endfor %} + double* lusphi = calloc(2*NSPHI, sizeof(double)); + double* lsphi = lusphi; + double* usphi = lusphi + NSPHI; + {%- for i in range(end=dims.nsphi) %} + {%- if constraints.lsphi[i] != 0 %} + lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; + {%- endif %} + {%- if constraints.usphi[i] != 0 %} + usphi[{{ i }}] = {{ constraints.usphi[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); + } + free(idxsphi); + free(lusphi); +{%- endif %} {%- endif %}{# solver_options.N_horizon > 0 #} /* terminal constraints */ @@ -1990,6 +1992,99 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(lubx_e); {%- endif %} +{% if dims.ng_e > 0 %} + // set up general constraints for last stage + double* C_e = calloc(NGN*NX, sizeof(double)); + double* lug_e = calloc(2*NGN, sizeof(double)); + double* lg_e = lug_e; + double* ug_e = lug_e + NGN; + + {%- for j in range(end=dims.ng_e) %} + {%- for k in range(end=dims.nx) %} + {%- if constraints.C_e[j][k] != 0 %} + C_e[{{ j }}+NGN * {{ k }}] = {{ constraints.C_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + + {%- for i in range(end=dims.ng_e) %} + {%- if constraints.lg_e[i] != 0 %} + lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; + {%- endif %} + {%- if constraints.ug_e[i] != 0 %} + ug_e[{{ i }}] = {{ constraints.ug_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); + free(C_e); + free(lug_e); +{%- endif %} + +{% if dims.nh_e > 0 %} + // set up nonlinear constraints for last stage + double* luh_e = calloc(2*NHN, sizeof(double)); + double* lh_e = luh_e; + double* uh_e = luh_e + NHN; + {%- for i in range(end=dims.nh_e) %} + {%- if constraints.lh_e[i] != 0 %} + lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; + {%- endif %} + {%- endfor %} + + {%- for i in range(end=dims.nh_e) %} + {%- if constraints.uh_e[i] != 0 %} + uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); + {% if solver_options.hessian_approx == "EXACT" %} + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", + &capsule->nl_constr_h_e_fun_jac_hess); + {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); + {% if solver_options.with_solution_sens_wrt_params %} + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", + &capsule->nl_constr_h_e_jac_p_hess_xu_p); + {% endif %} + {% if solver_options.with_value_sens_wrt_params %} + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", + &capsule->nl_constr_h_e_adj_p); + {% endif %} + free(luh_e); +{%- elif dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} + // set up convex-over-nonlinear constraints for last stage + double* luphi_e = calloc(2*NPHIN, sizeof(double)); + double* lphi_e = luphi_e; + double* uphi_e = luphi_e + NPHIN; + {%- for i in range(end=dims.nphi_e) %} + {%- if constraints.lphi_e[i] != 0 %} + lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; + {%- endif %} + {%- if constraints.uphi_e[i] != 0 %} + uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, + "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); + ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, + "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint_fun_jac_hess); + free(luphi_e); +{% endif %} + + +{% if dims.ns_e > 0 %} + /* terminal soft constraints */ +{% endif %} + {% if dims.nsg_e > 0 %} // set up soft bounds for general linear constraints int* idxsg_e = calloc(NSGN, sizeof(int)); @@ -2089,94 +2184,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(idxsbx_e); free(lusbx_e); {% endif %} - -{% if dims.ng_e > 0 %} - // set up general constraints for last stage - double* C_e = calloc(NGN*NX, sizeof(double)); - double* lug_e = calloc(2*NGN, sizeof(double)); - double* lg_e = lug_e; - double* ug_e = lug_e + NGN; - - {%- for j in range(end=dims.ng_e) %} - {%- for k in range(end=dims.nx) %} - {%- if constraints.C_e[j][k] != 0 %} - C_e[{{ j }}+NGN * {{ k }}] = {{ constraints.C_e[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - - {%- for i in range(end=dims.ng_e) %} - {%- if constraints.lg_e[i] != 0 %} - lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; - {%- endif %} - {%- if constraints.ug_e[i] != 0 %} - ug_e[{{ i }}] = {{ constraints.ug_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); - free(C_e); - free(lug_e); -{%- endif %} - -{% if dims.nh_e > 0 %} - // set up nonlinear constraints for last stage - double* luh_e = calloc(2*NHN, sizeof(double)); - double* lh_e = luh_e; - double* uh_e = luh_e + NHN; - {%- for i in range(end=dims.nh_e) %} - {%- if constraints.lh_e[i] != 0 %} - lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; - {%- endif %} - {%- endfor %} - - {%- for i in range(end=dims.nh_e) %} - {%- if constraints.uh_e[i] != 0 %} - uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); - {% if solver_options.hessian_approx == "EXACT" %} - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", - &capsule->nl_constr_h_e_fun_jac_hess); - {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); - {% if solver_options.with_solution_sens_wrt_params %} - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", - &capsule->nl_constr_h_e_jac_p_hess_xu_p); - {% endif %} - {% if solver_options.with_value_sens_wrt_params %} - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", - &capsule->nl_constr_h_e_adj_p); - {% endif %} - free(luh_e); -{%- elif dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} - // set up convex-over-nonlinear constraints for last stage - double* luphi_e = calloc(2*NPHIN, sizeof(double)); - double* lphi_e = luphi_e; - double* uphi_e = luphi_e + NPHIN; - {%- for i in range(end=dims.nphi_e) %} - {%- if constraints.lphi_e[i] != 0 %} - lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; - {%- endif %} - {%- if constraints.uphi_e[i] != 0 %} - uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, - "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); - ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, - "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint_fun_jac_hess); - free(luphi_e); -{% endif %} } From 88033f70cf5b88df2561391f74eb66fd1c0d187a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Jul 2025 10:52:11 +0200 Subject: [PATCH 116/164] Update HPIPM (#1602) - HPIPM now supports `idxs_rev` - relax tests as some behavior changed close to numerical precision --- .../linear_mass_model/linear_mass_test_problem.py | 4 ++-- .../acados_python/linear_mass_model/test_qpscaling_slacked.py | 2 +- external/hpipm | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py index 4b07aa3651..0580578a78 100644 --- a/examples/acados_python/linear_mass_model/linear_mass_test_problem.py +++ b/examples/acados_python/linear_mass_model/linear_mass_test_problem.py @@ -250,8 +250,8 @@ def solve_maratos_ocp(setting, use_deprecated_options=False): if sqp_iter != 18: raise Exception(f"acados solver took {sqp_iter} iterations, expected 18.") elif globalization == "MERIT_BACKTRACKING" and not use_deprecated_options: - if sqp_iter not in range(17, 23): - raise Exception(f"acados solver took {sqp_iter} iterations, expected range(17, 23).") + if sqp_iter not in range(15, 23): + raise Exception(f"acados solver took {sqp_iter} iterations, expected range(15, 23).") if PLOT: plot_linear_mass_system_X_state_space(simX, circle=circle, x_goal=x_goal) diff --git a/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py index 49161e0150..6e92f91e71 100644 --- a/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py +++ b/examples/acados_python/linear_mass_model/test_qpscaling_slacked.py @@ -229,7 +229,7 @@ def check_iteration_residual(stat_list = list[np.ndarray]): n_iter = stat.shape[1] if n_iter != n_iter_ref: raise ValueError(f"Statistics have different number of rows: {n_iter} vs {n_iter_ref}") - for jj in range(n_iter_ref): + for jj in range(n_iter_ref-1): # skip last iteration due to differences close to solver precision. # res_stat if not np.allclose(stats_ref[1, jj], stat[1, jj]): raise ValueError(f"res_stat differs in iter {jj}") diff --git a/external/hpipm b/external/hpipm index 36cef134c5..1891706e9c 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 36cef134c5dbca94088f3dc0a4fc2f57c12486fd +Subproject commit 1891706e9cf0cf5e47df8e2317b653daf0fc695e From 839ad2aaee80c4a79395deb6f7a3b2feabdef658 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 11 Aug 2025 16:26:16 +0200 Subject: [PATCH 117/164] Template fixes (#1606) - added `if (N > 0) in C template to fix solver warning: "exceeds maximum object size" see https://github.com/acados/acados/issues/1529 - minor fix in multi-phase --- .../c_templates_tera/acados_multi_solver.in.c | 4 +- .../c_templates_tera/acados_solver.in.c | 615 +++++++++--------- 2 files changed, 311 insertions(+), 308 deletions(-) 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 3b9c8733b1..9d1b2f64f7 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 @@ -516,13 +516,13 @@ void {{ name }}_acados_create_setup_functions({{ name }}_solver_capsule* capsule } {% endif %} {%- if solver_options.with_solution_sens_wrt_params %} - capsule->nl_constr_h_jac_p_hess_xu_p_{ jj } = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + capsule->nl_constr_h_jac_p_hess_xu_p_{ jj } = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*n_cost_path); for (int i = 0; i < n_cost_path; i++) { MAP_CASADI_FNC(nl_constr_h_jac_p_hess_xu_p_{{ jj }}[i], {{ model[jj].name }}_constr_h_jac_p_hess_xu_p); } {%- endif %} {%- if solver_options.with_value_sens_wrt_params %} - capsule->nl_constr_h_adj_p_{ jj } = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + capsule->nl_constr_h_adj_p_{ jj } = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*n_cost_path); for (int i = 0; i < n_cost_path; i++) { MAP_CASADI_FNC(nl_constr_h_adj_p_{{ jj }}[i], {{ model[jj].name }}_constr_h_adj_p); } 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 b699fcfe22..8a89d2cd55 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 @@ -467,363 +467,366 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps ext_fun_opts.external_workspace = true; {%- if solver_options.N_horizon > 0 %} -{%- if constraints.constr_type_0 == "BGH" and dims.nh_0 > 0 %} - MAP_CASADI_FNC(nl_constr_h_0_fun_jac, {{ model.name }}_constr_h_0_fun_jac_uxt_zt); - MAP_CASADI_FNC(nl_constr_h_0_fun, {{ model.name }}_constr_h_0_fun); - - {%- if solver_options.hessian_approx == "EXACT" %} - MAP_CASADI_FNC(nl_constr_h_0_fun_jac_hess, {{ model.name }}_constr_h_0_fun_jac_uxt_zt_hess); - {% endif %} - {%- if solver_options.with_solution_sens_wrt_params %} - MAP_CASADI_FNC(nl_constr_h_0_jac_p_hess_xu_p, {{ model.name }}_constr_h_0_jac_p_hess_xu_p); - {%- endif %} - {%- if solver_options.with_value_sens_wrt_params %} - MAP_CASADI_FNC(nl_constr_h_0_adj_p, {{ model.name }}_constr_h_0_adj_p); - {%- endif %} -{%- elif constraints.constr_type_0 == "BGP" %} - // convex-over-nonlinear constraint - MAP_CASADI_FNC(phi_0_constraint_fun_jac_hess, {{ model.name }}_phi_0_constraint_fun_jac_hess); - MAP_CASADI_FNC(phi_0_constraint_fun, {{ model.name }}_phi_0_constraint_fun); -{%- endif %} - - - -{%- if constraints.constr_type == "BGH" and dims.nh > 0 %} - // constraints.constr_type == "BGH" and dims.nh > 0 - capsule->nl_constr_h_fun_jac = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) { - MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt); - } - capsule->nl_constr_h_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) { - MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun); - } - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->nl_constr_h_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) { - MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess); - } - {%- endif %} - {%- if solver_options.with_solution_sens_wrt_params %} - capsule->nl_constr_h_jac_p_hess_xu_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) { - MAP_CASADI_FNC(nl_constr_h_jac_p_hess_xu_p[i], {{ model.name }}_constr_h_jac_p_hess_xu_p); - } - {%- endif %} - {%- if solver_options.with_value_sens_wrt_params %} - capsule->nl_constr_h_adj_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) { - MAP_CASADI_FNC(nl_constr_h_adj_p[i], {{ model.name }}_constr_h_adj_p); - } - {%- endif %} -{% elif constraints.constr_type == "BGP" %} - capsule->phi_constraint_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - capsule->phi_constraint_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) + if (N > 0) { - // convex-over-nonlinear constraint - MAP_CASADI_FNC(phi_constraint_fun_jac_hess[i], {{ model.name }}_phi_constraint_fun_jac_hess); - MAP_CASADI_FNC(phi_constraint_fun[i], {{ model.name }}_phi_constraint_fun); - } -{%- endif %} - + {%- if constraints.constr_type_0 == "BGH" and dims.nh_0 > 0 %} + MAP_CASADI_FNC(nl_constr_h_0_fun_jac, {{ model.name }}_constr_h_0_fun_jac_uxt_zt); + MAP_CASADI_FNC(nl_constr_h_0_fun, {{ model.name }}_constr_h_0_fun); -{%- if cost.cost_type_0 == "NONLINEAR_LS" %} - // nonlinear least squares function - MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun); - MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); - {%- if solver_options.hessian_approx == "EXACT" %} - MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); - {%- endif %} - -{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} - // convex-over-nonlinear cost - MAP_CASADI_FNC(conl_cost_0_fun, {{ model.name }}_conl_cost_0_fun); - MAP_CASADI_FNC(conl_cost_0_fun_jac_hess, {{ model.name }}_conl_cost_0_fun_jac_hess); - -{%- elif cost.cost_type_0 == "EXTERNAL" %} - // external cost - {%- if cost.cost_ext_fun_type_0 == "casadi" %} - MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun); - {%- else %} - capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }}; - external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, &ext_fun_opts); - {%- endif %} - - {%- if cost.cost_ext_fun_type_0 == "casadi" %} - MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac); - {%- else %} - capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }}; - external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, &ext_fun_opts); + {%- if solver_options.hessian_approx == "EXACT" %} + MAP_CASADI_FNC(nl_constr_h_0_fun_jac_hess, {{ model.name }}_constr_h_0_fun_jac_uxt_zt_hess); + {% endif %} + {%- if solver_options.with_solution_sens_wrt_params %} + MAP_CASADI_FNC(nl_constr_h_0_jac_p_hess_xu_p, {{ model.name }}_constr_h_0_jac_p_hess_xu_p); + {%- endif %} + {%- if solver_options.with_value_sens_wrt_params %} + MAP_CASADI_FNC(nl_constr_h_0_adj_p, {{ model.name }}_constr_h_0_adj_p); + {%- endif %} + {%- elif constraints.constr_type_0 == "BGP" %} + // convex-over-nonlinear constraint + MAP_CASADI_FNC(phi_0_constraint_fun_jac_hess, {{ model.name }}_phi_0_constraint_fun_jac_hess); + MAP_CASADI_FNC(phi_0_constraint_fun, {{ model.name }}_phi_0_constraint_fun); {%- endif %} - {%- if cost.cost_ext_fun_type_0 == "casadi" %} - MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess); - {%- else %} - capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }}; - external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, &ext_fun_opts); - {%- endif %} - {%- if solver_options.with_solution_sens_wrt_params %} - MAP_CASADI_FNC(ext_cost_0_hess_xu_p, {{ model.name }}_cost_ext_cost_0_hess_xu_p); - {%- endif %} - {%- if solver_options.with_value_sens_wrt_params %} - MAP_CASADI_FNC(ext_cost_0_grad_p, {{ model.name }}_cost_ext_cost_0_grad_p); + {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} + // constraints.constr_type == "BGH" and dims.nh > 0 + capsule->nl_constr_h_fun_jac = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) { + MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt); + } + capsule->nl_constr_h_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) { + MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun); + } + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) { + MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess); + } + {%- endif %} + {%- if solver_options.with_solution_sens_wrt_params %} + capsule->nl_constr_h_jac_p_hess_xu_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) { + MAP_CASADI_FNC(nl_constr_h_jac_p_hess_xu_p[i], {{ model.name }}_constr_h_jac_p_hess_xu_p); + } + {%- endif %} + {%- if solver_options.with_value_sens_wrt_params %} + capsule->nl_constr_h_adj_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) { + MAP_CASADI_FNC(nl_constr_h_adj_p[i], {{ model.name }}_constr_h_adj_p); + } + {%- endif %} + {% elif constraints.constr_type == "BGP" %} + capsule->phi_constraint_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + capsule->phi_constraint_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + // convex-over-nonlinear constraint + MAP_CASADI_FNC(phi_constraint_fun_jac_hess[i], {{ model.name }}_phi_constraint_fun_jac_hess); + MAP_CASADI_FNC(phi_constraint_fun[i], {{ model.name }}_phi_constraint_fun); + } {%- endif %} -{%- endif %} + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + // nonlinear least squares function + MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun); + MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); + {%- if solver_options.hessian_approx == "EXACT" %} + MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); + {%- endif %} -{% if solver_options.integrator_type == "ERK" %} - // explicit ode - capsule->expl_vde_forw = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_vde_forw[i], {{ model.name }}_expl_vde_forw); - } + {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + // convex-over-nonlinear cost + MAP_CASADI_FNC(conl_cost_0_fun, {{ model.name }}_conl_cost_0_fun); + MAP_CASADI_FNC(conl_cost_0_fun_jac_hess, {{ model.name }}_conl_cost_0_fun_jac_hess); - capsule->expl_ode_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun); - } + {%- elif cost.cost_type_0 == "EXTERNAL" %} + // external cost + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun); + {%- else %} + capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, &ext_fun_opts); + {%- endif %} - capsule->expl_vde_adj = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_vde_adj[i], {{ model.name }}_expl_vde_adj); - } + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac); + {%- else %} + capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, &ext_fun_opts); + {%- endif %} - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->expl_ode_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(expl_ode_hess[i], {{ model.name }}_expl_ode_hess); - } - {%- endif %} + {%- if cost.cost_ext_fun_type_0 == "casadi" %} + MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess); + {%- else %} + capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }}; + external_function_external_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, &ext_fun_opts); + {%- endif %} -{% elif solver_options.integrator_type == "IRK" %} - // implicit dae - capsule->impl_dae_fun = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); - {%- else %} - capsule->impl_dae_fun[i].fun = &{{ model.dyn_impl_dae_fun }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun[i], &ext_fun_opts); - {%- endif %} - } + {%- if solver_options.with_solution_sens_wrt_params %} + MAP_CASADI_FNC(ext_cost_0_hess_xu_p, {{ model.name }}_cost_ext_cost_0_hess_xu_p); + {%- endif %} - capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); - {%- else %} - capsule->impl_dae_fun_jac_x_xdot_z[i].fun = &{{ model.dyn_impl_dae_fun_jac }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], &ext_fun_opts); + {%- if solver_options.with_value_sens_wrt_params %} + MAP_CASADI_FNC(ext_cost_0_grad_p, {{ model.name }}_cost_ext_cost_0_grad_p); + {%- endif %} {%- endif %} - } - capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) { - {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); - {%- else %} - capsule->impl_dae_jac_x_xdot_u_z[i].fun = &{{ model.dyn_impl_dae_jac }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_jac_x_xdot_u_z[i], &ext_fun_opts); - {%- endif %} - } - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->impl_dae_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); - } - {%- endif %} -{% elif solver_options.integrator_type == "LIFTED_IRK" %} - // external functions (implicit model) - capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); - } - capsule->impl_dae_fun_jac_x_xdot_u = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); - } + {% if solver_options.integrator_type == "ERK" %} + // explicit ode + capsule->expl_vde_forw = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_vde_forw[i], {{ model.name }}_expl_vde_forw); + } -{% elif solver_options.integrator_type == "GNSF" %} - {% if model.gnsf_purely_linear != 1 %} - capsule->gnsf_phi_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun); - } + capsule->expl_ode_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun); + } - capsule->gnsf_phi_fun_jac_y = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y); - } + capsule->expl_vde_adj = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_vde_adj[i], {{ model.name }}_expl_vde_adj); + } - capsule->gnsf_phi_jac_y_uhat = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat); - } + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->expl_ode_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(expl_ode_hess[i], {{ model.name }}_expl_ode_hess); + } + {%- endif %} - {% if model.gnsf_nontrivial_f_LO == 1 %} - capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz); - } - {%- endif %} - {%- endif %} - capsule->gnsf_get_matrices_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) { - MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun); - } -{% elif solver_options.integrator_type == "DISCRETE" %} - // discrete dynamics - capsule->discr_dyn_phi_fun = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) - { + {% elif solver_options.integrator_type == "IRK" %} + // implicit dae + capsule->impl_dae_fun = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun); + MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); {%- else %} - capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], &ext_fun_opts); + capsule->impl_dae_fun[i].fun = &{{ model.dyn_impl_dae_fun }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun[i], &ext_fun_opts); {%- endif %} - } + } - capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) - { + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac); + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); {%- else %} - capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], &ext_fun_opts); + capsule->impl_dae_fun_jac_x_xdot_z[i].fun = &{{ model.dyn_impl_dae_fun_jac }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], &ext_fun_opts); {%- endif %} - } + } - {% if solver_options.with_solution_sens_wrt_params %} - capsule->discr_dyn_phi_jac_p_hess_xu_p = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) - { + capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(discr_dyn_phi_jac_p_hess_xu_p[i], {{ model.name }}_dyn_disc_phi_jac_p_hess_xu_p); + MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); {%- else %} - capsule->discr_dyn_phi_jac_p_hess_xu_p[i].fun = &{{ model.dyn_disc_params_jac }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_jac_p_hess_xu_p[i], &ext_fun_opts); + capsule->impl_dae_jac_x_xdot_u_z[i].fun = &{{ model.dyn_impl_dae_jac }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_jac_x_xdot_u_z[i], &ext_fun_opts); {%- endif %} - } - {% endif %} - - {% if solver_options.with_value_sens_wrt_params %} - capsule->discr_dyn_phi_adj_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); - for (int i = 0; i < N; i++) - { - MAP_CASADI_FNC(discr_dyn_phi_adj_p[i], {{ model.name }}_dyn_disc_phi_adj_p); - } - {% endif %} + } - {%- if solver_options.hessian_approx == "EXACT" %} - capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); - for (int i = 0; i < N; i++) - { - {%- if model.dyn_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess); - {%- else %} - capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }}; - external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], &ext_fun_opts); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); + } {%- endif %} - } - {%- endif %} -{%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + // external functions (implicit model) + capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); + } + capsule->impl_dae_fun_jac_x_xdot_u = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); + } + {% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf_purely_linear != 1 %} + capsule->gnsf_phi_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun); + } -{%- if cost.cost_type == "NONLINEAR_LS" %} - // nonlinear least squares cost - capsule->cost_y_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun); - } + capsule->gnsf_phi_fun_jac_y = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y); + } - capsule->cost_y_fun_jac_ut_xt = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt); - } + capsule->gnsf_phi_jac_y_uhat = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat); + } + + {% if model.gnsf_nontrivial_f_LO == 1 %} + capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz); + } + {%- endif %} + {%- endif %} + capsule->gnsf_get_matrices_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun); + } + {% elif solver_options.integrator_type == "DISCRETE" %} + // discrete dynamics + capsule->discr_dyn_phi_fun = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun); + {%- else %} + capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], &ext_fun_opts); + {%- endif %} + } + + capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac); + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], &ext_fun_opts); + {%- endif %} + } + + {% if solver_options.with_solution_sens_wrt_params %} + capsule->discr_dyn_phi_jac_p_hess_xu_p = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_jac_p_hess_xu_p[i], {{ model.name }}_dyn_disc_phi_jac_p_hess_xu_p); + {%- else %} + capsule->discr_dyn_phi_jac_p_hess_xu_p[i].fun = &{{ model.dyn_disc_params_jac }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_jac_p_hess_xu_p[i], &ext_fun_opts); + {%- endif %} + } + {% endif %} + + {% if solver_options.with_value_sens_wrt_params %} + capsule->discr_dyn_phi_adj_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) + { + MAP_CASADI_FNC(discr_dyn_phi_adj_p[i], {{ model.name }}_dyn_disc_phi_adj_p); + } + {% endif %} {%- if solver_options.hessian_approx == "EXACT" %} - capsule->cost_y_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); - } + capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_external_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ model.dyn_ext_fun_type }})*N); + for (int i = 0; i < N; i++) + { + {%- if model.dyn_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess); + {%- else %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }}; + external_function_external_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], &ext_fun_opts); + {%- endif %} + } + {%- endif %} {%- endif %} -{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} - // convex-over-nonlinear cost - capsule->conl_cost_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(conl_cost_fun[i], {{ model.name }}_conl_cost_fun); - } - capsule->conl_cost_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(conl_cost_fun_jac_hess[i], {{ model.name }}_conl_cost_fun_jac_hess); - } -{%- elif cost.cost_type == "EXTERNAL" %} - // external cost - capsule->ext_cost_fun = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); - for (int i = 0; i < N-1; i++) - { - {%- if cost.cost_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun); - {%- else %} - capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }}; - external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], &ext_fun_opts); - {%- endif %} - } - capsule->ext_cost_fun_jac = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); - for (int i = 0; i < N-1; i++) - { - {%- if cost.cost_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac); - {%- else %} - capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }}; - external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], &ext_fun_opts); - {%- endif %} - } + {%- if cost.cost_type == "NONLINEAR_LS" %} + // nonlinear least squares cost + capsule->cost_y_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun); + } - capsule->ext_cost_fun_jac_hess = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); - for (int i = 0; i < N-1; i++) - { - {%- if cost.cost_ext_fun_type == "casadi" %} - MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess); - {%- else %} - capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }}; - external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], &ext_fun_opts); + capsule->cost_y_fun_jac_ut_xt = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt); + } + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->cost_y_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); + } {%- endif %} - } - {% if solver_options.with_solution_sens_wrt_params %} - capsule->ext_cost_hess_xu_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(ext_cost_hess_xu_p[i], {{ model.name }}_cost_ext_cost_hess_xu_p); - } - {%- endif %} + {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + // convex-over-nonlinear cost + capsule->conl_cost_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(conl_cost_fun[i], {{ model.name }}_conl_cost_fun); + } + capsule->conl_cost_fun_jac_hess = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(conl_cost_fun_jac_hess[i], {{ model.name }}_conl_cost_fun_jac_hess); + } - {% if solver_options.with_value_sens_wrt_params %} - capsule->ext_cost_grad_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); - for (int i = 0; i < N-1; i++) - { - MAP_CASADI_FNC(ext_cost_grad_p[i], {{ model.name }}_cost_ext_cost_grad_p); - } + {%- elif cost.cost_type == "EXTERNAL" %} + // external cost + capsule->ext_cost_fun = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun); + {%- else %} + capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], &ext_fun_opts); + {%- endif %} + } + + capsule->ext_cost_fun_jac = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac); + {%- else %} + capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], &ext_fun_opts); + {%- endif %} + } + + capsule->ext_cost_fun_jac_hess = (external_function_external_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_external_param_{{ cost.cost_ext_fun_type }})*(N-1)); + for (int i = 0; i < N-1; i++) + { + {%- if cost.cost_ext_fun_type == "casadi" %} + MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess); + {%- else %} + capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }}; + external_function_external_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], &ext_fun_opts); + {%- endif %} + } + + {% if solver_options.with_solution_sens_wrt_params %} + capsule->ext_cost_hess_xu_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(ext_cost_hess_xu_p[i], {{ model.name }}_cost_ext_cost_hess_xu_p); + } + {%- endif %} + + {% if solver_options.with_value_sens_wrt_params %} + capsule->ext_cost_grad_p = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*(N-1)); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(ext_cost_grad_p[i], {{ model.name }}_cost_ext_cost_grad_p); + } + {%- endif %} {%- endif %} -{%- endif %} + } // N > 0 {%- endif %}{# solver_options.N_horizon > 0 #} From 1659d464eb7d0636fdfa8b4c1478405fc4c97588 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20=C3=96rjehag?= Date: Mon, 11 Aug 2025 17:08:46 +0200 Subject: [PATCH 118/164] Fix build warnings for ocp (#1604) Hi, I'm no expert on Makefiles or the acados project, but tried to fix some build warnings to the best of my abilities. Feel free to merge if you want, or take inspiration from this PR if you want to fix the issues some other way and close this PR :) Build output before fix: ``` make: *** No rule to make target 'clean_all'. Stop. ``` Build output after fix: ``` rm -f libacados_ocp_solver_drever.so rm -f acados_solver_drever.o rm -f acados_ocp_solver_pyx.so rm -f acados_ocp_solver_pyx.o ``` Numpy warnings before fix (that are no longer showing up after the fix): ``` cc -c -O2 \ -fPIC \ -o acados_ocp_solver_pyx.o \ -I /opt/acados/include/blasfeo/include/ \ -I /opt/acados/include/hpipm/include/ \ -I /opt/acados/include \ -I /usr/lib/python3/dist-packages/numpy/core/include \ -I /usr/include/python3.10 \ acados_ocp_solver_pyx.c \ In file included from /usr/lib/python3/dist-packages/numpy/core/include/numpy/ndarraytypes.h:1969, from /usr/lib/python3/dist-packages/numpy/core/include/numpy/ndarrayobject.h:12, from /usr/lib/python3/dist-packages/numpy/core/include/numpy/arrayobject.h:4, from acados_ocp_solver_pyx.c:1118: /usr/lib/python3/dist-packages/numpy/core/include/numpy/npy_1_7_deprecated_api.h:17:2: warning: #warning "Using deprecated NumPy API, disable it with " "#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION" [-Wcpp] 17 | #warning "Using deprecated NumPy API, disable it with " \ | ^~~~~~~ cc -c -O2 \ -fPIC \ -o acados_sim_solver_pyx.o \ -I /opt/acados/include/blasfeo/include/ \ -I /opt/acados/include/hpipm/include/ \ -I /opt/acados/include \ -I /usr/lib/python3/dist-packages/numpy/core/include \ -I /usr/include/python3.10 \ acados_sim_solver_pyx.c \ In file included from /usr/lib/python3/dist-packages/numpy/core/include/numpy/ndarraytypes.h:1969, from /usr/lib/python3/dist-packages/numpy/core/include/numpy/ndarrayobject.h:12, from /usr/lib/python3/dist-packages/numpy/core/include/numpy/arrayobject.h:4, from acados_sim_solver_pyx.c:1118: /usr/lib/python3/dist-packages/numpy/core/include/numpy/npy_1_7_deprecated_api.h:17:2: warning: #warning "Using deprecated NumPy API, disable it with " "#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION" [-Wcpp] 17 | #warning "Using deprecated NumPy API, disable it with " \ ``` --- interfaces/acados_template/acados_template/acados_ocp_solver.py | 2 +- .../acados_template/c_templates_tera/Makefile.in | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 4343fbee78..b0dbbffc19 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -161,7 +161,7 @@ def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = make_cmd = 'make' if with_cython: - verbose_system_call([make_cmd, 'clean_all'], verbose) + verbose_system_call([make_cmd, 'clean_ocp_cython'], verbose) verbose_system_call([make_cmd, 'ocp_cython'], verbose) else: if cmake_builder is not None: diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index 041326c02f..1b03e3dba0 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -260,6 +260,7 @@ ocp_cython_c: ocp_shared_lib ocp_cython_o: ocp_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ -o acados_ocp_solver_pyx.o \ -I $(INCLUDE_PATH)/blasfeo/include/ \ -I $(INCLUDE_PATH)/hpipm/include/ \ @@ -291,6 +292,7 @@ sim_cython_c: sim_shared_lib sim_cython_o: sim_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ -o acados_sim_solver_pyx.o \ -I $(INCLUDE_PATH)/blasfeo/include/ \ -I $(INCLUDE_PATH)/hpipm/include/ \ From a092d05a1b06803aee0291288bde5002d2c5d715 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 11 Aug 2025 23:37:21 +0200 Subject: [PATCH 119/164] CodeQL update to v3 (#1607) --- .github/workflows/codeql.yml | 16 ++++++++-------- .github/workflows/fail_on_error.py | 7 +++++-- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml index b29081b215..8672122ab1 100644 --- a/.github/workflows/codeql.yml +++ b/.github/workflows/codeql.yml @@ -12,12 +12,12 @@ name: "CodeQL" on: - # push: - # branches: [ "main", "master" ] - # schedule: - # - cron: '0 0 * * *' pull_request: branches: '*' + push: + branches-ignore: + - 'doc*' + - 'wip*' jobs: analyze: @@ -51,7 +51,7 @@ jobs: # Initializes the CodeQL tools for scanning. - name: Initialize CodeQL - uses: github/codeql-action/init@v2 + uses: github/codeql-action/init@v3 with: languages: ${{ matrix.language }} # If you wish to specify custom queries, you can do so here or in a config file. @@ -66,7 +66,7 @@ jobs: # Autobuild attempts to build any compiled languages (C/C++, C#, Go, Java, or Swift). # If this step fails, then you should remove it and run the build manually (see below) #- name: Autobuild - # uses: github/codeql-action/autobuild@v2 + # uses: github/codeql-action/autobuild@v3 # ℹ️ Command-line programs to run using the OS shell. # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun @@ -78,7 +78,7 @@ jobs: ./.github/workflows/codeql-buildscript.sh - name: Perform CodeQL Analysis - uses: github/codeql-action/analyze@v2 + uses: github/codeql-action/analyze@v3 with: category: "/language:${{matrix.language}}" upload: false @@ -108,7 +108,7 @@ jobs: output: ${{ steps.step1.outputs.sarif-output }}/cpp.sarif - name: Upload CodeQL results to code scanning - uses: github/codeql-action/upload-sarif@v2 + uses: github/codeql-action/upload-sarif@v3 with: sarif_file: ${{ steps.step1.outputs.sarif-output }} category: "/language:${{matrix.language}}" diff --git a/.github/workflows/fail_on_error.py b/.github/workflows/fail_on_error.py index 29791742b2..3cdbddc4c6 100755 --- a/.github/workflows/fail_on_error.py +++ b/.github/workflows/fail_on_error.py @@ -11,8 +11,11 @@ def codeql_sarif_contain_error(filename): for run in s.get('runs', []): rules_metadata = run['tool']['driver']['rules'] if not rules_metadata: - rules_metadata = run['tool']['extensions'][0]['rules'] - + extensions = run['tool'].get('extensions', []) + if extensions and 'rules' in extensions[0]: + rules_metadata = extensions[0]['rules'] + else: + rules_metadata = [] for res in run.get('results', []): if 'ruleIndex' in res: rule_index = res['ruleIndex'] From 3fb63b106ad06bf9b7ee12835fac7ad0c15aef62 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Tue, 12 Aug 2025 15:50:12 +0200 Subject: [PATCH 120/164] `AcadosCasadiOcp`: add support for single shooting formulation (#1605) - Modify `AcadosCasadiOcp` for Single shooting support - Add a test for validating the single shooting formulation - Add support for `get()` and `set()` in single shooting formulation --- .../test_casadi_single_shooting.py | 98 ++++++ interfaces/CMakeLists.txt | 20 +- .../acados_casadi_ocp_solver.py | 299 +++++++++++------- 3 files changed, 294 insertions(+), 123 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_single_shooting.py diff --git a/examples/acados_python/casadi_tests/test_casadi_single_shooting.py b/examples/acados_python/casadi_tests/test_casadi_single_shooting.py new file mode 100644 index 0000000000..0f7d7d736b --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_single_shooting.py @@ -0,0 +1,98 @@ +import sys +sys.path.insert(0, '../getting_started') +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model +from utils import plot_pendulum + +PLOT = False + +def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + N_horizon = 5 + Tf = 1.0 + ocp = formulate_ocp(Tf, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + if status != 0: + raise Exception(f'acados returned status {status}.') + result = ocp_solver.store_iterate_to_obj() + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp, solver="alpaqa", use_single_shooting=True, verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + + # evaluate difference + acados_x = np.array([ocp_solver.get(i, "x") for i in range(N_horizon + 1)]) + casadi_x = np.array([casadi_ocp_solver.get(i, "x") for i in range(N_horizon + 1)]) + acados_u = np.array([ocp_solver.get(i, "u") for i in range(N_horizon)]) + casadi_u = np.array([casadi_ocp_solver.get(i, "u") for i in range(N_horizon)]) + + diff_u = np.linalg.norm(acados_u - casadi_u) + diff_x = np.linalg.norm(acados_x - casadi_x) + print(f"Difference in states: {diff_x}") + print(f"Difference in control inputs: {diff_u}") + if diff_u > 1e-5 or diff_x > 1e-5: + raise Exception("result differ significantly between acados and casadi.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 043cb6f4e2..855b13966e 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -367,18 +367,21 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_casadi_get_set_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_get_set.py) - add_test(NAME test_casadi_parametric + add_test(NAME python_test_casadi_parametric COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_parametric.py) - add_test(NAME test_casadi_p_in_constraint_and_cost + add_test(NAME python_test_casadi_p_in_constraint_and_cost COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_p_in_constraint_and_cost.py) - add_test(NAME test_casadi_constraint + add_test(NAME python_test_casadi_constraint COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_constraint.py) - add_test(NAME test_casadi_slack_in_h + add_test(NAME python_test_casadi_slack_in_h COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_slack_in_h.py) + add_test(NAME python_test_casadi_single_shooting + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_single_shooting.py) # Sim add_test(NAME python_pendulum_ext_sim_example @@ -459,10 +462,11 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_fast_zoro_example PROPERTIES DEPENDS python_zoro_diff_drive_example) # casadi_tests - set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS test_casadi_p_in_constraint_and_cost) - set_tests_properties(test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS test_casadi_parametric) - set_tests_properties(test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) - set_tests_properties(test_casadi_slack_in_h PROPERTIES DEPENDS test_casadi_constraint) + set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS python_test_casadi_p_in_constraint_and_cost) + set_tests_properties(python_test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS python_test_casadi_parametric) + set_tests_properties(python_test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) + set_tests_properties(python_test_casadi_slack_in_h PROPERTIES DEPENDS python_test_casadi_constraint) + set_tests_properties(python_test_casadi_single_shooting PROPERTIES DEPENDS python_test_casadi_slack_in_h) # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 52a462dbd7..675c31b971 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -41,7 +41,7 @@ class AcadosCasadiOcp: - def __init__(self, ocp: AcadosOcp, with_hessian=False): + def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): """ Creates an equivalent CasADi NLP formulation of the OCP. Experimental, not fully implemented yet. @@ -96,31 +96,46 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # create primal variables and slack variables ca_symbol = model.get_casadi_symbol() - xtraj_node = [] - utraj_node = [] - sl_node = [] - su_node = [] - for i in range(N_horizon+1): - self._append_node(ca_symbol, xtraj_node, utraj_node, sl_node, su_node, i, dims) + xtraj_nodes = [] + utraj_nodes = [] + sl_nodes = [] + su_nodes = [] + if multiple_shooting: + for i in range(N_horizon+1): + self._append_node('x', ca_symbol, xtraj_nodes, i, dims) + self._append_node('u', ca_symbol, utraj_nodes, i, dims) + self._append_node('sl', ca_symbol, sl_nodes, i, dims) + self._append_node('su', ca_symbol, su_nodes, i, dims) + else: # single_shooting + self._x_traj_fun = [] + for i in range(N_horizon): + self._append_node('u', ca_symbol, utraj_nodes, i, dims) + self._append_node('sl', ca_symbol, sl_nodes, i, dims) + self._append_node('su', ca_symbol, su_nodes, i, dims) # parameters - ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon+1)] + ptraj_nodes = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon+1)] # setup state and control bounds - lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + lb_xtraj_nodes = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] + ub_xtraj_nodes = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] + lb_utraj_nodes = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + ub_utraj_nodes = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] # setup slack variables # TODO: speicify different bounds for lsbu, lsbx, lsg, lsh ,lsphi - lb_slack_node = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ + lb_slack_nodes = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ ([0* ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ ([0 * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) - ub_slack_node = ([np.inf * ca.DM.ones((dims.ns, 1))] if dims.ns_0 else []) + \ + ub_slack_nodes = ([np.inf * ca.DM.ones((dims.ns, 1))] if dims.ns_0 else []) + \ ([np.inf * ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ ([np.inf * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) - for i in range(0, N_horizon+1): - self._set_bounds_indices(i, lb_xtraj_node, ub_xtraj_node, lb_utraj_node, ub_utraj_node, constraints, dims) + if multiple_shooting: + for i in range(0, N_horizon+1): + self._set_bounds_indices('x', i, lb_xtraj_nodes, ub_xtraj_nodes, constraints, dims) + self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) + else: # single_shooting + for i in range(0, N_horizon): + self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) ### Concatenate primal variables and bounds # w = [x0, u0, sl0, su0, x1, u1, ...] @@ -131,32 +146,28 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): p_list = [] offset_p = 0 x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) - for i in range(N_horizon+1): - if i < N_horizon: - # add x - self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_node, lb_xtraj_node, ub_xtraj_node, i, dims, x_guess) - # add u - self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_node, lb_utraj_node, ub_utraj_node, i, dims, x_guess) - # add slack variables - self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_node, su_node], lb_slack_node, ub_slack_node, i, dims, x_guess) - # add parameters + if multiple_shooting: + for i in range(N_horizon+1): + self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_nodes, lb_xtraj_nodes, ub_xtraj_nodes, i, dims, x_guess) + if i < N_horizon: + self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) p_list.append(ocp.parameter_values) self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) offset_p += dims.np - else: - ## terminal stage - # add x - self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_node, lb_xtraj_node, ub_xtraj_node, i, dims, x_guess) - # add slack variables - self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_node, su_node], lb_slack_node, ub_slack_node, i, dims, x_guess) - # add parameters + else: # single_shooting + xtraj_nodes.append(x_guess) + self._x_traj_fun.append(x_guess) + for i in range(N_horizon+1): + if i < N_horizon: + self._append_variables_and_bounds('u', w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) p_list.append(ocp.parameter_values) self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) offset_p += dims.np - # add global parameters - p_list.append(ocp.p_global_values) - self._index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) - offset_p += dims.np_global + p_list.append(ocp.p_global_values) + self._index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) + offset_p += dims.np_global nw = self.offset_w # number of primal variables @@ -164,7 +175,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): w = ca.vertcat(*w_sym_list) lbw = ca.vertcat(*lbw_list) ubw = ca.vertcat(*ubw_list) - p_nlp = ca.vertcat(*ptraj_node, model.p_global) + p_nlp = ca.vertcat(*ptraj_nodes, model.p_global) ### Create Constraints g = [] @@ -177,37 +188,47 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): if solver_options.integrator_type == "DISCRETE": f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) elif solver_options.integrator_type == "ERK": - para = ca.vertcat(model.u, model.p, model.p_global) - ca_expl_ode = ca.Function('ca_expl_ode', [model.x, para], [model.f_expl_expr]) + param = ca.vertcat(model.u, model.p, model.p_global) + ca_expl_ode = ca.Function('ca_expl_ode', [model.x, param], [model.f_expl_expr]) f_discr_fun = ca.simpleRK(ca_expl_ode, solver_options.sim_method_num_steps[0], solver_options.sim_method_num_stages[0]) else: raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") for i in range(N_horizon+1): # add dynamics constraints - if i < N_horizon: - if solver_options.integrator_type == "DISCRETE": - dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) - elif solver_options.integrator_type == "ERK": - para = ca.vertcat(utraj_node[i], ptraj_node[i], model.p_global) - dyn_equality = xtraj_node[i+1] - f_discr_fun(xtraj_node[i], para, solver_options.time_steps[i]) - self._append_constraints(i, 'dyn', g, lbg, ubg, - g_expr = dyn_equality, - lbg_expr = np.zeros((dims.nx, 1)), - ubg_expr = np.zeros((dims.nx, 1)), - cons_dim=dims.nx) - if with_hessian: - # add hessian of dynamics constraints - lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) - lam_g.append(lam_g_dyn) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_dyn: - adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - + if multiple_shooting: + if i < N_horizon: + if solver_options.integrator_type == "DISCRETE": + dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i], model.p_global) + elif solver_options.integrator_type == "ERK": + param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i], model.p_global) + dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], param, solver_options.time_steps[i]) + self._append_constraints(i, 'dyn', g, lbg, ubg, + g_expr = dyn_equality, + lbg_expr = np.zeros((dims.nx, 1)), + ubg_expr = np.zeros((dims.nx, 1)), + cons_dim=dims.nx) + if with_hessian: + # add hessian of dynamics constraints + lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) + lam_g.append(lam_g_dyn) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_dyn: + adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + else: # single_shooting + if i < N_horizon: + x_current = xtraj_nodes[i] + if solver_options.integrator_type == "DISCRETE": + x_next = f_discr_fun(x_current, utraj_nodes[i], ptraj_nodes[i], model.p_global) + elif solver_options.integrator_type == "ERK": + param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i], model.p_global) + x_next = f_discr_fun(x_current, param, solver_options.time_steps[i]) + xtraj_nodes.append(x_next) + self._x_traj_fun.append(f_discr_fun) # Nonlinear Constraints # initial stage lg, ug, lh, uh, lphi, uphi, ng, nh, nphi, nsg, nsh, nsphi, idxsh, linear_constr_expr, h_i_nlp_expr, conl_constr_fun =\ - self._get_constraint_node(i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims) + self._get_constraint_node(i, N_horizon, xtraj_nodes, utraj_nodes, ptraj_nodes, model, constraints, dims) # add linear constraints if ng > 0: @@ -227,13 +248,13 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): if index_in_nh in soft_h_indices: index_in_soft = soft_h_indices.tolist().index(index_in_nh) self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr[index_in_nh] + sl_node[i][index_in_soft], + g_expr = h_i_nlp_expr[index_in_nh] + sl_nodes[i][index_in_soft], lbg_expr = lh[index_in_nh], ubg_expr = np.inf * ca.DM.ones((1, 1)), cons_dim=1, sl=True) self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr[index_in_nh] - su_node[i][index_in_soft], + g_expr = h_i_nlp_expr[index_in_nh] - su_nodes[i][index_in_soft], lbg_expr = -np.inf * ca.DM.ones((1, 1)), ubg_expr = uh[index_in_nh], cons_dim=1, @@ -258,10 +279,10 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - # add compound nonlinear constraints + # add convex-over-nonlinear constraints if nphi > 0: self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = conl_constr_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global), + g_expr = conl_constr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i], model.p_global), lbg_expr = lphi, ubg_expr = uphi, cons_dim=nphi) @@ -271,7 +292,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # always use CONL Hessian approximation here, disregarding inner second derivative outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi, model.con_r_expr) - r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_node[-1]) + r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_nodes[-1]) dr_dw = ca.jacobian(r_in_nlp, w) hess_l += dr_dw.T @ outer_hess_r @ dr_dw @@ -279,7 +300,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): nlp_cost = 0 for i in range(N_horizon+1): xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, ns, zl, Zl, zu, Zu = \ - self._get_cost_node(i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_node, su_node, ocp, dims, cost) + self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, ptraj_nodes, sl_nodes, su_nodes, ocp, dims, cost) cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, model.p, model.p_global], [cost_expr_i]) nlp_cost += solver_options.cost_scaling[i] * cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) @@ -318,7 +339,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): self.__nlp_hess_l_custom = nlp_hess_l_custom self.__hess_approx_expr = hess_l - def _append_node(self, ca_symbol, xtraj_node:list, utraj_node:list, sl_node:list, su_node:list, i, dims): + def _append_node(self, _field, ca_symbol, node:list, i, dims): """ Helper function to append a node to the NLP formulation. """ @@ -328,40 +349,60 @@ def _append_node(self, ca_symbol, xtraj_node:list, utraj_node:list, sl_node:list ns = dims.ns else: ns = dims.ns_e - xtraj_node.append(ca_symbol(f'x{i}', dims.nx, 1)) - utraj_node.append(ca_symbol(f'u{i}', dims.nu, 1)) - if ns > 0: - sl_node.append(ca_symbol(f'sl_0', ns, 1)) - su_node.append(ca_symbol(f'su_0', ns, 1)) - else: - sl_node.append([]) - su_node.append([]) + if _field == 'x': + node.append(ca_symbol(f'x{i}', dims.nx, 1)) + elif _field == 'u': + node.append(ca_symbol(f'u{i}', dims.nu, 1)) + elif _field == 'sl': + if ns > 0: + node.append(ca_symbol(f'sl{i}', ns, 1)) + else: + node.append([]) + elif _field == 'su': + if ns > 0: + node.append(ca_symbol(f'su{i}', ns, 1)) + else: + node.append([]) - def _set_bounds_indices(self, i, lb_xtraj_node, ub_xtraj_node, lb_utraj_node, ub_utraj_node, constraints, dims): + def _set_bounds_indices(self, _field, i, lb_node, ub_node, constraints, dims): """ Helper function to set bounds and indices for the primal variables. """ if i == 0: - lb_xtraj_node[i][constraints.idxbx_0] = constraints.lbx_0 - ub_xtraj_node[i][constraints.idxbx_0] = constraints.ubx_0 - self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx_0)) - self.offset_lam += dims.nx + idxbx = constraints.idxbx_0 + idxbu = constraints.idxbu + lbx = constraints.lbx_0 + ubx = constraints.ubx_0 + lbu = constraints.lbu + ubu = constraints.ubu elif i < dims.N: - lb_xtraj_node[i][constraints.idxbx] = constraints.lbx - ub_xtraj_node[i][constraints.idxbx] = constraints.ubx - self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx)) - self.offset_lam += dims.nx + idxbx = constraints.idxbx + idxbu = constraints.idxbu + lbx = constraints.lbx + ubx = constraints.ubx + lbu = constraints.lbu + ubu = constraints.ubu elif i == dims.N: - lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e - ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e - self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + constraints.idxbx_e)) - self.offset_lam += dims.nx + idxbx = constraints.idxbx_e + lbx = constraints.lbx_e + ubx = constraints.ubx_e if i < dims.N: - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu - self._index_map['lam_bu_in_lam_w'].append(list(self.offset_lam + constraints.idxbu)) - self.offset_lam += dims.nu - self.offset_lam += 2*dims.ns_0 if i == 0 else 2*dims.ns + if _field == 'x': + lb_node[i][idxbx] = lbx + ub_node[i][idxbx] = ubx + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) + self.offset_lam += dims.nx + elif _field == 'u': + lb_node[i][idxbu] = lbu + ub_node[i][idxbu] = ubu + self._index_map['lam_bu_in_lam_w'].append(list(self.offset_lam + idxbu)) + self.offset_lam += dims.nu + elif i == dims.N: + if _field == 'x': + lb_node[i][idxbx] = lbx + ub_node[i][idxbx] = ubx + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) + self.offset_lam += dims.nx def _append_variables_and_bounds(self, _field, w_sym_list, lbw_list, ubw_list, w0_list, node_list, lb_node_list, ub_node_list, i, dims, x_guess): @@ -605,15 +646,19 @@ class AcadosCasadiOcpSolver: def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, casadi_solver_opts: Optional[dict] = None, - use_acados_hessian: bool = False): + use_acados_hessian: bool = False, + use_single_shooting: bool = False): if not isinstance(ocp, AcadosOcp): raise TypeError('ocp should be of type AcadosOcp.') self.ocp = ocp - + self.multiple_shooting = not use_single_shooting # create casadi NLP formulation - casadi_nlp_obj = AcadosCasadiOcp(ocp, with_hessian=use_acados_hessian) + casadi_nlp_obj = AcadosCasadiOcp(ocp = ocp, + with_hessian = use_acados_hessian, + multiple_shooting= self.multiple_shooting + ) self.acados_casadi_ocp = casadi_nlp_obj @@ -623,6 +668,8 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, self.p = casadi_nlp_obj.p_nlp_values self.index_map = casadi_nlp_obj.index_map self.nlp_hess_l_custom = casadi_nlp_obj.nlp_hess_l_custom + if use_single_shooting: + self.x_traj_fun = casadi_nlp_obj._x_traj_fun # create NLP solver if casadi_solver_opts is None: @@ -639,8 +686,8 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, self.casadi_solver = ca.nlpsol("nlp_solver", solver, self.casadi_nlp, casadi_solver_opts) # create solution and initial guess - self.lam_x0 = np.empty(self.casadi_nlp['x'].shape).flatten() - self.lam_g0 = np.empty(self.casadi_nlp['g'].shape).flatten() + self.lam_x0 = np.zeros(self.casadi_nlp['x'].shape).flatten() + self.lam_g0 = np.zeros(self.casadi_nlp['g'].shape).flatten() self.nlp_sol = None @@ -662,13 +709,23 @@ def solve(self) -> int: self.nlp_sol_w = self.nlp_sol['x'].full() self.nlp_sol_lam_g = self.nlp_sol['lam_g'].full() self.nlp_sol_lam_x = self.nlp_sol['lam_x'].full() + if not self.multiple_shooting: + self.nlp_sol_x = [self.x_traj_fun[0]] + for i in range(0, self.ocp.dims.N): + x_current = self.nlp_sol_x[i] + if self.ocp.solver_options.integrator_type == "DISCRETE": + x_next = self.x_traj_fun[i+1](x_current, self.nlp_sol_w[i], self.ocp.parameter_values, self.ocp.p_global_values) + elif self.ocp.solver_options.integrator_type == "ERK": + param = np.concatenate([self.nlp_sol_w[i], self.ocp.parameter_values, self.ocp.p_global_values]) + x_next = self.x_traj_fun[i+1](x_current, param, self.ocp.solver_options.time_steps[i]) + self.nlp_sol_x.append(x_next.full()) # statistics solver_stats = self.casadi_solver.stats() - # timing = solver_stats['t_proc_total'] - self.status = solver_stats['return_status'] - self.nlp_iter = solver_stats['iter_count'] - self.time_total = solver_stats['t_wall_total'] + # timing = solver_stats['t_proc_total'] + self.status = solver_stats['return_status'] if 'return_status' in solver_stats else solver_stats['success'] + self.nlp_iter = solver_stats['iter_count'] if 'iter_count' in solver_stats else None + self.time_total = solver_stats['t_wall_total'] if 't_wall_total' in solver_stats else None self.solver_stats = solver_stats # nlp_res = ca.norm_inf(sol['g']).full()[0][0] # cost_val = ca.norm_inf(sol['f']).full()[0][0] @@ -701,12 +758,16 @@ def get(self, stage: int, field: str): if self.nlp_sol is None: raise ValueError('No solution available. Please call solve() first.') dims = self.ocp.dims - if field == 'x': + if field == 'x' and self.multiple_shooting: return self.nlp_sol_w[self.index_map['x_in_w'][stage]].flatten() + elif field == 'x' and not self.multiple_shooting: + return self.nlp_sol_x[stage].flatten() elif field == 'u': return self.nlp_sol_w[self.index_map['u_in_w'][stage]].flatten() - elif field == 'pi': + elif field == 'pi' and self.multiple_shooting: return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() + elif field == 'pi' and not self.multiple_shooting: + return [] elif field == 'p': return self.p[self.index_map['p_in_p_nlp'][stage]].flatten() elif field == 'sl': @@ -715,20 +776,20 @@ def get(self, stage: int, field: str): return self.nlp_sol_w[self.index_map['su_in_w'][stage]].flatten() elif field == 'lam': if stage == 0: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage < dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage == dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] bu_lam = np.empty((0, 1)) g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] - lbx_lam = np.maximum(0, -bx_lam) - ubx_lam = np.maximum(0, bx_lam) + lbx_lam = np.maximum(0, -bx_lam) if self.multiple_shooting else np.empty((0, 1)) + ubx_lam = np.maximum(0, bx_lam) if self.multiple_shooting else np.empty((0, 1)) lbu_lam = np.maximum(0, -bu_lam) ubu_lam = np.maximum(0, bu_lam) if any([dims.ns_0, dims.ns, dims.ns_e]): @@ -899,12 +960,16 @@ def set(self, stage: int, field: str, value_: np.ndarray): """ dims = self.ocp.dims - if field == 'x': + if field == 'x' and self.multiple_shooting: self.w0[self.index_map['x_in_w'][stage]] = value_.flatten() + elif field == 'x' and not self.multiple_shooting: + pass elif field == 'u': self.w0[self.index_map['u_in_w'][stage]] = value_.flatten() - elif field == 'pi': + elif field == 'pi' and self.multiple_shooting: self.lam_g0[self.index_map['pi_in_lam_g'][stage]] = -value_.flatten() + elif field == 'pi' and not self.multiple_shooting: + pass elif field == 'p': self.p[self.index_map['p_in_p_nlp'][stage]] = value_.flatten() elif field == 'sl': @@ -913,17 +978,17 @@ def set(self, stage: int, field: str, value_: np.ndarray): self.w0[self.index_map['su_in_w'][stage]] = value_.flatten() elif field == 'lam': if stage == 0: - nbx = dims.nbx_0 + nbx = dims.nbx_0 if self.multiple_shooting else 0 nbu = dims.nbu n_ghphi = dims.ng + dims.nh_0 + dims.nphi_0 ns = dims.ns_0 elif stage < dims.N: - nbx = dims.nbx + nbx = dims.nbx if self.multiple_shooting else 0 nbu = dims.nbu n_ghphi = dims.ng + dims.nh + dims.nphi ns = dims.ns elif stage == dims.N: - nbx = dims.nbx_e + nbx = dims.nbx_e if self.multiple_shooting else 0 nbu = 0 n_ghphi = dims.ng_e + dims.nh_e + dims.nphi_e ns = dims.ns_e @@ -949,13 +1014,17 @@ def set(self, stage: int, field: str, value_: np.ndarray): ug_lam_soft = ug_lam[sl_indices] if stage != dims.N: - self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + if self.multiple_shooting: + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + else: + self.lam_x0[self.index_map['lam_bu_in_lam_w'][stage]] = ubu_lam-lbu_lam self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam_hard-lg_lam_hard self.lam_g0[self.index_map['lam_sl_in_lam_g'][stage]] = -lg_lam_soft self.lam_g0[self.index_map['lam_su_in_lam_g'][stage]] = ug_lam_soft self.lam_x0[self.index_map['sl_in_w'][stage]+self.index_map['su_in_w'][stage]] = -soft_lam else: - self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]] = ubx_lam-lbx_lam + if self.multiple_shooting: + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]] = ubx_lam-lbx_lam self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam_hard-lg_lam_hard self.lam_g0[self.index_map['lam_sl_in_lam_g'][stage]] = -lg_lam_soft self.lam_g0[self.index_map['lam_su_in_lam_g'][stage]] = ug_lam_soft From 8410d694a408fbc9f2bc942d1171e841406cf3f2 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Wed, 13 Aug 2025 17:36:59 +0200 Subject: [PATCH 121/164] `AcadosCasadiOcpSolver`: add support for checking LICQ (#1597) - add function for checking strict complementarity in `AcadosCasadiOcpSolver`, not tested yet - add function for checking LICQ in `AcadosCasadiOcpSolver` - add test for validating check LICQ --- .../test_casadi_LICQ_violation.py | 98 ++++++++ interfaces/CMakeLists.txt | 4 + .../acados_casadi_ocp_solver.py | 229 +++++++++++++++++- 3 files changed, 322 insertions(+), 9 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_LICQ_violation.py diff --git a/examples/acados_python/casadi_tests/test_casadi_LICQ_violation.py b/examples/acados_python/casadi_tests/test_casadi_LICQ_violation.py new file mode 100644 index 0000000000..ac6c278b33 --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_LICQ_violation.py @@ -0,0 +1,98 @@ +import sys +sys.path.insert(0, '../getting_started') +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model + +def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + # set initial bounds and state + ocp.constraints.lbx_0 = np.array([0, np.pi, -0.2, 0]) + ocp.constraints.ubx_0 = np.array([0, np.pi, 0.2, 0]) + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set linear constraints + ocp.constraints.C = np.array([[0, 0, 0, 0]]) + ocp.constraints.D = np.array([[0.1]]) + ocp.constraints.lg = np.array([-8]) + ocp.constraints.ug = np.array([8]) + + # set x_1 at the end of the horizon + ocp.constraints.C_e = np.array([[1, 0, 0, 0]]) + ocp.constraints.lg_e = np.array([0.3]) + ocp.constraints.ug_e = np.array([0.5]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + N_horizon = 3 + Tf = 1.0 + ocp = formulate_ocp(Tf, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + # get solution + result = ocp_solver.store_iterate_to_obj() + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp,solver="ipopt",verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + licq = casadi_ocp_solver.satisfies_LICQ() + + # Check for violation of specific stage, at least one stage should violate LICQ + if licq: + raise ValueError("LICQ condition is not violated in the solution.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 855b13966e..6f18beccb1 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -382,6 +382,9 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_test_casadi_single_shooting COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_single_shooting.py) + add_test(NAME python_test_casadi_LICQ_violation + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_LICQ_violation.py) # Sim add_test(NAME python_pendulum_ext_sim_example @@ -467,6 +470,7 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) set_tests_properties(python_test_casadi_slack_in_h PROPERTIES DEPENDS python_test_casadi_constraint) set_tests_properties(python_test_casadi_single_shooting PROPERTIES DEPENDS python_test_casadi_slack_in_h) + set_tests_properties(python_test_casadi_LICQ_violation PROPERTIES DEPENDS python_test_casadi_single_shooting) # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 675c31b971..978a49daf8 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -707,8 +707,9 @@ def solve(self) -> int: lbg=self.bounds['lbg'], ubg=self.bounds['ubg'] ) self.nlp_sol_w = self.nlp_sol['x'].full() + self.nlp_sol_g = self.nlp_sol['g'].full() self.nlp_sol_lam_g = self.nlp_sol['lam_g'].full() - self.nlp_sol_lam_x = self.nlp_sol['lam_x'].full() + self.nlp_sol_lam_w = self.nlp_sol['lam_x'].full() if not self.multiple_shooting: self.nlp_sol_x = [self.x_traj_fun[0]] for i in range(0, self.ocp.dims.N): @@ -776,15 +777,15 @@ def get(self, stage: int, field: str): return self.nlp_sol_w[self.index_map['su_in_w'][stage]].flatten() elif field == 'lam': if stage == 0: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] - bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_w[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] + bu_lam = self.nlp_sol_lam_w[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage < dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] - bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_w[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] + bu_lam = self.nlp_sol_lam_w[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage == dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] + bx_lam = self.nlp_sol_lam_w[self.index_map['lam_bx_in_lam_w'][stage]] if self.multiple_shooting else [] bu_lam = np.empty((0, 1)) g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] @@ -793,8 +794,8 @@ def get(self, stage: int, field: str): lbu_lam = np.maximum(0, -bu_lam) ubu_lam = np.maximum(0, bu_lam) if any([dims.ns_0, dims.ns, dims.ns_e]): - lw_soft_lam = self.nlp_sol_lam_x[self.index_map['sl_in_w'][stage]] - uw_soft_lam = self.nlp_sol_lam_x[self.index_map['su_in_w'][stage]] + lw_soft_lam = self.nlp_sol_lam_w[self.index_map['sl_in_w'][stage]] + uw_soft_lam = self.nlp_sol_lam_w[self.index_map['su_in_w'][stage]] lg_soft_lam = self.nlp_sol_lam_g[self.index_map['lam_sl_in_lam_g'][stage]] ug_soft_lam = self.nlp_sol_lam_g[self.index_map['lam_su_in_lam_g'][stage]] if self.index_map['lam_su_in_lam_g'][stage]: @@ -846,7 +847,7 @@ def get_flat(self, field_: str) -> np.ndarray: return self.p[self.index_map['p_global_in_p_nlp']].flatten() # casadi variables. TODO: maybe remove this. elif field_ == 'lam_x': - return self.nlp_sol_lam_x.flatten() + return self.nlp_sol_lam_w.flatten() elif field_ == 'lam_g': return self.nlp_sol_lam_g.flatten() elif field_ == 'lam_p': @@ -1037,3 +1038,213 @@ def cost_get(self, stage_: int, field_: str) -> np.ndarray: def cost_set(self, stage_: int, field_: str, value_): raise NotImplementedError() + + def get_constraints_value(self, stage: int): + """ + Get the constraints values and lambda for a given stage. + """ + if not isinstance(stage, int): + raise TypeError('stage should be integer.') + if self.nlp_sol is None: + raise ValueError('No solution available. Please call solve() first.') + + # create constraints value and lambda in the order as [bx, bu, bg, bh, bphi] + if stage < self.ocp.dims.N: + constraints_value = np.concatenate((self.nlp_sol_w[self.index_map['lam_bx_in_lam_w'][stage]], + self.nlp_sol_w[self.index_map['lam_bu_in_lam_w'][stage]], + self.nlp_sol_g[self.index_map['pi_in_lam_g'][stage]], + self.nlp_sol_g[self.index_map['lam_gnl_in_lam_g'][stage]])).flatten() + lambda_values = np.concatenate((self.nlp_sol_lam_w[self.index_map['lam_bx_in_lam_w'][stage]], + self.nlp_sol_lam_w[self.index_map['lam_bu_in_lam_w'][stage]], + self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]], + self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]])).flatten() + lb = ca.vertcat(self.bounds['lbx'][self.index_map['lam_bx_in_lam_w'][stage]], + self.bounds['lbx'][self.index_map['lam_bu_in_lam_w'][stage]], + self.bounds['lbg'][self.index_map['pi_in_lam_g'][stage]], + self.bounds['lbg'][self.index_map['lam_gnl_in_lam_g'][stage]]).full().flatten() + ub = ca.vertcat(self.bounds['ubx'][self.index_map['lam_bx_in_lam_w'][stage]], + self.bounds['ubx'][self.index_map['lam_bu_in_lam_w'][stage]], + self.bounds['ubg'][self.index_map['pi_in_lam_g'][stage]], + self.bounds['ubg'][self.index_map['lam_gnl_in_lam_g'][stage]]).full().flatten() + elif stage == self.ocp.dims.N: + constraints_value = np.concatenate((self.nlp_sol_w[self.index_map['lam_bx_in_lam_w'][stage]], + self.nlp_sol_g[self.index_map['lam_gnl_in_lam_g'][stage]])).flatten() + lambda_values = np.concatenate((self.nlp_sol_lam_w[self.index_map['lam_bx_in_lam_w'][stage]], + self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]])).flatten() + lb = ca.vertcat(self.bounds['lbx'][self.index_map['lam_bx_in_lam_w'][stage]], + self.bounds['lbg'][self.index_map['lam_gnl_in_lam_g'][stage]]).full().flatten() + ub = ca.vertcat(self.bounds['ubx'][self.index_map['lam_bx_in_lam_w'][stage]], + self.bounds['ubg'][self.index_map['lam_gnl_in_lam_g'][stage]]).full().flatten() + return constraints_value, lambda_values, lb, ub + + def get_constraints_indices(self, stage: int): + """ + Get the indices of the constraints for a given stage. + This function distinguishes between inequality and equality constraints + returns indices of + (inequality, equality for decision variables, equality for dynamic and gnl, lower active inequality, upper active inequality). + """ + constraints_value, _, lb, ub = self.get_constraints_value(stage) + tol = self.ocp.solver_options.nlp_solver_tol_ineq + # distinguish between equality and inequality constraints + if stage == 0: + nbx = self.ocp.dims.nbx_0 + nbu = self.ocp.dims.nbu + elif stage < self.ocp.dims.N: + nbx = self.ocp.dims.nbx + nbu = self.ocp.dims.nbu + elif stage == self.ocp.dims.N: + nbx = self.ocp.dims.nbx_e + nbu = 0 + + ineq_indices = [] + eq_indices_bounds = [] + eq_indices_ca_g = [] + + for i in range(len(lb)): + if lb[i] != ub[i]: + ineq_indices.append(i) + else: + #distinguish between equality in decision variables and in constraints + if i in range(nbx + nbu): + eq_indices_bounds.append(i) + else: + eq_indices_ca_g.append(i) + # get the inequality violations + violations_ineq_lb = constraints_value[ineq_indices] - lb[ineq_indices] + violations_ineq_ub = ub[ineq_indices] - constraints_value[ineq_indices] + # any negative value in violations means infeasible constraint, raise an error + if np.any(violations_ineq_lb < -tol) or np.any(violations_ineq_ub < -tol): + raise ValueError('Constraints are violated. Please check the solution.') + # get active inequality indices from inequality constraints + active_ineq_lb_indices = np.take(ineq_indices, np.where(violations_ineq_lb < tol)[0]) + active_ineq_ub_indices = np.take(ineq_indices, np.where(violations_ineq_ub < tol)[0]) + return ineq_indices, eq_indices_bounds, eq_indices_ca_g, active_ineq_lb_indices, active_ineq_ub_indices + + def satisfies_strict_complementarity_stage_wise(self, stage: int, tol: float) -> bool: + """ + Check if the solution satisfies strict complementarity conditions for a given stage. + This checks that the Lagrange multipliers for active inequality constraints are strictly positive. + Not tested yet. + """ + tol = self.ocp.solver_options.nlp_solver_tol_ineq + if self.nlp_sol is None: + raise ValueError('No solution available. Please call solve() first.') + + _, lambda_value, _, _ = self.get_constraints_value(stage) + _, _, _, active_ineq_lb_indices, active_ineq_ub_indices = self.get_constraints_indices(stage) + + for i in active_ineq_lb_indices: + lam = np.maximum(0, -lambda_value[i]) + if lam < tol: + return False + for i in active_ineq_ub_indices: + lam = np.maximum(0, lambda_value[i]) + if lam < tol: + return False + return True + + def satisfies_strict_complementarity_stages(self, tol: float) -> list[bool]: + """ + Check if the solution satisfies strict complementarity conditions for all stages. + Not tested yet. + """ + tol = self.ocp.solver_options.nlp_solver_tol_ineq + dims = self.ocp.dims + complementarity = [] + for stage in range(dims.N + 1): + complementarity.append(self.satisfies_strict_complementarity_stage_wise(stage, tol)) + return complementarity + + def satisfies_strict_complementarity(self) -> bool: + """ + Check if the solution satisfies strict complementarity conditions for all stages. + Not tested yet. + """ + stage_wise_complementarity = self.satisfies_strict_complementarity_stages(self.ocp.solver_options.nlp_solver_tol_ineq) + if all(stage_wise_complementarity): + return True + else: + return False + + def satisfies_LICQ_stage_wise(self, stage) -> bool: + """ + Check if the solution satisfies the Linear Independence Constraint Qualification (LICQ) for a given stage. + """ + if self.nlp_sol is None: + raise ValueError('No solution available. Please call solve() first.') + + _, eq_indices_bounds, eq_indices_ca_g, active_ineq_lb_indices, active_ineq_ub_indices = self.get_constraints_indices(stage) + + w, w_value, constraints_expr_stage, eq_indices = self._get_w_and_constraints_for_LICQ(stage, eq_indices_bounds, eq_indices_ca_g) + + eq_constraints = constraints_expr_stage[eq_indices] if len(eq_indices) != 0 else ca.vertcat() + active_ineq_lb_constraints = constraints_expr_stage[active_ineq_lb_indices] if len(active_ineq_lb_indices) != 0 else ca.vertcat() + active_ineq_ub_constraints = constraints_expr_stage[active_ineq_ub_indices] if len(active_ineq_ub_indices) != 0 else ca.vertcat() + constraint_matrix = ca.vertcat(eq_constraints, + -active_ineq_lb_constraints, + active_ineq_ub_constraints) + constraint_jac_expr = ca.Function('constraint_jac', [w], [ca.jacobian(constraint_matrix, w)]) + constraint_jac_value = constraint_jac_expr(w_value).full() + + # Check if the Jacobian of the constraints is full rank + rank = np.linalg.matrix_rank(constraint_jac_value) if constraint_jac_value.any() else 0 + if rank == constraint_jac_value.shape[0]: + return True + else: + return False + + def satisfies_LICQ_stages(self) -> list[bool]: + """ + Check if the solution satisfies the Linear Independence Constraint Qualification (LICQ) for all stages. + return a list of booleans, each indicating whether LICQ is satisfied for the corresponding stage. + """ + dims = self.ocp.dims + stage_wise_LICQ = [] + for stage in range(dims.N + 1): + stage_wise_LICQ.append(self.satisfies_LICQ_stage_wise(stage)) + return stage_wise_LICQ + + def satisfies_LICQ(self) -> bool: + """ + Check if the solution satisfies the Linear Independence Constraint Qualification (LICQ) for all stages. + return True if LICQ is satisfied for all stages, otherwise False. + """ + stage_wise_LICQ = self.satisfies_LICQ_stages() + if all(stage_wise_LICQ): + return True + else: + return False + + def _get_w_and_constraints_for_LICQ(self, stage: int, eq_indices_bounds, eq_indices_ca_g): + """ + Helper function to get the w vector and constraints expression for a given stage. + This is used to compute the Jacobian of the constraints for checking LICQ. + """ + if stage == 0: + w = ca.vertcat(self.casadi_nlp['x'][self.index_map['x_in_w'][stage]], + self.casadi_nlp['x'][self.index_map['u_in_w'][stage]]) + w_value = np.concatenate((self.nlp_sol_w[self.index_map['x_in_w'][stage]], + self.nlp_sol_w[self.index_map['u_in_w'][stage]])).flatten() + constraints_expr_stage = ca.vertcat(self.casadi_nlp['x'][self.index_map['lam_bx_in_lam_w'][stage]], + self.casadi_nlp['x'][self.index_map['lam_bu_in_lam_w'][stage]], + self.casadi_nlp['g'][self.index_map['pi_in_lam_g'][stage]], + self.casadi_nlp['g'][self.index_map['lam_gnl_in_lam_g'][stage]]) + eq_indices = eq_indices_ca_g + elif stage < self.ocp.dims.N: + w = ca.vertcat(self.casadi_nlp['x'][self.index_map['x_in_w'][stage]], + self.casadi_nlp['x'][self.index_map['u_in_w'][stage]]) + w_value = np.concatenate((self.nlp_sol_w[self.index_map['x_in_w'][stage]], + self.nlp_sol_w[self.index_map['u_in_w'][stage]])).flatten() + constraints_expr_stage = ca.vertcat(self.casadi_nlp['x'][self.index_map['lam_bx_in_lam_w'][stage]], + self.casadi_nlp['x'][self.index_map['lam_bu_in_lam_w'][stage]], + self.casadi_nlp['g'][self.index_map['pi_in_lam_g'][stage]], + self.casadi_nlp['g'][self.index_map['lam_gnl_in_lam_g'][stage]]) + eq_indices = eq_indices_bounds + eq_indices_ca_g + elif stage == self.ocp.dims.N: + w = self.casadi_nlp['x'][self.index_map['x_in_w'][stage]] + w_value = self.nlp_sol_w[self.index_map['x_in_w'][stage]].flatten() + constraints_expr_stage = ca.vertcat(self.casadi_nlp['x'][self.index_map['lam_bx_in_lam_w'][stage]], + self.casadi_nlp['g'][self.index_map['lam_gnl_in_lam_g'][stage]]) + eq_indices = eq_indices_bounds + eq_indices_ca_g + return w, w_value, constraints_expr_stage, eq_indices \ No newline at end of file From 4cd8553d3bc50d201acfc0cd509e9f477b9e96a1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 15 Aug 2025 14:02:24 +0200 Subject: [PATCH 122/164] Python 3.8 compatible typehint closes #1609 (#1610) --- .../acados_template/acados_casadi_ocp_solver.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 978a49daf8..f72240d635 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -31,7 +31,7 @@ import casadi as ca -from typing import Union, Optional +from typing import Union, Optional, List import numpy as np @@ -1144,7 +1144,7 @@ def satisfies_strict_complementarity_stage_wise(self, stage: int, tol: float) -> return False return True - def satisfies_strict_complementarity_stages(self, tol: float) -> list[bool]: + def satisfies_strict_complementarity_stages(self, tol: float) -> List[bool]: """ Check if the solution satisfies strict complementarity conditions for all stages. Not tested yet. @@ -1194,7 +1194,7 @@ def satisfies_LICQ_stage_wise(self, stage) -> bool: else: return False - def satisfies_LICQ_stages(self) -> list[bool]: + def satisfies_LICQ_stages(self) -> List[bool]: """ Check if the solution satisfies the Linear Independence Constraint Qualification (LICQ) for all stages. return a list of booleans, each indicating whether LICQ is satisfied for the corresponding stage. From 85a7334c31125601e9c80adae3f92b5d4dca184d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Sun, 17 Aug 2025 16:21:25 +0200 Subject: [PATCH 123/164] Add support for `idxs_rev` slack formulation in acados (#1608) ## A new way to formulate soft constraints Soft constraints can be formulated in two ways: 1) via `idxsbu, idxsbx, idxsg, idxsh, idxsphi, lsbu, usbu, lsbx, usbx, lsg, usg, lsh, ush, lsphi, usphi` 2) via `idxs_rev, ls, us` and `*_0, *_e` variants Option 1) is what was implemented in acados <= 0.5.1 Option 2) is the new way of formulating soft constraints, which is more flexible, as one slack variable can be used for multiple constraints. ## Documentation of new properties - `idxs_rev`: Indices of slack variables associated with each constraint at shooting nodes (1 to N-1), zero-based. - `ls`: Lower bounds on slacks associated with lower bound constraints at shooting nodes (1 to N-1). - `us`: Lower bounds on slacks associated with upper bound constraints at shooting nodes (1 to N-1). ## Limitations - mixing the two formulations for path and initial node is not supported - the `idxs_rev` formulation is only compatible with `HPIPM` QP solvers for now. And not compatible with `SQP_WITH_FEASIBLE_QP`. --- .github/workflows/full_build_windows.yml | 12 + acados/dense_qp/dense_qp_common.c | 56 +++- acados/dense_qp/dense_qp_common.h | 5 +- acados/dense_qp/dense_qp_daqp.c | 9 +- acados/dense_qp/dense_qp_daqp.h | 1 + acados/dense_qp/dense_qp_qpoases.c | 103 +++--- acados/dense_qp/dense_qp_qpoases.h | 1 + acados/ocp_nlp/ocp_nlp_common.c | 80 +++-- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 163 +++++++++- acados/ocp_nlp/ocp_nlp_constraints_bgh.h | 6 +- acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 177 +++++++++-- acados/ocp_nlp/ocp_nlp_constraints_bgp.h | 8 +- acados/ocp_nlp/ocp_nlp_constraints_common.h | 1 + acados/ocp_qp/ocp_qp_common.c | 6 +- acados/ocp_qp/ocp_qp_common.h | 9 +- acados/utils/print.c | 6 +- .../main_slack_min_formulations.m | 42 +++ .../slack_min_formulation.m | 192 ++++++++++++ .../casadi_tests/test_casadi_slack_in_h.py | 6 +- .../ocp/slack_min_formulation.py | 223 +++++++++++++ examples/c/dense_qp.c | 4 +- external/hpipm | 2 +- interfaces/CMakeLists.txt | 7 +- interfaces/acados_c/ocp_qp_interface.c | 7 - interfaces/acados_matlab_octave/AcadosOcp.m | 193 +++++++++++- .../AcadosOcpConstraints.m | 25 ++ .../acados_matlab_octave/ns_from_idxs_rev.m | 7 + .../acados_template/acados_ocp.py | 156 ++++++++- .../acados_template/acados_ocp_constraints.py | 136 +++++++- .../c_templates_tera/acados_multi_solver.in.c | 296 ++++++++++++------ .../c_templates_tera/acados_solver.in.c | 93 ++++++ .../acados_template/acados_template/utils.py | 5 + 32 files changed, 1769 insertions(+), 268 deletions(-) create mode 100644 examples/acados_matlab_octave/pendulum_on_cart_model/main_slack_min_formulations.m create mode 100644 examples/acados_matlab_octave/pendulum_on_cart_model/slack_min_formulation.m create mode 100644 examples/acados_python/pendulum_on_cart/ocp/slack_min_formulation.py create mode 100644 interfaces/acados_matlab_octave/ns_from_idxs_rev.m diff --git a/.github/workflows/full_build_windows.yml b/.github/workflows/full_build_windows.yml index 2f67b2e66d..69176ad40b 100644 --- a/.github/workflows/full_build_windows.yml +++ b/.github/workflows/full_build_windows.yml @@ -123,6 +123,18 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/p_global_example main + - name: MATLAB idxs_rev example + uses: matlab-actions/run-command@v2 + if: always() + with: + command: | + cd ${{runner.workspace}}/acados/.github/windows + setup_mingw + cd ${{runner.workspace}}/acados/examples/acados_matlab_octave + acados_env_variables_windows + cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/pendulum_on_cart_model + main_slack_min_formulations + # This fails undeterministically on Github actions: # Two actions with the same commit, 1 fail, 1 success. # https://github.com/sandmaennchen/acados/actions/runs/11783596401/job/32821083440 diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 3864504a8c..f58c2f1079 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -165,8 +165,6 @@ dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory) qp_in->dim->nb = dims->nb; qp_in->dim->ng = dims->ng; qp_in->dim->ns = dims->ns; - qp_in->dim->nsb = dims->nsb; - qp_in->dim->nsg = dims->nsg; assert((char *) raw_memory + dense_qp_in_calculate_size(dims) >= c_ptr); @@ -414,18 +412,52 @@ void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]) -void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out) +void dense_qp_stack_slacks_dims_upperbound(dense_qp_dims *in, dense_qp_dims *out) { out->nv = in->nv + 2 * in->ns; out->ne = in->ne; - out->nb = in->nb - in->nsb + 2 * in->ns; - out->ng = in->ns > 0 ? in->ng + in->nsb : in->ng; + out->nb = in->nb + 2 * in->ns; + out->ng = in->ng + in->nb; out->ns = 0; - out->nsb = 0; - out->nsg = 0; } +// TODO: generalize this to handle real idxs_rev. +void dense_qp_stack_slacks_recover_nsb_nsg_from_idxs_rev(dense_qp_dims *in, int *idxs_rev, int* nsb, int *nsg) +{ + // recover nsb, nsg from idxs_rev + *nsb = 0; + *nsg = 0; + + for (int ii = 0; ii < in->nb; ii++) + { + if (idxs_rev[ii] > -1) + { + *nsb += 1; + } + } + for (int ii = in->nb; ii < in->nb + in->ng; ii++) + { + if (idxs_rev[ii] > -1) + { + *nsg += 1; + } + } +} + + +void dense_qp_stack_slacks_dims_from_idxs_rev(dense_qp_dims *in, int *idxs_rev, dense_qp_dims *out) +{ + int nsb, nsg; + dense_qp_stack_slacks_recover_nsb_nsg_from_idxs_rev(in, idxs_rev, &nsb, &nsg); + // set dims + out->nv = in->nv + 2 * in->ns; + out->ne = in->ne; + out->nb = in->nb - nsb + 2 * in->ns; + out->ng = in->ns > 0 ? in->ng + nsb : in->ng; + out->ns = 0; +} + void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) { @@ -434,8 +466,6 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) int nb = in->dim->nb; int ng = in->dim->ng; int ns = in->dim->ns; - int nsb = in->dim->nsb; - // int nsg = in->dim->nsg; int *idxs_rev = in->idxs_rev; int *idxb = in->idxb; @@ -444,6 +474,9 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) int nb2 = out->dim->nb; int ng2 = out->dim->ng; + int nsb, nsg; + dense_qp_stack_slacks_recover_nsb_nsg_from_idxs_rev(in->dim, idxs_rev, &nsb, &nsg); + assert(nv2 == nv+2*ns && "Dimensions are wrong!"); assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!"); assert(ng2 == ng+nsb && "Dimensions are wrong!"); @@ -590,8 +623,6 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out int nb = qp_out->dim->nb; int ng = qp_out->dim->ng; int ns = qp_out->dim->ns; - int nsb = qp_out->dim->nsb; - // int nsg = qp_out->dim->nsg; int *idxs_rev = qp_out->idxs_rev; @@ -600,6 +631,9 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out int nb2 = in->dim->nb; int ng2 = in->dim->ng; + int nsb, nsg; + dense_qp_stack_slacks_recover_nsb_nsg_from_idxs_rev(in->dim, idxs_rev, &nsb, &nsg); + UNUSED(nsb); UNUSED(nv2); diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index 42ea02d69f..e3e897cbea 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -145,8 +145,11 @@ acados_size_t dense_qp_seed_calculate_size(dense_qp_dims *dims); dense_qp_seed *dense_qp_seed_assign(dense_qp_dims *dims, void *raw_memory); /* misc */ +void dense_qp_stack_slacks_dims_upperbound(dense_qp_dims *in, dense_qp_dims *out); // -void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); +void dense_qp_stack_slacks_dims_from_idxs_rev(dense_qp_dims *in, int *idxs_rev, dense_qp_dims *out); +// +void dense_qp_stack_slacks_recover_nsb_nsg_from_idxs_rev(dense_qp_dims *in, int *idxs_rev, int* nsb, int *nsg); // void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out); // diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c index 22f8adee29..3ac52f2b4f 100644 --- a/acados/dense_qp/dense_qp_daqp.c +++ b/acados/dense_qp/dense_qp_daqp.c @@ -207,6 +207,7 @@ acados_size_t dense_qp_daqp_memory_calculate_size(void *config_, dense_qp_dims * int m = dims->nv + dims->ng + dims->ne; int ms = dims->nv; int nb = dims->nb; + int ng = dims->ng; int ns = dims->ns; acados_size_t size = sizeof(dense_qp_daqp_memory); @@ -215,6 +216,7 @@ acados_size_t dense_qp_daqp_memory_calculate_size(void *config_, dense_qp_dims * size += nb * 2 * sizeof(c_float); // lb_tmp & ub_tmp size += nb * 1 * sizeof(int); // idbx + size += (nb + ng) * sizeof(int); // idxs_rev size += n * 1 * sizeof(int); // idxv_to_idxb; size += ns * 1 * sizeof(int); // idbs size += m * 1 * sizeof(int); // idxdaqp_to_idxs; @@ -355,6 +357,7 @@ void *dense_qp_daqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts int m = dims->nv + dims->ng + dims->ne; int ms = dims->nv; int nb = dims->nb; + int ng = dims->ng; int ns = dims->ns; // char pointer @@ -380,6 +383,10 @@ void *dense_qp_daqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts mem->idxb = (int *) c_ptr; c_ptr += nb * 1 * sizeof(int); + mem->idxs_rev = (int *) c_ptr; + c_ptr += (nb + ng) * sizeof(int); + + mem->idxv_to_idxb = (int *) c_ptr; c_ptr += n * 1 * sizeof(int); @@ -489,7 +496,7 @@ static void dense_qp_daqp_update_memory(dense_qp_in *qp_in, const dense_qp_daqp_ work->qp->A+nv*ng, work->qp->bupper+nv+ng, // equalities idxb, lb_tmp, ub_tmp, // bounds work->qp->A, work->qp->blower+nv, work->qp->bupper+nv, // general linear constraints - mem->Zl, mem->Zu, mem->zl, mem->zu, idxs, mem->d_ls, mem->d_us // slacks + mem->Zl, mem->Zu, mem->zl, mem->zu, idxs, mem->idxs_rev, mem->d_ls, mem->d_us // slacks ); // printf("\nDAQP: matrix A\n"); diff --git a/acados/dense_qp/dense_qp_daqp.h b/acados/dense_qp/dense_qp_daqp.h index 425c734a4a..f40f7e84bd 100644 --- a/acados/dense_qp/dense_qp_daqp.h +++ b/acados/dense_qp/dense_qp_daqp.h @@ -61,6 +61,7 @@ typedef struct dense_qp_daqp_memory_ int* idxb; int* idxv_to_idxb; int* idxs; + int* idxs_rev; int* idxdaqp_to_idxs; double* Zl; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 0a15bbe601..4e0d5bd4a4 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -215,21 +215,33 @@ acados_size_t dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dim { dense_qp_dims dims_stacked; - int nv = dims->nv; - int ne = dims->ne; - int ng = dims->ng; - int nb = dims->nb; - int nsb = dims->nsb; - // int nsg = dims->nsg; - int ns = dims->ns; - - int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? ng + nsb : ng; - int nb2 = nb - nsb + 2 * ns; + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int ns = dims->ns; + + int ng2, nv2, nb2; // size in bytes acados_size_t size = sizeof(dense_qp_qpoases_memory); + if (ns > 0) + { + dense_qp_stack_slacks_dims_upperbound(dims, &dims_stacked); + size += dense_qp_in_calculate_size(&dims_stacked); + ng2 = dims_stacked.ng; + nv2 = dims_stacked.nv; + nb2 = dims_stacked.nb; + } + else + { + ng2 = ng; + nv2 = nv; + nb2 = nb; + } + // same logic as in dense_qp_stack_slacks_dims_upperbound, but no memory to call the function here. + size += 1 * nv * nv * sizeof(double); // H size += 1 * nv2 * nv2 * sizeof(double); // HH size += 1 * nv2 * nv2 * sizeof(double); // R @@ -245,17 +257,12 @@ acados_size_t dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dim size += 2 * ng2 * sizeof(double); // d_lg d_ug size += 1 * nb * sizeof(int); // idxb size += 1 * nb2 * sizeof(int); // idxb_stacked + size += 1 * (nb+ng) * sizeof(int); // idxs_rev size += 1 * ns * sizeof(int); // idxs size += 1 * nv2 * sizeof(double); // prim_sol size += 1 * (nv2 + ng2) * sizeof(double); // dual_sol size += 6 * ns * sizeof(double); // Zl, Zu, zl, zu, d_ls, d_us - if (ns > 0) - { - dense_qp_stack_slacks_dims(dims, &dims_stacked); - size += dense_qp_in_calculate_size(&dims_stacked); - } - if (ng > 0 || ns > 0) // QProblem size += QProblem_calculateMemorySize(nv2, ng2); else // QProblemB @@ -274,17 +281,13 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o dense_qp_qpoases_memory *mem; dense_qp_dims dims_stacked; - int nv = dims->nv; - int ne = dims->ne; - int ng = dims->ng; - int nb = dims->nb; - int nsb = dims->nsb; - // int nsg = dims->nsg; - int ns = dims->ns; + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int ns = dims->ns; - int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? ng + nsb : ng; - int nb2 = nb - nsb + 2 * ns; + int ng2, nv2, nb2; // char pointer char *c_ptr = (char *) raw_memory; @@ -296,13 +299,19 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o if (ns > 0) { - dense_qp_stack_slacks_dims(dims, &dims_stacked); + dense_qp_stack_slacks_dims_upperbound(dims, &dims_stacked); mem->qp_stacked = dense_qp_in_assign(&dims_stacked, c_ptr); c_ptr += dense_qp_in_calculate_size(&dims_stacked); + ng2 = dims_stacked.ng; + nv2 = dims_stacked.nv; + nb2 = dims_stacked.nb; } else { mem->qp_stacked = NULL; + ng2 = ng; + nv2 = nv; + nb2 = nb; } assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); @@ -350,6 +359,7 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o assign_and_advance_int(nb, &mem->idxb, &c_ptr); assign_and_advance_int(nb2, &mem->idxb_stacked, &c_ptr); assign_and_advance_int(ns, &mem->idxs, &c_ptr); + assign_and_advance_int(nb+ng, &mem->idxs_rev, &c_ptr); assert((char *) raw_memory + dense_qp_qpoases_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -444,6 +454,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo int *idxb = memory->idxb; int *idxb_stacked = memory->idxb_stacked; int *idxs = memory->idxs; + int *idxs_rev = memory->idxs_rev; double *prim_sol = memory->prim_sol; double *dual_sol = memory->dual_sol; QProblemB *QPB = memory->QPB; @@ -451,24 +462,34 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo dense_qp_in *qp_stacked = memory->qp_stacked; // extract dense qp size - int nv = qp_in->dim->nv; - // int ne = qp_in->dim->ne; - int ng = qp_in->dim->ng; - int nb = qp_in->dim->nb; - int nsb = qp_in->dim->nsb; - // int nsg = qp_in->dim->nsg; - int ns = qp_in->dim->ns; - - int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? ng + nsb : ng; - int nb2 = nb - nsb + 2 * ns; + int nv = qp_in->dim->nv; + // int ne = qp_in->dim->ne; + int ng = qp_in->dim->ng; + int nb = qp_in->dim->nb; + int ns = qp_in->dim->ns; + + int ng2, nv2, nb2; // fill in the upper triangular of H in dense_qp blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); // extract data from qp_in in row-major d_dense_qp_get_all_rowmaj(qp_in, H, g, A, b, idxb, d_lb0, d_ub0, C, d_lg0, d_ug0, - Zl, Zu, zl, zu, idxs, d_ls, d_us); + Zl, Zu, zl, zu, idxs, idxs_rev, d_ls, d_us); + + if (ns > 0) + { + dense_qp_stack_slacks_dims_from_idxs_rev(qp_in->dim, idxs_rev, qp_stacked->dim); + ng2 = qp_stacked->dim->ng; + nv2 = qp_stacked->dim->nv; + nb2 = qp_stacked->dim->nb; + } + else + { + ng2 = ng; + nv2 = nv; + nb2 = nb; + } // reorder box constraints bounds for (int ii = 0; ii < nv2; ii++) @@ -481,7 +502,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo { dense_qp_stack_slacks(qp_in, qp_stacked); d_dense_qp_get_all_rowmaj(qp_stacked, HH, gg, A, b, idxb_stacked, d_lb0, d_ub0, CC, d_lg, - d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL); + d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL); for (int ii = 0; ii < nb2; ii++) { diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 2c17540f2e..bb3f5cd253 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -84,6 +84,7 @@ typedef struct dense_qp_qpoases_memory_ int *idxb; int *idxb_stacked; int *idxs; + int *idxs_rev; double *prim_sol; double *dual_sol; void *QPB; // NOTE(giaf): cast to QProblemB to use diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 69edff4f27..4fc0bc614b 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -576,6 +576,12 @@ void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_, const char *field, config->cost[i]->dims_set(config->cost[i], dims->cost[i], "ns", &int_array[i]); } + // constraints + for (int i = 0; i <= N; i++) + { + config->constraints[i]->dims_set(config->constraints[i], + dims->constraints[i], "ns", &int_array[i]); + } // qp solver // if (!config->with_feasible_qp) // { @@ -622,19 +628,30 @@ void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_, const char *field, -static void ocp_nlp_update_qp_solver_ns_from_qp_solver_nsbxug(void *config_, void *dims_, int stage) +static void ocp_nlp_update_relaxed_qp_solver_ns_from_constraints_module(void *config_, void *dims_, int stage) { ocp_nlp_config *config = config_; ocp_nlp_dims *dims = dims_; - int tmp_int; - int ns = 0; - config->relaxed_qp_solver->dims_get(config->relaxed_qp_solver, dims->relaxed_qp_solver, stage, "nsbu", &tmp_int); - ns += tmp_int; - config->relaxed_qp_solver->dims_get(config->relaxed_qp_solver, dims->relaxed_qp_solver, stage, "nsbx", &tmp_int); - ns += tmp_int; - config->relaxed_qp_solver->dims_get(config->relaxed_qp_solver, dims->relaxed_qp_solver, stage, "nsg", &tmp_int); - ns += tmp_int; + int ng_qp_solver, nsbu, nsbx; + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "ng_qp_solver", &ng_qp_solver); + + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "nsbu", &nsbu); + + if (stage == 0) + { + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "nsbx", &nsbx); + } + else + { + // slack all x constraints + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "nbx", &nsbx); + } + int ns = ng_qp_solver + nsbu + nsbx; config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, stage, "ns", &ns); } @@ -648,7 +665,6 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c int *int_value = (int *) value_; int i = stage; - int tmp_int; // set in constraint module config->constraints[i]->dims_set(config->constraints[i], dims->constraints[i], @@ -668,34 +684,18 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c { config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, field, int_value); config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, field, int_value); - if ((!strcmp(field, "nbx")) && (stage != 0)) - { - config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, "nsbx", int_value); - } - ocp_nlp_update_qp_solver_ns_from_qp_solver_nsbxug(config, dims, stage); + ocp_nlp_update_relaxed_qp_solver_ns_from_constraints_module(config, dims, stage); // regularization config->regularize->dims_set(config->regularize, dims->regularize, i, (char *) field, int_value); } else if (!strcmp(field, "nsbx")) { - // qp solver - config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, field, int_value); - // relaxed_qp_solver - if (stage == 0) - { - config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "nsbx", &tmp_int); - config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, field, &tmp_int); - } - ocp_nlp_update_qp_solver_ns_from_qp_solver_nsbxug(config, dims, stage); + ocp_nlp_update_relaxed_qp_solver_ns_from_constraints_module(config, dims, stage); } else if (!strcmp(field, "nsbu")) { - // qp solver - config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, field, int_value); - // relaxed_qp_solver: nsbu = nsbu - config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, field, int_value); - ocp_nlp_update_qp_solver_ns_from_qp_solver_nsbxug(config, dims, stage); + ocp_nlp_update_relaxed_qp_solver_ns_from_constraints_module(config, dims, stage); } else if ( (!strcmp(field, "ng")) || (!strcmp(field, "nh")) || (!strcmp(field, "nphi"))) { @@ -707,20 +707,11 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, "ng", &ng_qp_solver); // relaxed qp solver: nsg = ng; config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, "ng", &ng_qp_solver); - config->relaxed_qp_solver->dims_set(config->relaxed_qp_solver, dims->relaxed_qp_solver, i, "nsg", &ng_qp_solver); - ocp_nlp_update_qp_solver_ns_from_qp_solver_nsbxug(config, dims, stage); + ocp_nlp_update_relaxed_qp_solver_ns_from_constraints_module(config, dims, stage); // regularization config->regularize->dims_set(config->regularize, dims->regularize, i, "ng", &ng_qp_solver); } - else if ( (!strcmp(field, "nsg")) || (!strcmp(field, "nsh")) || (!strcmp(field, "nsphi"))) - { - int nsg_qp_solver; - config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "nsg_qp_solver", &nsg_qp_solver); - - // qp solver - config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, "nsg", &nsg_qp_solver); - } else if (!strcmp(field, "nbxe")) { config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, field, int_value); @@ -3480,7 +3471,8 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl dims->nh_total += tmp; } - // precompute + /* precompute submodules */ + // dyn for (ii = 0; ii < N; ii++) { // set T @@ -3493,12 +3485,18 @@ int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nl if (status != ACADOS_SUCCESS) return status; } + // cost for (ii = 0; ii <= N; ii++) { - // cost precompute config->cost[ii]->precompute(config->cost[ii], dims->cost[ii], in->cost[ii], opts->cost[ii], mem->cost[ii], work->cost[ii]); } + // constraints + for (ii = 0; ii <= N; ii++) + { + config->constraints[ii]->precompute(config->constraints[ii], dims->constraints[ii], in->constraints[ii], + opts->constraints[ii], mem->constraints[ii], work->constraints[ii]); + } ocp_nlp_alias_memory_to_submodules(config, dims, in, out, opts, mem, work); if (opts->fixed_hess) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 3d4d0895fb..d39d5b2d56 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -96,7 +96,7 @@ static void bgh_dims_update_nb(ocp_nlp_constraints_bgh_dims *dims) static void bgh_dims_update_ns(ocp_nlp_constraints_bgh_dims *dims) { - dims->ns = dims->nsbu + dims->nsbx + dims->nsg + dims->nsh; + dims->ns_derived = dims->nsbu + dims->nsbx + dims->nsg + dims->nsh; } void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, const char *field, @@ -137,6 +137,10 @@ void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, const char *fi { dims->nh = *value; } + else if (!strcmp(field, "ns")) + { + dims->ns = *value; + } else if (!strcmp(field, "nsbu")) { dims->nsbu = *value; @@ -301,6 +305,7 @@ acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *d size += sizeof(int) * nb; // idxb size += sizeof(int) * ns; // idxs + size += sizeof(int) * (nb+ng+nh); // idxs_rev size += sizeof(int)*(nbue+nbxe+nge+nhe); // idxe size += blasfeo_memsize_dvec(2 * nb + 2 * ng + 2 * nh + 2 * ns); // d size += blasfeo_memsize_dmat(nu + nx, ng); // DCt @@ -346,6 +351,8 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims_, void *raw_ assign_and_advance_int(nb, &model->idxb, &c_ptr); // idxs assign_and_advance_int(ns, &model->idxs, &c_ptr); + // idxs_rev + assign_and_advance_int(nb + ng + nh, &model->idxs_rev, &c_ptr); // idxe assign_and_advance_int(nbue+nbxe+nge+nhe, &model->idxe, &c_ptr); @@ -368,6 +375,8 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims_, void *raw_ for(ii=0; iiidxe[ii] = 0; + model->use_idxs_rev = 0; + // assert assert((char *) raw_memory + ocp_nlp_constraints_bgh_model_calculate_size(config, dims) >= c_ptr); @@ -522,6 +531,27 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, blasfeo_pack_dvec(nh, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_upper(model, nh, offset); } + // idxs_rev formulation + else if (!strcmp(field, "idxs_rev")) + { + ptr_i = (int *) value; + for (ii=0; ii < nb+ng+nh; ii++) + model->idxs_rev[ii] = ptr_i[ii]; + model->use_idxs_rev = 1; + } + else if (!strcmp(field, "ls")) + { + offset = 2*nb+2*ng+2*nh; + blasfeo_pack_dvec(ns, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, ns, offset); + } + else if (!strcmp(field, "us")) + { + offset = 2*nb+2*ng+2*nh+ns; + blasfeo_pack_dvec(ns, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, ns, offset); + } + // idxs_* formulation else if (!strcmp(field, "idxsbu")) { ptr_i = (int *) value; @@ -595,6 +625,7 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, blasfeo_pack_dvec(nsh, value, 1, &model->d, offset); ocp_nlp_constraints_bgh_update_mask_lower(model, nsh, offset); } + // equalities else if (!strcmp(field, "idxbue")) { ptr_i = (int *) value; @@ -1142,6 +1173,7 @@ void ocp_nlp_constraints_bgh_initialize(void *config_, void *dims_, void *model_ int nu = dims->nu; int nb = dims->nb; int ng = dims->ng; + int nh = dims->nh; int ns = dims->ns; int nbue = dims->nbue; int nbxe = dims->nbxe; @@ -1155,9 +1187,19 @@ void ocp_nlp_constraints_bgh_initialize(void *config_, void *dims_, void *model_ } // initialize idxs_rev - for (j = 0; j < ns; j++) + if (model->use_idxs_rev) { - memory->idxs_rev[model->idxs[j]] = j; + for (j = 0; j < nb + ng + nh; j++) + { + memory->idxs_rev[j] = model->idxs_rev[j]; + } + } + else + { + for (j = 0; j < ns; j++) + { + memory->idxs_rev[model->idxs[j]] = j; + } } // initialize idxe @@ -1364,10 +1406,29 @@ void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims_, void // adj += DCt * tmp_ni[nb:] blasfeo_dgemv_n(nu+nx, ng+nh, 1.0, memory->DCt, 0, 0, &work->tmp_ni, nb, 1.0, &memory->adj, 0, &memory->adj, 0); // soft - // adj[nu+nx:nu+nx+ns] = lam[idxs] - blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, 0, &memory->adj, nu+nx); - // adj[nu+nx+ns : nu+nx+2*ns] = lam[idxs + nb+ng+nh] - blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, nb+ng+nh, &memory->adj, nu+nx+ns); + if (model->use_idxs_rev) + { + int is; + // max of lam corresponding to each slack? + // adj[nu+nx : nu+nx+2*ns] = 0.0 + blasfeo_dvecse(2*ns, 0.0, &memory->adj, nu+nx); + for (int ii = 0; ii < nb+ng+nh; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->adj, nu+nx+is) += BLASFEO_DVECEL(memory->lam, ii); + BLASFEO_DVECEL(&memory->adj, nu+nx+ns+is) += BLASFEO_DVECEL(memory->lam, nb+ng+nh+ii); + } + } + } + else + { + // adj[nu+nx:nu+nx+ns] = lam[idxs] + blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, 0, &memory->adj, nu+nx); + // adj[nu+nx+ns : nu+nx+2*ns] = lam[idxs + nb+ng+nh] + blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, nb+ng+nh, &memory->adj, nu+nx+ns); + } // adj[nu+nx: ] += lam[2*nb+2*ng+2*nh :] blasfeo_daxpy(2*ns, 1.0, memory->lam, 2*nb+2*ng+2*nh, &memory->adj, nu+nx, &memory->adj, nu+nx); } @@ -1470,8 +1531,24 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model // soft // subtract slacks from softened constraints // fun_i = fun_i - slack_i for i \in I_slacked - blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx, model->idxs, &memory->fun, 0); - blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nh); + if (model->use_idxs_rev) + { + int is; + for (int ii = 0; ii < nb+ng+nh; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->fun, ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+is); + BLASFEO_DVECEL(&memory->fun, nb+ng+nh+ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+ns+is); + } + } + } + else + { + blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx, model->idxs, &memory->fun, 0); + blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nh); + } // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); @@ -1511,8 +1588,24 @@ void ocp_nlp_constraints_bgh_update_qp_vectors(void *config_, void *dims_, void // soft // subtract slacks from softened constraints // fun_i = fun_i - slack_i for i \in I_slacked - blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx, model->idxs, &memory->fun, 0); - blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nh); + if (model->use_idxs_rev) + { + int is; + for (int ii = 0; ii < nb+ng+nh; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->fun, ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+is); + BLASFEO_DVECEL(&memory->fun, nb+ng+nh+ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+ns+is); + } + } + } + else + { + blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx, model->idxs, &memory->fun, 0); + blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nh); + } // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); @@ -1524,6 +1617,53 @@ void ocp_nlp_constraints_bgh_update_qp_vectors(void *config_, void *dims_, void } + +void ocp_nlp_constraints_bgh_precompute(void *config_, void *dims_, void *model_, + void *opts_, void *memory_, void *work_) +{ + ocp_nlp_constraints_bgh_dims *dims = dims_; + ocp_nlp_constraints_bgh_model *model = model_; + // ocp_nlp_constraints_bgh_opts *opts = opts_; + // ocp_nlp_constraints_bgh_memory *memory = memory_; + // ocp_nlp_constraints_bgh_workspace *work = work_; + + if (model->use_idxs_rev) + { + if (dims->ns_derived > 0) + { + printf("ocp_nlp_constraints_bgh_precompute: using idxs_rev, but individual slack dimensions are set."); + printf("Got ns = %d, dims->ns_derived = %d, nsbu = %d, nsbx = %d, nsg = %d, nsh = %d\n", dims->ns, dims->ns_derived, dims->nsbu, dims->nsbx, dims->nsg, dims->nsh); + exit(1); + } + } + else + { + if (dims->ns_derived != dims->ns) + { + printf("ocp_nlp_constraints_bgh_precompute: not using idxs_rev, but individual slack dimensions don't add up."); + printf("Got ns = %d != dims->ns_derived = %d, nsbu = %d, nsbx = %d, nsg = %d, nsh = %d\n", dims->ns, dims->ns_derived, dims->nsbu, dims->nsbx, dims->nsg, dims->nsh); + exit(1); + } + /* Use below to test idxs_rev implementation, even if idxs formulation is provided. */ + // for (int ii=0; iinb+dims->ng+dims->nh; ii++) + // { + // model->idxs_rev[ii] = -1; + // } + // for (int ii=0; iins; ii++) + // { + // model->idxs_rev[model->idxs[ii]] = ii; + // } + // model->use_idxs_rev = 1; + // // printf("BGH precompute: forcing idxs_rev implementation: \nidxs_rev =\n"); + // // for (int ii=0; iinb+dims->ng+dims->nh; ii++) + // // { + // // printf("%d ", model->idxs_rev[ii]); + // // } + // // printf("\n"); + } +} + + size_t ocp_nlp_constraints_bgh_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_) { ocp_nlp_constraints_bgh_model *model = model_; @@ -1735,6 +1875,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_, int stage) config->get_external_fun_workspace_requirement = &ocp_nlp_constraints_bgh_get_external_fun_workspace_requirement; config->set_external_fun_workspaces = &ocp_nlp_constraints_bgh_set_external_fun_workspaces; config->initialize = &ocp_nlp_constraints_bgh_initialize; + config->precompute = &ocp_nlp_constraints_bgh_precompute; config->update_qp_matrices = &ocp_nlp_constraints_bgh_update_qp_matrices; config->update_qp_vectors = &ocp_nlp_constraints_bgh_update_qp_vectors; config->compute_fun = &ocp_nlp_constraints_bgh_compute_fun; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index e3241fbd55..e1d8a390e2 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -63,7 +63,8 @@ typedef struct int nbx; // number of state box constraints int ng; // number of general linear constraints int nh; // number of nonlinear path constraints - int ns; // nsbu + nsbx + nsg + nsh + int ns; // number of slack variables per side, i.e. lower + upper, nsbu + nsbx + nsg + nsh + int ns_derived; // number of slack variables derived from nsbu + nsbx + nsg + nsh int nsbu; // number of softened input bounds int nsbx; // number of softened state bounds int nsg; // number of softened general linear constraints @@ -73,6 +74,7 @@ typedef struct int nge; // number of general linear constraints which are equality int nhe; // number of nonlinear path constraints which are equality int np_global; + } ocp_nlp_constraints_bgh_dims; // @@ -92,8 +94,10 @@ void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, typedef struct { + int use_idxs_rev; // flag to indicate if idxs_rev formulation is used int *idxb; int *idxs; + int *idxs_rev; int *idxe; struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; // gathers bounds diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index 3366c132b4..290cfb852c 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -95,7 +95,7 @@ static void bgp_dims_update_nb(ocp_nlp_constraints_bgp_dims *dims) static void bgp_dims_update_ns(ocp_nlp_constraints_bgp_dims *dims) { - dims->ns = dims->nsbu + dims->nsbx + dims->nsg + dims->nsphi; + dims->ns_derived = dims->nsbu + dims->nsbx + dims->nsg + dims->nsphi; } @@ -133,6 +133,10 @@ void ocp_nlp_constraints_bgp_dims_set(void *config_, void *dims_, { dims->nphi = *value; } + else if (!strcmp(field, "ns")) + { + dims->ns = *value; + } else if (!strcmp(field, "nsbu")) { dims->nsbu = *value; @@ -309,11 +313,12 @@ acados_size_t ocp_nlp_constraints_bgp_model_calculate_size(void *config, void *d size += sizeof(ocp_nlp_constraints_bgp_model); - size += sizeof(int) * nb; // idxb - size += sizeof(int) * ns; // idxs - size += sizeof(int)*(nbue+nbxe+nge+nphie); // idxe + size += sizeof(int) * nb; // idxb + size += sizeof(int) * ns; // idxs + size += sizeof(int) * (nb + ng + nphi); // idxs_rev + size += sizeof(int)*(nbue+nbxe+nge+nphie); // idxe size += blasfeo_memsize_dvec(2 * nb + 2 * ng + 2 * nphi + 2 * ns); // d - size += blasfeo_memsize_dmat(nu + nx, ng); // DCt + size += blasfeo_memsize_dmat(nu + nx, ng); // DCt size += 64; // blasfeo_mem align make_int_multiple_of(8, &size); @@ -363,12 +368,9 @@ void *ocp_nlp_constraints_bgp_model_assign(void *config, void *dims_, void *raw_ // default initialization to zero blasfeo_dvecse(2*nb+2*ng+2*nphi+2*ns, 0.0, &model->d, 0); - // int - // idxb assign_and_advance_int(nb, &model->idxb, &c_ptr); - // idxs assign_and_advance_int(ns, &model->idxs, &c_ptr); - // idxe + assign_and_advance_int(nb + ng + nphi, &model->idxs_rev, &c_ptr); assign_and_advance_int(nbue+nbxe+nge+nphie, &model->idxe, &c_ptr); // h @@ -523,6 +525,27 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, blasfeo_pack_dvec(nphi, value, 1, &model->d, offset); ocp_nlp_constraints_bgp_update_mask_upper(model, nphi, offset); } + // idxs_rev formulation + else if (!strcmp(field, "idxs_rev")) + { + ptr_i = (int *) value; + for (ii=0; ii < nb+ng+nphi; ii++) + model->idxs_rev[ii] = ptr_i[ii]; + model->use_idxs_rev = 1; + } + else if (!strcmp(field, "ls")) + { + offset = 2*nb+2*ng+2*nphi; + blasfeo_pack_dvec(ns, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, ns, offset); + } + else if (!strcmp(field, "us")) + { + offset = 2*nb+2*ng+2*nphi+ns; + blasfeo_pack_dvec(ns, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, ns, offset); + } + // idxs_* formulation else if (!strcmp(field, "idxsbu")) { ptr_i = (int *) value; @@ -595,6 +618,7 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, blasfeo_pack_dvec(nsphi, value, 1, &model->d, offset); ocp_nlp_constraints_bgp_update_mask_lower(model, nsphi, offset); } + // equalities else if (!strcmp(field, "idxbue")) { ptr_i = (int *) value; @@ -1099,6 +1123,7 @@ void ocp_nlp_constraints_bgp_initialize(void *config_, void *dims_, void *model_ int nu = dims->nu; int nb = dims->nb; int ng = dims->ng; + int nphi = dims->nphi; int ns = dims->ns; int nbue = dims->nbue; int nbxe = dims->nbxe; @@ -1112,11 +1137,22 @@ void ocp_nlp_constraints_bgp_initialize(void *config_, void *dims_, void *model_ } // initialize idxs_rev - for (j = 0; j < ns; j++) + if (model->use_idxs_rev) + { + for (j = 0; j < nb + ng + nphi; j++) + { + memory->idxs_rev[j] = model->idxs_rev[j]; + } + } + else { - memory->idxs_rev[model->idxs[j]] = j; + for (j = 0; j < ns; j++) + { + memory->idxs_rev[model->idxs[j]] = j; + } } + // initialize idxe for (j = 0; j < nbue+nbxe+nge+nphie; j++) { @@ -1269,10 +1305,29 @@ void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims_, void blasfeo_dgemv_n(nu+nx, ng+nphi, 1.0, memory->DCt, 0, 0, &work->tmp_ni, nb, 1.0, &memory->adj, 0, &memory->adj, 0); // soft - blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, 0, &memory->adj, nu + nx); - blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, nb+ng+nphi, &memory->adj, nu+nx+ns); - blasfeo_daxpy(2 * ns, 1.0, memory->lam, 2 * nb + 2 * ng + 2 * nphi, &memory->adj, nu + nx, - &memory->adj, nu + nx); + if (model->use_idxs_rev) + { + int is; + // max of lam corresponding to each slack? + // adj[nu+nx : nu+nx+2*ns] = 0.0 + blasfeo_dvecse(2*ns, 0.0, &memory->adj, nu+nx); + for (int ii = 0; ii < nb+ng+nphi; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->adj, nu+nx+is) += BLASFEO_DVECEL(memory->lam, ii); + BLASFEO_DVECEL(&memory->adj, nu+nx+ns+is) += BLASFEO_DVECEL(memory->lam, nb+ng+nphi+ii); + } + } + } + else + { + blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, 0, &memory->adj, nu + nx); + blasfeo_dvecex_sp(ns, 1.0, model->idxs, memory->lam, nb+ng+nphi, &memory->adj, nu+nx+ns); + blasfeo_daxpy(2 * ns, 1.0, memory->lam, 2 * nb + 2 * ng + 2 * nphi, &memory->adj, nu + nx, + &memory->adj, nu + nx); + } } if (opts->compute_hess) @@ -1369,8 +1424,24 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model // soft // subtract slacks from softened constraints // fun_i = fun_i - slack_i for i \in I_slacked - blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx, model->idxs, &memory->fun, 0); - blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nphi); + if (model->use_idxs_rev) + { + int is; + for (int ii = 0; ii < nb+ng+nphi; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->fun, ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+is); + BLASFEO_DVECEL(&memory->fun, nb+ng+nphi+ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+ns+is); + } + } + } + else + { + blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx, model->idxs, &memory->fun, 0); + blasfeo_dvecad_sp(ns, -1.0, ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nphi); + } // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); @@ -1413,8 +1484,26 @@ void ocp_nlp_constraints_bgp_update_qp_vectors(void *config_, void *dims_, void // soft // subtract slacks from softened constraints // fun_i = fun_i - slack_i for i \in I_slacked - blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx, model->idxs, &memory->fun, 0); - blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nphi); + if (model->use_idxs_rev) + { + int is; + for (int ii = 0; ii < nb+ng+nphi; ii++) + { + is = memory->idxs_rev[ii]; + if (is >= 0) + { + BLASFEO_DVECEL(&memory->fun, ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+is); + BLASFEO_DVECEL(&memory->fun, nb+ng+nphi+ii) -= BLASFEO_DVECEL(memory->ux, nu+nx+ns+is); + } + } + } + else + { + blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx, model->idxs, &memory->fun, 0); + blasfeo_dvecad_sp(ns, -1.0, memory->ux, nu+nx+ns, model->idxs, &memory->fun, nb+ng+nphi); + } + + // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); @@ -1440,6 +1529,55 @@ void ocp_nlp_constraints_bgp_compute_adj_p(void* config_, void *dims_, void *mod exit(1); } + + +void ocp_nlp_constraints_bgp_precompute(void *config_, void *dims_, void *model_, + void *opts_, void *memory_, void *work_) +{ + ocp_nlp_constraints_bgp_dims *dims = dims_; + ocp_nlp_constraints_bgp_model *model = model_; + // ocp_nlp_constraints_bgp_opts *opts = opts_; + // ocp_nlp_constraints_bgp_memory *memory = memory_; + // ocp_nlp_constraints_bgp_workspace *work = work_; + + if (model->use_idxs_rev) + { + if (dims->ns_derived > 0) + { + printf("ocp_nlp_constraints_bgp_precompute: using idxs_rev, but individual slack dimensions are set."); + printf("Got ns = %d, dims->ns_derived = %d, nsbu = %d, nsbx = %d, nsg = %d, nsphi = %d\n", dims->ns, dims->ns_derived, dims->nsbu, dims->nsbx, dims->nsg, dims->nsphi); + exit(1); + } + } + else + { + if (dims->ns_derived != dims->ns) + { + printf("ocp_nlp_constraints_bgp_precompute: not using idxs_rev, but individual slack dimensions don't add up."); + printf("Got ns = %d != dims->ns_derived = %d, nsbu = %d, nsbx = %d, nsg = %d, nsphi = %d\n", dims->ns, dims->ns_derived, dims->nsbu, dims->nsbx, dims->nsg, dims->nsphi); + exit(1); + } + + /* Use below to test idxs_rev implementation, even if idxs formulation is provided. */ + // for (int ii=0; iinb+dims->ng+dims->nphi; ii++) + // { + // model->idxs_rev[ii] = -1; + // } + // for (int ii=0; iins; ii++) + // { + // model->idxs_rev[model->idxs[ii]] = ii; + // } + // model->use_idxs_rev = 1; + // // printf("BGP precompute: forcing idxs_rev implementation: \nidxs_rev =\n"); + // // for (int ii=0; iinb+dims->ng+dims->nphi; ii++) + // // { + // // printf("%d ", model->idxs_rev[ii]); + // // } + // // printf("\n"); + } +} + + size_t ocp_nlp_constraints_bgp_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_) { ocp_nlp_constraints_bgp_model *model = model_; @@ -1504,6 +1642,7 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_, int stage) config->get_external_fun_workspace_requirement = &ocp_nlp_constraints_bgp_get_external_fun_workspace_requirement; config->set_external_fun_workspaces = &ocp_nlp_constraints_bgp_set_external_fun_workspaces; config->initialize = &ocp_nlp_constraints_bgp_initialize; + config->precompute = &ocp_nlp_constraints_bgp_precompute; config->update_qp_matrices = &ocp_nlp_constraints_bgp_update_qp_matrices; config->compute_fun = &ocp_nlp_constraints_bgp_compute_fun; config->update_qp_vectors = &ocp_nlp_constraints_bgp_update_qp_vectors; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index 86c992c43b..1acbb2a2f5 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -61,7 +61,8 @@ typedef struct int nbx; int ng; // number of general linear constraints int nphi; // dimension of convex outer part - int ns; // nsbu + nsbx + nsg + nsphi + int ns; // number of slack variables per side, i.e. lower + upper + int ns_derived; // number of slack variables derived from nsbu + nsbx + nsg + nsphi int nsbu; // number of softened input bounds int nsbx; // number of softened state bounds int nsg; // number of softened general linear constraints @@ -85,9 +86,10 @@ void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *fi typedef struct { - // ocp_nlp_constraints_bgp_dims *dims; + int use_idxs_rev; // flag to indicate if idxs_rev formulation is used int *idxb; int *idxs; + int *idxs_rev; int *idxe; struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; @@ -141,7 +143,7 @@ typedef struct struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory int *idxb; // pointer to idxb[ii] in qp_in - int *idxs_rev; // pointer to idxs_rev[ii] in qp_in + int *idxs_rev; // pointer to idxs_rev[ii] in qp_in int *idxe; // pointer to idxe[ii] in qp_in } ocp_nlp_constraints_bgp_memory; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index fdfca35594..f46be44f11 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -88,6 +88,7 @@ typedef struct void (*set_external_fun_workspaces)(void *config, void *dims, void *opts_, void *in, void *work_); void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work); // + void (*precompute)(void *config, void *dims, void *model, void *opts, void *mem, void *work); void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work); void (*update_qp_vectors)(void *config, void *dims, void *model, void *opts, void *mem, void *work); void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work); diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 128067ace5..7e5e3644c3 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -134,9 +134,6 @@ ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory) for (int i=0; i<=N; i++) { - dims->nsbu[i] = 0; - dims->nsbx[i] = 0; - dims->nsg[i] = 0; dims->ns[i] = 0; } @@ -673,6 +670,8 @@ void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]) } +/* +ocp_qp_stack_slacks -> not used anymore, broken when migrating to idxs_rev void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out) { @@ -871,6 +870,7 @@ void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) } } } +*/ diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index bf62e82874..91218ede46 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -195,11 +195,12 @@ ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory); /* misc */ // -void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out); -// -void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out); -// void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out); +// +// ocp_qp_stack_slacks -> not used anymore, broken when migrating to idxs_rev +// void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out); +// // +// void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/utils/print.c b/acados/utils/print.c index 0f3a9c0f55..1d7b74ba25 100644 --- a/acados/utils/print.c +++ b/acados/utils/print.c @@ -198,12 +198,12 @@ void print_ocp_qp_dims(ocp_qp_dims *dims) { int N = dims->N; - printf("k\tnx\tnu\tnb\tnbx\tnbu\tng\tnsbu\tnsbx\tnsg\tns\tnbxe\tnbue\tnge\n"); + printf("k\tnx\tnu\tnb\tnbx\tnbu\tng\tns\tnbxe\tnbue\tnge\n"); for (int kk = 0; kk < N + 1; kk++) { - printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n", kk, dims->nx[kk], dims->nu[kk], dims->nb[kk], - dims->nbx[kk], dims->nbu[kk], dims->ng[kk], dims->nsbu[kk], dims->nsbx[kk], dims->nsg[kk], dims->ns[kk], + printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n", kk, dims->nx[kk], dims->nu[kk], dims->nb[kk], + dims->nbx[kk], dims->nbu[kk], dims->ng[kk], dims->ns[kk], dims->nbxe[kk], dims->nbue[kk], dims->nge[kk]); } } diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/main_slack_min_formulations.m b/examples/acados_matlab_octave/pendulum_on_cart_model/main_slack_min_formulations.m new file mode 100644 index 0000000000..cb01b994b8 --- /dev/null +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/main_slack_min_formulations.m @@ -0,0 +1,42 @@ +% 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. + +formulations = {'u_slack', 'u_slack2', 's_slack'}; +xtraj_list = {}; +for i = 1:length(formulations) + xtraj = slack_min_formulation(formulations{i}); + if i == 1 + xtraj_ref = xtraj; + else + diff_x = max(abs(xtraj(:) - xtraj_ref(:))); + fprintf('diff xtraj %s vs ref: %g\n', formulations{i}, diff_x); + if diff_x > 1e-6 + error('xtraj %s differs from reference by %g, expected to be close to zero.', formulations{i}, diff_x); + end + end +end diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/slack_min_formulation.m b/examples/acados_matlab_octave/pendulum_on_cart_model/slack_min_formulation.m new file mode 100644 index 0000000000..81e0c3dc61 --- /dev/null +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/slack_min_formulation.m @@ -0,0 +1,192 @@ +% 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. + + +function xtraj = slack_min_formulation(formulation) + if nargin < 1 + formulation = 's_slack'; + end + + import casadi.* + + ACADOS_INFTY = get_acados_infty(); + + % create ocp object to formulate the OCP + ocp = AcadosOcp(); + + % set model + model = get_pendulum_on_cart_model(); + ocp.model = model; + ocp.model.name = formulation; + + Tf = 1.0; + N = 10; + + % set prediction horizon + ocp.solver_options.N_horizon = N; + ocp.solver_options.tf = Tf; + + % cost matrices + Q_mat = 2*diag([1e3, 1e3, 1e-2, 1e-2]); + R_mat = 2*diag([1e-2]); + + % path cost + ocp.cost.cost_type = 'EXTERNAL'; + W_mat = blkdiag(Q_mat, R_mat); + ocp.model.cost_expr_ext_cost = 0.5 * (vertcat(model.x, model.u))' * W_mat * vertcat(model.x, model.u); + % terminal cost + ocp.cost.cost_type_e = 'EXTERNAL'; + ocp.model.cost_expr_ext_cost_e = 0.5 * model.x' * Q_mat * model.x; + + % set constraints + Fmax = 40; + ocp.constraints.lbu = [-Fmax]; + ocp.constraints.ubu = [+Fmax]; + ocp.constraints.idxbu = 0; + + % initial condition + ocp.constraints.x0 = [0.0; 0.2*pi; 0.0; 0.0]; + + fprintf('using formulation %s\n', formulation); + + % add cost term min(x[0], x[2]) to cost via slack: s <= x[0], s <= x[2] + if strcmp(formulation, 'u_slack') + % add u + new_u = SX.sym('new_u', 1, 1); + ocp.model.u = vertcat(model.u, new_u); + % add constraints u <= x_i <=> -inf <= u - x_i <= 0 + ocp.model.con_h_expr = vertcat(new_u - model.x(1), new_u - model.x(4)); + ocp.constraints.uh = zeros(2, 1); + ocp.constraints.lh = -ACADOS_INFTY * ones(2, 1); + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr; + ocp.constraints.uh_0 = ocp.constraints.uh; + ocp.constraints.lh_0 = ocp.constraints.lh; + + % add cost -u + ocp.model.cost_expr_ext_cost = ocp.model.cost_expr_ext_cost - new_u; + elseif strcmp(formulation, 'u_slack2') + % add u + new_u = SX.sym('new_u', 1, 1); + ocp.model.u = vertcat(model.u, new_u); + % add constraints u <= x_i <=> -inf <= u - x_i <= 0 + ocp.model.con_h_expr = vertcat(new_u + model.x(1), new_u + model.x(4)); + ocp.constraints.uh = ACADOS_INFTY * ones(2, 1); + ocp.constraints.lh = zeros(2, 1); + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr; + ocp.constraints.uh_0 = ocp.constraints.uh; + ocp.constraints.lh_0 = ocp.constraints.lh; + + % add cost u + ocp.model.cost_expr_ext_cost = ocp.model.cost_expr_ext_cost + new_u; + elseif strcmp(formulation, 's_slack') + % add s + ns = 1; + % add constraints: s <= x_i + ocp.model.con_h_expr = vertcat(model.x(1), model.x(4)); + ocp.constraints.uh = ACADOS_INFTY * ones(2, 1); + ocp.constraints.lh = zeros(2, 1); + ocp.constraints.idxs_rev = [-1; 0; 0]; + ocp.constraints.ls = -ACADOS_INFTY * ones(ns, 1); + ocp.constraints.us = zeros(ns, 1); + ocp.cost.zl = 1.0; + ocp.cost.Zl = -0.0; + ocp.cost.zu = 1.0; + ocp.cost.Zu = 0.0; + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr; + ocp.constraints.uh_0 = ocp.constraints.uh; + ocp.constraints.lh_0 = ocp.constraints.lh; + ocp.cost.zl_0 = ocp.cost.zl; + ocp.cost.Zl_0 = ocp.cost.Zl; + ocp.cost.zu_0 = ocp.cost.zu; + ocp.cost.Zu_0 = ocp.cost.Zu; + nbx_0 = numel(ocp.constraints.lbx_0); + nbu = numel(ocp.constraints.lbu); + ocp.constraints.idxs_rev_0 = [-ones(nbx_0+nbu,1); 0; 0]; + ocp.constraints.ls_0 = ocp.constraints.ls; + ocp.constraints.us_0 = ocp.constraints.us; + end + ocp.solver_options.qp_solver_t0_init = 0; + + % set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'; + ocp.solver_options.integrator_type = 'IRK'; + ocp.solver_options.nlp_solver_type = 'SQP'; + % ocp.solver_options.print_level = 5; + % ocp.solver_options.nlp_solver_max_iter = 2; + + nx = length(model.x); + nu = length(model.u); + + ocp_solver = AcadosOcpSolver(ocp); + + xtraj = zeros(N+1, nx); + utraj = zeros(N, nu); + + ocp_solver.solve(); + status = ocp_solver.get('status'); + ocp_solver.print_statistics(); + + if status ~= 0 + error('acados returned status %d.', status); + end + + % get solution + for i = 1:N + xtraj(i,:) = ocp_solver.get('x', i-1); + utraj(i,:) = ocp_solver.get('u', i-1); + end + xtraj(N+1,:) = ocp_solver.get('x', N); + + min_x_vals = min(xtraj(:,1), xtraj(:,4)); + if strcmp(formulation, 'u_slack') + slack_vals = utraj(:,2); + assert(all(abs(min_x_vals(1:end-1) - slack_vals) < 1e-6)); + elseif strcmp(formulation, 'u_slack2') + slack_vals = utraj(:,2); + assert(all(abs(min_x_vals(1:end-1) + slack_vals) < 1e-6)); + elseif strcmp(formulation, 's_slack') + slack_vals = zeros(N, 1); + unused_slack_vals = zeros(N, 1); + for i = 1:N + sl = ocp_solver.get('sl', i-1); + su = ocp_solver.get('su', i-1); + slack_vals(i) = sl(1); + unused_slack_vals(i) = su(1); + end + assert(all(abs(min_x_vals(1:end-1) + slack_vals) < 1e-6)); + disp('unused_slack_vals='); + disp(unused_slack_vals); + % plot slacks + utraj = [utraj, slack_vals]; + end +end + diff --git a/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py index 49f708e0d2..2d567a0eee 100644 --- a/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py +++ b/examples/acados_python/casadi_tests/test_casadi_slack_in_h.py @@ -49,9 +49,8 @@ def formulate_ocp(using_soft_constraints=True): nx = model.x.rows() nu = model.u.rows() - - # set prediction horizon + # set prediction horizon ocp.solver_options.N_horizon = N_horizon ocp.solver_options.tf = Tf @@ -135,8 +134,7 @@ def formulate_ocp(using_soft_constraints=True): def main(): ocp = formulate_ocp() - N = ocp.solver_options.N_horizon - + # create solver ocp_solver = AcadosOcpSolver(ocp, verbose=False) # solve OCP diff --git a/examples/acados_python/pendulum_on_cart/ocp/slack_min_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/slack_min_formulation.py new file mode 100644 index 0000000000..512bbed307 --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ocp/slack_min_formulation.py @@ -0,0 +1,223 @@ +# +# 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.; +# + +import sys +sys.path.insert(0, '../common') + +from acados_template import AcadosOcp, AcadosOcpSolver, plot_trajectories, AcadosModel, ACADOS_INFTY +from pendulum_model import export_pendulum_ode_model +import numpy as np +import casadi as ca + +def main(formulation='s_slack', plot_traj=True): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model: AcadosModel = export_pendulum_ode_model() + ocp.model = model + + Tf = 1.0 + N = 10 + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'EXTERNAL' + W_mat = ca.diagcat(Q_mat, R_mat).full() + ocp.model.cost_expr_ext_cost = .5*ca.vertcat(model.x, model.u).T @ W_mat @ ca.vertcat(model.x, model.u) + # terminal cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = .5 * model.x.T @ Q_mat @ model.x + + # set constraints + Fmax = 40 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + # initial condition + ocp.constraints.x0 = np.array([0.0, 0.2 * np.pi, 0.0, 0.0]) + + print(f"using formulation {formulation}") + # add cost term min(x[0], x[2]) to cost + # via slack: s <= x[0], s <= x[2] + if formulation == 'u_slack': + # add u + new_u = ca.SX.sym('new_u', 1, 1) + ocp.model.u = ca.vertcat(model.u, new_u) + ocp.model.u_labels.append('new_u') + # add constraints u <= x_i + # <=> -inf <= u - x_i <= 0 + ocp.model.con_h_expr = ca.vertcat(new_u - model.x[0], new_u - model.x[3]) + ocp.constraints.uh = np.zeros((2, 1)) + ocp.constraints.lh = - ACADOS_INFTY * np.ones((2, 1)) + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr + ocp.constraints.uh_0 = ocp.constraints.uh + ocp.constraints.lh_0 = ocp.constraints.lh + + # add cost -u + ocp.model.cost_expr_ext_cost -= new_u + elif formulation == 'u_slack2': + # add u + new_u = ca.SX.sym('new_u', 1, 1) + ocp.model.u = ca.vertcat(model.u, new_u) + ocp.model.u_labels.append('new_u') + # add constraints u <= x_i + # <=> -inf <= u - x_i <= 0 + ocp.model.con_h_expr = ca.vertcat(new_u + model.x[0], new_u + model.x[3]) + ocp.constraints.uh = ACADOS_INFTY * np.ones((2, 1)) + ocp.constraints.lh = 0 * np.ones((2, 1)) + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr + ocp.constraints.uh_0 = ocp.constraints.uh + ocp.constraints.lh_0 = ocp.constraints.lh + + # add cost u + ocp.model.cost_expr_ext_cost += new_u + elif formulation == "s_slack": + # add s + ns = 1 + # add constraints: s <= x_i + ocp.model.con_h_expr = ca.vertcat(model.x[0], model.x[3]) + ocp.constraints.uh = ACADOS_INFTY * np.ones((2, 1)) + ocp.constraints.lh = np.zeros((2, 1)) + ocp.constraints.idxs_rev = np.array([-1, 0, 0]) + ocp.constraints.ls = -ACADOS_INFTY * np.ones((ns, )) + ocp.constraints.us = 0 * np.ones((ns, )) + ocp.cost.zl = np.array([1.0]) + ocp.cost.Zl = np.array([-0.0]) + ocp.cost.zu = np.array([1.0]) + ocp.cost.Zu = np.array([0.0]) + + ocp.model.con_h_expr_0 = ocp.model.con_h_expr + ocp.constraints.uh_0 = ocp.constraints.uh + ocp.constraints.lh_0 = ocp.constraints.lh + ocp.cost.zl_0 = ocp.cost.zl + ocp.cost.Zl_0 = ocp.cost.Zl + ocp.cost.zu_0 = ocp.cost.zu + ocp.cost.Zu_0 = ocp.cost.Zu + nbx_0 = ocp.constraints.lbx_0.shape[0] + nbu = ocp.constraints.lbu.shape[0] + ocp.constraints.idxs_rev_0 = np.array((nbx_0+nbu) * [-1] + [0, 0]) + ocp.constraints.ls_0 = ocp.constraints.ls + ocp.constraints.us_0 = ocp.constraints.us + ocp.solver_options.qp_solver_t0_init = 0 + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'IRK' + ocp.solver_options.nlp_solver_type = 'SQP' + # ocp.solver_options.print_level = 5 + # ocp.solver_options.nlp_solver_max_iter = 2 + + + nx = model.x.rows() + nu = model.u.rows() + + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + xtraj = np.zeros((N+1, nx)) + utraj = np.zeros((N, nu)) + + status = ocp_solver.solve() + ocp_solver.print_statistics() + + if status != 0: + raise Exception(f'acados returned status {status}.') + + # get solution + for i in range(N): + xtraj[i,:] = ocp_solver.get(i, "x") + utraj[i,:] = ocp_solver.get(i, "u") + xtraj[N,:] = ocp_solver.get(N, "x") + + min_x_vals = np.minimum(xtraj[:, 0], xtraj[:, 3]) + if formulation == 'u_slack': + slack_vals = utraj[:, 1] + assert np.allclose(min_x_vals[:-1], slack_vals, atol=1e-6) + elif formulation == 'u_slack2': + slack_vals = utraj[:, 1] + assert np.allclose(min_x_vals[:-1], -slack_vals, atol=1e-6) + elif formulation == 's_slack': + slack_vals = np.zeros((N, )) + unused_slack_vals = np.zeros((N, )) + for i in range(N): + slack_vals[i] = ocp_solver.get(i, "sl")[0] + unused_slack_vals[i] = ocp_solver.get(i, "su")[0] + assert np.allclose(min_x_vals[:-1], -slack_vals, atol=1e-6) + print(f"{unused_slack_vals=}") + # plot slacks + utraj = np.append(utraj, np.atleast_2d(slack_vals).transpose(), axis=1) + model.u_labels.append('slack') + + if plot_traj: + plot_trajectories( + x_traj_list=[xtraj], + u_traj_list=[utraj], + time_traj_list=[np.linspace(0, Tf, N+1)], + time_label=model.t_label, + labels_list=['OCP result'], + x_labels=model.x_labels, + u_labels=model.u_labels, + idxbu=ocp.constraints.idxbu, + lbu=ocp.constraints.lbu, + ubu=ocp.constraints.ubu, + X_ref=None, + U_ref=None, + # fig_filename='pendulum_ocp.png', + x_min=None, + x_max=None, + ) + + return xtraj + + +if __name__ == '__main__': + formulations = ['u_slack', 'u_slack2', 's_slack'] + # formulations = ['s_slack'] + xtraj_list = [] + for i, formulation in enumerate(formulations): + xtraj = main(formulation, plot_traj=False) + if i == 0: + xtraj_ref = xtraj + else: + diff_x = np.max(np.abs(xtraj - xtraj_ref)) + print(f"diff xtraj {formulation} vs ref: {diff_x}") + if diff_x > 1e-6: + raise Exception(f"xtraj {formulation} differs from reference by {diff_x}, expected to be close to zero.") diff --git a/examples/c/dense_qp.c b/examples/c/dense_qp.c index 18e3e90ae3..d793f7b017 100644 --- a/examples/c/dense_qp.c +++ b/examples/c/dense_qp.c @@ -96,12 +96,10 @@ int main() { dims.nb = 2; dims.ng = 2; dims.ns = 3; - dims.nsb = 1; - dims.nsg = 2; dense_qp_in *qp_in = dense_qp_in_create(config, &dims); - d_dense_qp_set_all(H, g, NULL, NULL, idxb, d_lb, d_ub, C, d_lg, d_ug, Zl, Zu, zl, zu, idxs, d_ls, d_us, qp_in); + d_dense_qp_set_all(H, g, NULL, NULL, idxb, d_lb, d_ub, C, d_lg, d_ug, Zl, Zu, zl, zu, idxs, NULL, d_ls, d_us, qp_in); print_dense_qp_in(qp_in); diff --git a/external/hpipm b/external/hpipm index 1891706e9c..c24243df52 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 1891706e9cf0cf5e47df8e2317b653daf0fc695e +Subproject commit c24243df52b685278f7032b833f8651596cd0fa9 diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 6f18beccb1..4e611b99b2 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -268,6 +268,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp python ocp_example_h_init_contraints.py) + add_test(NAME python_slack_min_formulation + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp + python slack_min_formulation.py) + add_test(NAME python_chain_ocp COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/chain_mass python main.py) @@ -526,7 +530,8 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_nonuniform_discretization_ocp_example PROPERTIES DEPENDS python_rti_loop_ocp_example) set_tests_properties(python_rti_loop_ocp_example PROPERTIES DEPENDS python_render_simulink_wrapper) set_tests_properties(python_render_simulink_wrapper PROPERTIES DEPENDS python_constraints_expression_example) - set_tests_properties(python_constraints_expression_example PROPERTIES DEPENDS pendulum_optimal_value_gradient) + set_tests_properties(python_constraints_expression_example PROPERTIES DEPENDS python_slack_min_formulation) + set_tests_properties(python_slack_min_formulation PROPERTIES DEPENDS pendulum_optimal_value_gradient) set_tests_properties(pendulum_optimal_value_gradient PROPERTIES DEPENDS python_pendulum_ocp_IRK) set_tests_properties(python_pendulum_ocp_IRK PROPERTIES DEPENDS python_pendulum_ocp_GNSF) set_tests_properties(python_pendulum_ocp_GNSF PROPERTIES DEPENDS python_example_ocp_dynamics_formulations_cmake) diff --git a/interfaces/acados_c/ocp_qp_interface.c b/interfaces/acados_c/ocp_qp_interface.c index c7de1f6db0..3d9648b958 100644 --- a/interfaces/acados_c/ocp_qp_interface.c +++ b/interfaces/acados_c/ocp_qp_interface.c @@ -242,13 +242,6 @@ ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create_from_ocp_qp_dims( ocp_qp_dims_get(config, dims, i, "ns", &tmp_int); ocp_qp_xcond_solver_dims_set_(config, solver_dims, i, "ns", &tmp_int); - ocp_qp_dims_get(config, dims, i, "nsbx", &tmp_int); - ocp_qp_xcond_solver_dims_set_(config, solver_dims, i, "nsbx", &tmp_int); - ocp_qp_dims_get(config, dims, i, "nsbu", &tmp_int); - ocp_qp_xcond_solver_dims_set_(config, solver_dims, i, "nsbu", &tmp_int); - ocp_qp_dims_get(config, dims, i, "nsg", &tmp_int); - ocp_qp_xcond_solver_dims_set_(config, solver_dims, i, "nsg", &tmp_int); - ocp_qp_dims_get(config, dims, i, "nbxe", &tmp_int); ocp_qp_xcond_solver_dims_set_(config, solver_dims, i, "nbxe", &tmp_int); ocp_qp_dims_get(config, dims, i, "nbue", &tmp_int); diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index cadf31e4ec..6484dad8ab 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -366,6 +366,58 @@ function make_consistent_constraints_terminal(self) dims.nh_e = nh_e; end + function make_consistent_slacks_rev_path(self) + constraints = self.constraints; + dims = self.dims; + opts = self.solver_options; + cost = self.cost; + + if opts.N_horizon == 0 + return + end + + % constraints that are not slack bounds + ni_no_s = dims.nbu + dims.nbx + dims.nh + dims.ng + dims.nphi; + + % sanity checks + if length(constraints.idxs_rev) ~= ni_no_s + error(['inconsistent dimension idxs_rev = ', num2str(length(constraints.idxs_rev)), ... + '. Should be equal to ni_no_s = ', num2str(ni_no_s), '.']); + end + possible_vals = -1:(ni_no_s-1); + if any(~ismember(constraints.idxs_rev, possible_vals)) + error(['idxs_rev = [', num2str(constraints.idxs_rev(:).'), ... + '] contains value not in range -1:', num2str(ni_no_s-1), '.']); + end + + ns = ns_from_idxs_rev(constraints.idxs_rev); + + % slack bounds + if isempty(constraints.ls) + constraints.ls = zeros(ns, 1); + elseif length(constraints.ls) ~= ns + error('inconsistent dimension ns, regarding idxs_rev, ls.'); + end + if isempty(constraints.us) + constraints.us = zeros(ns, 1); + elseif length(constraints.us) ~= ns + error('inconsistent dimension ns, regarding idxs_rev, us.'); + end + + % check cost penalty + fields = {'Zl', 'Zu', 'zl', 'zu'}; + for i = 1:length(fields) + field = fields{i}; + dim = size(cost.(field), 1); + if dim ~= ns + error(['Inconsistent size for field ', field, ', with dimension ', num2str(dim), ... + ', Detected ns = ', num2str(ns), '.']); + end + end + + dims.ns = ns; + end + function make_consistent_slacks_path(self) constraints = self.constraints; dims = self.dims; @@ -481,6 +533,57 @@ function make_consistent_slacks_path(self) dims.nsphi = nsphi; end + function make_consistent_slacks_rev_initial(self) + constraints = self.constraints; + dims = self.dims; + opts = self.solver_options; + cost = self.cost; + if opts.N_horizon == 0 + return + end + + % constraints that are not slack bounds + ni_no_s = dims.nbu + dims.nbx_0 + dims.nh_0 + dims.ng + dims.nphi_0; + + % sanity checks + if length(constraints.idxs_rev_0) ~= ni_no_s + error(['inconsistent dimension idxs_rev_0 = ', num2str(length(constraints.idxs_rev_0)), ... + '. Should be equal to ni_no_s = ', num2str(ni_no_s), '.']); + end + possible_vals = -1:(ni_no_s-1); + if any(~ismember(constraints.idxs_rev_0, possible_vals)) + error(['idxs_rev_0 = [', num2str(constraints.idxs_rev_0(:).'), ... + '] contains value not in range -1:', num2str(ni_no_s-1), '.']); + end + + ns_0 = ns_from_idxs_rev(constraints.idxs_rev_0); + + % slack bounds + if isempty(constraints.ls_0) + constraints.ls_0 = zeros(ns_0, 1); + elseif length(constraints.ls_0) ~= ns_0 + error('inconsistent dimension ns_0, regarding idxs_rev_0, ls_0.'); + end + if isempty(constraints.us_0) + constraints.us_0 = zeros(ns_0, 1); + elseif length(constraints.us_0) ~= ns_0 + error('inconsistent dimension ns_0, regarding idxs_rev_0, us_0.'); + end + + % check cost penalty + fields = {'Zl_0', 'Zu_0', 'zl_0', 'zu_0'}; + for i = 1:length(fields) + field = fields{i}; + dim = size(cost.(field), 1); + if dim ~= ns_0 + error(['Inconsistent size for field ', field, ', with dimension ', num2str(dim), ... + ', Detected ns_0 = ', num2str(ns_0), '.']); + end + end + + dims.ns_0 = ns_0; + end + function make_consistent_slacks_initial(self) constraints = self.constraints; dims = self.dims; @@ -553,6 +656,54 @@ function make_consistent_slacks_initial(self) dims.nsphi_0 = nsphi_0; end + function make_consistent_slacks_rev_terminal(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + + % constraints that are not slack bounds + ni_no_s = dims.nbx_e + dims.nh_e + dims.ng_e + dims.nphi_e; + + % sanity checks + if length(constraints.idxs_rev_e) ~= ni_no_s + error(['inconsistent dimension idxs_rev_e = ', num2str(length(constraints.idxs_rev_e)), ... + '. Should be equal to ni_no_s = ', num2str(ni_no_s), '.']); + end + possible_vals = -1:(ni_no_s-1); + if any(~ismember(constraints.idxs_rev_e, possible_vals)) + error(['idxs_rev_e = [', num2str(constraints.idxs_rev_e(:).'), ... + '] contains value not in range -1:', num2str(ni_no_s-1), '.']); + end + + ns_e = ns_from_idxs_rev(constraints.idxs_rev_e); + + % slack bounds + if isempty(constraints.ls_e) + constraints.ls_e = zeros(ns_e, 1); + elseif length(constraints.ls_e) ~= ns_e + error('inconsistent dimension ns_e, regarding idxs_rev_e, ls_e.'); + end + if isempty(constraints.us_e) + constraints.us_e = zeros(ns_e, 1); + elseif length(constraints.us_e) ~= ns_e + error('inconsistent dimension ns_e, regarding idxs_rev_e, us_e.'); + end + + % check cost penalty + fields = {'Zl_e', 'Zu_e', 'zl_e', 'zu_e'}; + for i = 1:length(fields) + field = fields{i}; + dim = size(cost.(field), 1); + if dim ~= ns_e + error(['Inconsistent size for field ', field, ', with dimension ', num2str(dim), ... + ', Detected ns_e = ', num2str(ns_e), '.']); + end + end + + dims.ns_e = ns_e; + end + + function make_consistent_slacks_terminal(self) constraints = self.constraints; dims = self.dims; @@ -911,10 +1062,46 @@ function make_consistent(self, is_mocp_phase) self.make_consistent_constraints_path(); self.make_consistent_constraints_terminal(); + % if idxs_rev formulation is used at initial or path, no idxs* should be defined + if ~isempty(constraints.idxs_rev_0) || ~isempty(constraints.idxs_rev) + idxs_names = {'idxsbx', 'idxsbu', 'idxsg', 'idxsh_0', 'idxsh', 'idxsphi_0', 'idxsphi'}; + for i = 1:length(idxs_names) + idxs_name = idxs_names{i}; + idxs_val = constraints.(idxs_name); + if ~isempty(idxs_val) + error(['Mixing idxs_rev and idxs_* formulations for initial and intermediate nodes is not supported. Found non empty idxs_rev or idxs_rev_0 and non empty ', idxs_name]); + end + end + end + + % Check idxs_rev formulation compatibility + if any([~isempty(constraints.idxs_rev_0), ~isempty(constraints.idxs_rev), ~isempty(constraints.idxs_rev_e)]) + if isempty(strfind(opts.qp_solver, 'HPIPM')) + error(['idxs_rev formulation is only supported with HPIPM QP solvers yet, got ', opts.qp_solver, '.']); + end + if strcmp(opts.nlp_solver_type, 'SQP_WITH_FEASIBLE_QP') + error('idxs_rev formulation is not compatible with SQP_WITH_FEASIBLE_QP yet.'); + end + end + %% slack dimensions - self.make_consistent_slacks_path(); - self.make_consistent_slacks_initial(); - self.make_consistent_slacks_terminal(); + if isempty(constraints.idxs_rev) + self.make_consistent_slacks_path(); + else + self.make_consistent_slacks_rev_path(); + end + + if isempty(constraints.idxs_rev_0) + self.make_consistent_slacks_initial(); + else + self.make_consistent_slacks_rev_initial(); + end + + if isempty(constraints.idxs_rev_e) + self.make_consistent_slacks_terminal(); + else + self.make_consistent_slacks_rev_terminal(); + end % check for ACADOS_INFTY if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) diff --git a/interfaces/acados_matlab_octave/AcadosOcpConstraints.m b/interfaces/acados_matlab_octave/AcadosOcpConstraints.m index 8583b57653..8f3a572b5f 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpConstraints.m +++ b/interfaces/acados_matlab_octave/AcadosOcpConstraints.m @@ -86,6 +86,19 @@ lphi_e uphi_e + % idxs_rev slack formulation + idxs_rev_0 + idxs_rev + idxs_rev_e + + ls_0 + ls + ls_e + + us_0 + us + us_e + % SLACKS % bounds on slacks corresponding to softened bounds on x and u lsbx % lower bounds on slacks corresponding to soft lower bounds on x @@ -185,6 +198,18 @@ obj.lphi_e = []; obj.uphi_e = []; + % idxs_rev formulation + obj.idxs_rev_0 = []; + obj.idxs_rev = []; + obj.idxs_rev_e = []; + + obj.ls_0 = []; + obj.ls = []; + obj.ls_e = []; + + obj.us_0 = []; + obj.us = []; + obj.us_e = []; % SLACKS obj.lsbx = []; obj.usbx = []; diff --git a/interfaces/acados_matlab_octave/ns_from_idxs_rev.m b/interfaces/acados_matlab_octave/ns_from_idxs_rev.m new file mode 100644 index 0000000000..5d93f5a7d1 --- /dev/null +++ b/interfaces/acados_matlab_octave/ns_from_idxs_rev.m @@ -0,0 +1,7 @@ +function n = ns_from_idxs_rev(idxs_rev) + if isempty(idxs_rev) + n = 0; + else + n = max(idxs_rev) + 1; + end +end diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index beccd442f5..80ea786576 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -47,7 +47,7 @@ from .acados_ocp_iterate import AcadosOcpIterate from .utils import (get_acados_path, format_class_dict, make_object_json_dumpable, render_template, - get_shared_lib_ext, is_column, is_empty, casadi_length, check_if_square, + get_shared_lib_ext, is_column, is_empty, casadi_length, check_if_square, ns_from_idxs_rev, check_casadi_version, ACADOS_INFTY) from .penalty_utils import symmetric_huber_penalty, one_sided_huber_penalty @@ -521,6 +521,45 @@ def _make_consistent_constraints_terminal(self): dims.nr_e = casadi_length(model.con_r_expr_e) + def _make_consistent_slacks_rev_initial(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + if opts.N_horizon == 0: + return + + ni_no_s = dims.nbu + dims.nbx_0 + dims.nh_0 + dims.ng + dims.nphi_0 # constraints that are not slack bounds + + # sanity checks + if constraints.idxs_rev_0.shape[0] != ni_no_s: + raise ValueError(f'inconsistent dimension idxs_rev_0 = {constraints.idxs_rev_0.shape[0]}. Should be equal to ni_no_s = {ni_no_s}.') + possible_vals = np.arange(-1, ni_no_s) + if any([i not in possible_vals for i in constraints.idxs_rev_0]): + raise ValueError(f'idxs_rev_0 = {constraints.idxs_rev_0} contains value not in range -1:{ni_no_s-1}.') + + ns_0 = ns_from_idxs_rev(constraints.idxs_rev_0) + + # slack bounds + if is_empty(constraints.ls_0): + constraints.ls_0 = np.zeros((ns_0,)) + elif constraints.ls_0.shape[0] != ns_0: + raise ValueError('inconsistent dimension ns_0, regarding idxs_rev_0, ls_0.') + if is_empty(constraints.us_0): + constraints.us_0 = np.zeros((ns_0,)) + elif constraints.us_0.shape[0] != ns_0: + raise ValueError('inconsistent dimension ns_0, regarding idxs_rev_0, us_0.') + + # check cost penalty + for field in ("Zl_0", "Zu_0", "zl_0", "zu_0"): + dim = getattr(cost, field).shape[0] + if dim != ns_0: + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + + f'Detected ns_0 = {ns_0}.') + + dims.ns_0 = ns_0 + + def _make_consistent_slacks_initial(self): constraints = self.constraints dims = self.dims @@ -565,6 +604,7 @@ def _make_consistent_slacks_initial(self): # Note: at stage 0 bounds on x are not slacked! ns_0 = nsbu + nsg + nsphi_0 + nsh_0 # NOTE: nsbx not supported at stage 0 + dims.ns_0 = ns_0 if cost.zl_0 is None and cost.zu_0 is None and cost.Zl_0 is None and cost.Zu_0 is None: if ns_0 == 0: @@ -585,10 +625,49 @@ def _make_consistent_slacks_initial(self): for field in ("Zl_0", "Zu_0", "zl_0", "zu_0"): dim = getattr(cost, field).shape[0] if dim != ns_0: - raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') - dims.ns_0 = ns_0 + + + def _make_consistent_slacks_rev_path(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + + if opts.N_horizon == 0: + return + + ni_no_s = dims.nbu + dims.nbx + dims.nh + dims.ng + dims.nphi # constraints that are not slack bounds + + # sanity checks + if constraints.idxs_rev.shape[0] != ni_no_s: + raise ValueError(f'inconsistent dimension idxs_rev = {constraints.idxs_rev.shape[0]}. Should be equal to ni_no_s = {ni_no_s}.') + possible_vals = np.arange(-1, ni_no_s) + if any([i not in possible_vals for i in constraints.idxs_rev]): + raise ValueError(f'idxs_rev = {constraints.idxs_rev} contains value not in range -1:{ni_no_s-1}.') + + ns = ns_from_idxs_rev(constraints.idxs_rev) + + # slack bounds + if is_empty(constraints.ls): + constraints.ls = np.zeros((ns,)) + elif constraints.ls.shape[0] != ns: + raise ValueError('inconsistent dimension ns, regarding idxs_rev, ls.') + if is_empty(constraints.us): + constraints.us = np.zeros((ns,)) + elif constraints.us.shape[0] != ns: + raise ValueError('inconsistent dimension ns, regarding idxs_rev, us.') + + # check cost penalty + for field in ("Zl", "Zu", "zl", "zu"): + dim = getattr(cost, field).shape[0] + if dim != ns: + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + + f'Detected ns = {ns}.') + + dims.ns = ns def _make_consistent_slacks_path(self): @@ -682,12 +761,49 @@ def _make_consistent_slacks_path(self): for field in ("Zl", "Zu", "zl", "zu"): dim = getattr(cost, field).shape[0] if dim != ns: - raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns + + def _make_consistent_slacks_rev_terminal(self): + constraints = self.constraints + dims = self.dims + cost = self.cost + + ni_no_s = dims.nbx_e + dims.nh_e + dims.ng_e + dims.nphi_e # constraints that are not slack bounds + + # sanity checks + if constraints.idxs_rev_e.shape[0] != ni_no_s: + raise ValueError(f'inconsistent dimension idxs_rev_e = {constraints.idxs_rev_e.shape[0]}. Should be equal to ni_no_s = {ni_no_s}.') + possible_vals = np.arange(-1, ni_no_s) + if any([i not in possible_vals for i in constraints.idxs_rev_e]): + raise ValueError(f'idxs_rev_e = {constraints.idxs_rev_e} contains value not in range -1:{ni_no_s-1}.') + + ns_e = ns_from_idxs_rev(constraints.idxs_rev_e) + + # slack bounds + if is_empty(constraints.ls_e): + constraints.ls_e = np.zeros((ns_e,)) + elif constraints.ls_e.shape[0] != ns_e: + raise ValueError('inconsistent dimension ns_e, regarding idxs_rev_e, ls_e.') + if is_empty(constraints.us_e): + constraints.us_e = np.zeros((ns_e,)) + elif constraints.us_e.shape[0] != ns_e: + raise ValueError('inconsistent dimension ns_e, regarding idxs_rev_e, us_e.') + + # check cost penalty + for field in ("Zl_e", "Zu_e", "zl_e", "zu_e"): + dim = getattr(cost, field).shape[0] + if dim != ns_e: + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + + f'Detected ns_e = {ns_e}.') + + dims.ns_e = ns_e + + def _make_consistent_slacks_terminal(self): constraints = self.constraints dims = self.dims @@ -761,7 +877,7 @@ def _make_consistent_slacks_terminal(self): for field in ("Zl_e", "Zu_e", "zl_e", "zu_e"): dim = getattr(cost, field).shape[0] if dim != ns_e: - raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + raise Exception(f'Inconsistent size for field {field}, with dimension {dim}, \n\t'\ + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}.') @@ -934,9 +1050,33 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None self._make_consistent_constraints_path() self._make_consistent_constraints_terminal() - self._make_consistent_slacks_path() - self._make_consistent_slacks_initial() - self._make_consistent_slacks_terminal() + # if idxs_rev formulation is used at initial or path, no idxs* should be defined + if not is_empty(constraints.idxs_rev_0) or not is_empty(constraints.idxs_rev): + for idxs_name in ['idxsbx', 'idxsbu', 'idxsg', 'idxsh_0', 'idxsh', 'idxsphi_0', 'idxsphi']: + idxs_val = getattr(constraints, idxs_name) + if not is_empty(idxs_val): + raise ValueError(f"Mixing idxs_rev and idxs_* formulations for initial and intermediate nodes is not supported. Found non empty idxs_rev or idxs_rev_0 and non empty {idxs_name}") + + if any([not is_empty(idxs_rev) for idxs_rev in [constraints.idxs_rev_0, constraints.idxs_rev, constraints.idxs_rev_e]]): + if not "HPIPM" in opts.qp_solver: + raise ValueError(f"idxs_rev formulation is only supported with HPIPM QP solvers yet, got {opts.qp_solver}.") + if opts.nlp_solver_type == "SQP_WITH_FEASIBLE_QP": + raise ValueError("idxs_rev formulation is not compatible with SQP_WITH_FEASIBLE_QP yet.") + + if is_empty(constraints.idxs_rev): + self._make_consistent_slacks_path() + else: + self._make_consistent_slacks_rev_path() + + if is_empty(constraints.idxs_rev_0): + self._make_consistent_slacks_initial() + else: + self._make_consistent_slacks_rev_initial() + + if is_empty(constraints.idxs_rev_e): + self._make_consistent_slacks_terminal() + else: + self._make_consistent_slacks_rev_terminal() # check for ACADOS_INFTY if opts.qp_solver not in ["PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM", "FULL_CONDENSING_DAQP"]: diff --git a/interfaces/acados_template/acados_template/acados_ocp_constraints.py b/interfaces/acados_template/acados_template/acados_ocp_constraints.py index f04845b45f..01cfc2e16e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_constraints.py +++ b/interfaces/acados_template/acados_template/acados_ocp_constraints.py @@ -34,7 +34,14 @@ class AcadosOcpConstraints: """ - class containing the description of the constraints + Class containing the description of the constraints. + + Soft constraints can be formulated in two ways: + 1) via idxsbu, idxsbx, idxsg, idxsh, idxsphi, lsbu, usbu, lsbx, usbx, lsg, usg, lsh, ush, lsphi, usphi + 2) via idxs_rev, ls, us and *_0, *_e variants + + Option 1) is what was implemented in acados <= 0.5.1 + Option 2) is the new way of formulating soft constraints, which is more flexible, as one slack variable can be used for multiple constraints. """ def __init__(self): self.__constr_type_0 = 'BGH' @@ -83,6 +90,20 @@ def __init__(self): self.__uphi = np.array([]) self.__lphi_e = np.array([]) self.__uphi_e = np.array([]) + + # idxs_rev slack formulation + self.__idxs_rev_0 = np.array([]) + self.__idxs_rev = np.array([]) + self.__idxs_rev_e = np.array([]) + + self.__ls_0 = np.array([]) + self.__ls = np.array([]) + self.__ls_e = np.array([]) + + self.__us_0 = np.array([]) + self.__us = np.array([]) + self.__us_e = np.array([]) + # SLACK BOUNDS # soft bounds on x self.__lsbx = np.array([]) @@ -95,7 +116,7 @@ def __init__(self): # soft bounds on x at shooting node N self.__lsbx_e = np.array([]) self.__usbx_e = np.array([]) - self.__idxsbx_e= np.array([]) + self.__idxsbx_e = np.array([]) # soft bounds on general linear constraints self.__lsg = np.array([]) self.__usg = np.array([]) @@ -440,6 +461,71 @@ def uphi_0(self): return self.__uphi_0 + # SLACK formulation + # idxs_rev slack formulation + @property + def idxs_rev_0(self): + """Indices of slack variables associated with each constraint at initial shooting node 0, zero-based. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__idxs_rev_0 + + @property + def idxs_rev(self): + """Indices of slack variables associated with each constraint at shooting nodes (1 to N-1), zero-based. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__idxs_rev + + @property + def idxs_rev_e(self): + """Indices of slack variables associated with each constraint at terminal shooting node N, zero-based. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__idxs_rev_e + + @property + def ls_0(self): + """Lower bounds on slacks associated with lower bound constraints at initial shooting node 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ls_0 + + @property + def ls(self): + """Lower bounds on slacks associated with lower bound constraints at shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ls + + @property + def ls_e(self): + """Lower bounds on slacks associated with lower bound constraints at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__ls_e + + @property + def us_0(self): + """Lower bounds on slacks associated with upper bound constraints at initial shooting node 0. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__us_0 + + @property + def us(self): + """Lower bounds on slacks associated with upper bound constraints at shooting nodes (1 to N-1). + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__us + + @property + def us_e(self): + """Lower bounds on slacks associated with upper bound constraints at terminal shooting node N. + Type: :code:`np.ndarray`; default: :code:`np.array([])`. + """ + return self.__us_e + # SLACK bounds # soft bounds on x @property @@ -1012,6 +1098,52 @@ def uphi_0(self, value): value = check_if_nparray_and_flatten(value, 'uphi_0') self.__uphi_0 = value + # idxs_rev slack formulation + @idxs_rev_0.setter + def idxs_rev_0(self, idxs_rev_0): + idxs_rev_0 = check_if_nparray_and_flatten(idxs_rev_0, "idxs_rev_0") + self.__idxs_rev_0 = idxs_rev_0 + + @idxs_rev.setter + def idxs_rev(self, idxs_rev): + idxs_rev = check_if_nparray_and_flatten(idxs_rev, "idxs_rev") + self.__idxs_rev = idxs_rev + + @idxs_rev_e.setter + def idxs_rev_e(self, idxs_rev_e): + idxs_rev_e = check_if_nparray_and_flatten(idxs_rev_e, "idxs_rev_e") + self.__idxs_rev_e = idxs_rev_e + + @ls_0.setter + def ls_0(self, ls_0): + ls_0 = check_if_nparray_and_flatten(ls_0, "ls_0") + self.__ls_0 = ls_0 + + @ls.setter + def ls(self, ls): + ls = check_if_nparray_and_flatten(ls, "ls") + self.__ls = ls + + @ls_e.setter + def ls_e(self, ls_e): + ls_e = check_if_nparray_and_flatten(ls_e, "ls_e") + self.__ls_e = ls_e + + @us_0.setter + def us_0(self, us_0): + us_0 = check_if_nparray_and_flatten(us_0, "us_0") + self.__us_0 = us_0 + + @us.setter + def us(self, us): + us = check_if_nparray_and_flatten(us, "us") + self.__us = us + + @us_e.setter + def us_e(self, us_e): + us_e = check_if_nparray_and_flatten(us_e, "us_e") + self.__us_e = us_e + # SLACK bounds # soft bounds on x @lsbx.setter 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 9d1b2f64f7..589dc39481 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 @@ -1148,6 +1148,36 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(luphi_0); {% endif %} + +{% set_global n_idxs_rev_0 = constraints_0.idxs_rev_0 | length %} +{% if n_idxs_rev_0 > 0 %}{# idxs* formulation initial #} + + {% set ni_no_s = dims_0.nbu + dims_0.nbx_0 + dims_0.ng + dims_0.nh_0 + dims_0.nphi_0 %} + int* idxs_rev_0 = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev_0[{{ i }}] = {{ constraints_0.idxs_rev_0[i] }}; + {%- endfor %} + + double* lus_0 = calloc(2*NS0, sizeof(double)); + double* ls_0 = lus_0; + double* us_0 = lus_0 + NS0; + {%- for i in range(end=dims_0.ns_0) %} + {%- if constraints_0.ls_0[i] != 0 %} + ls_0[{{ i }}] = {{ constraints_0.ls_0[i] }}; + {%- endif %} + {%- if constraints_0.us_0[i] != 0 %} + us_0[{{ i }}] = {{ constraints_0.us_0[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxs_rev", idxs_rev_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ls", ls_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "us", us_0); + free(idxs_rev_0); + free(lus_0); + +{% else %}{# idxs* formulation initial #} + {% if dims_0.nsh_0 > 0 %} // set up soft bounds for nonlinear constraints int* idxsh_0 = malloc({{ dims_0.nsh_0 }} * sizeof(int)); @@ -1197,6 +1227,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(idxsphi_0); free(lusphi_0); {%- endif %} +{%- endif %}{# idxs* formulation initial #} /* Path related delarations */ int i_fun; @@ -1733,6 +1764,37 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(luphi); {%- endif %} +{% set_global n_idxs_rev = constraints[jj].idxs_rev | length %} +{% if n_idxs_rev > 0 %} + {% set ni_no_s = phases_dims[jj].nbu + phases_dims[jj].nbx + phases_dims[jj].ng + phases_dims[jj].nh + phases_dims[jj].nphi %} + int* idxs_rev = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev[{{ i }}] = {{ constraints[jj].idxs_rev[i] }}; + {%- endfor %} + + double* lus = calloc(2*NS0, sizeof(double)); + double* ls = lus; + double* us = lus + NS0; + {%- for i in range(end=phases_dims[jj].ns) %} + {%- if constraints[jj].ls[i] != 0 %} + ls[{{ i }}] = {{ constraints[jj].ls[i] }}; + {%- endif %} + {%- if constraints[jj].us[i] != 0 %} + us[{{ i }}] = {{ constraints[jj].us[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxs_rev", idxs_rev); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ls", ls); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "us", us); + } + free(idxs_rev); + free(lus); + +{% else %}{# idxs* formulation intermediate #} + {%- if phases_dims[jj].nsbx > 0 %} {# TODO: introduce nsbx0 #} @@ -1824,7 +1886,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(idxsphi); free(lusphi); {%- endif %} - +{%- endif %}{# idxs* formulation intermediate #} {%- endfor %}{# phases loop !#} @@ -1962,105 +2024,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i free(lubx_e); {%- endif %} -{% if dims_e.nsg_e > 0 %} - // set up soft bounds for general linear constraints - int* idxsg_e = calloc({{ dims_e.nsg_e }}, sizeof(int)); - {%- for i in range(end=dims_e.nsg_e) %} - idxsg_e[{{ i }}] = {{ constraints_e.idxsg_e[i] }}; - {%- endfor %} - double* lusg_e = calloc(2*{{ dims_e.nsg_e }}, sizeof(double)); - double* lsg_e = lusg_e; - double* usg_e = lusg_e + {{ dims_e.nsg_e }}; - {%- for i in range(end=dims_e.nsg_e) %} - {%- if constraints_e.lsg_e[i] != 0 %} - lsg_e[{{ i }}] = {{ constraints_e.lsg_e[i] }}; - {%- endif %} - {%- if constraints_e.usg_e[i] != 0 %} - usg_e[{{ i }}] = {{ constraints_e.usg_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); - free(idxsg_e); - free(lusg_e); -{%- endif %} - -{% if dims_e.nsh_e > 0 %} - // set up soft bounds for nonlinear constraints - int* idxsh_e = malloc({{ dims_e.nsh_e }} * sizeof(int)); - {%- for i in range(end=dims_e.nsh_e) %} - idxsh_e[{{ i }}] = {{ constraints_e.idxsh_e[i] }}; - {%- endfor %} - double* lush_e = calloc(2*{{ dims_e.nsh_e }}, sizeof(double)); - double* lsh_e = lush_e; - double* ush_e = lush_e + {{ dims_e.nsh_e }}; - {%- for i in range(end=dims_e.nsh_e) %} - {%- if constraints_e.lsh_e[i] != 0 %} - lsh_e[{{ i }}] = {{ constraints_e.lsh_e[i] }}; - {%- endif %} - {%- if constraints_e.ush_e[i] != 0 %} - ush_e[{{ i }}] = {{ constraints_e.ush_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); - free(idxsh_e); - free(lush_e); -{%- endif %} - -{% if dims_e.nsphi_e > 0 %} - // set up soft bounds for convex-over-nonlinear constraints - int* idxsphi_e = malloc({{ dims_e.nsphi_e }} * sizeof(int)); - {%- for i in range(end=dims_e.nsphi_e) %} - idxsphi_e[{{ i }}] = {{ constraints_e.idxsphi_e[i] }}; - {%- endfor %} - double* lusphi_e = calloc(2*{{ dims_e.nsphi_e }}, sizeof(double)); - double* lsphi_e = lusphi_e; - double* usphi_e = lusphi_e + {{ dims_e.nsphi_e }}; - {%- for i in range(end=dims_e.nsphi_e) %} - {%- if constraints_e.lsphi_e[i] != 0 %} - lsphi_e[{{ i }}] = {{ constraints_e.lsphi_e[i] }}; - {%- endif %} - {%- if constraints_e.usphi_e[i] != 0 %} - usphi_e[{{ i }}] = {{ constraints_e.usphi_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); - free(idxsphi_e); - free(lusphi_e); -{%- endif %} - -{% if dims_e.nsbx_e > 0 %} - // soft bounds on x - int* idxsbx_e = malloc({{ dims_e.nsbx_e }} * sizeof(int)); - {%- for i in range(end=dims_e.nsbx_e) %} - idxsbx_e[{{ i }}] = {{ constraints_e.idxsbx_e[i] }}; - {%- endfor %} - double* lusbx_e = calloc(2*{{ dims_e.nsbx_e }}, sizeof(double)); - double* lsbx_e = lusbx_e; - double* usbx_e = lusbx_e + {{ dims_e.nsbx_e }}; - {%- for i in range(end=dims_e.nsbx_e) %} - {%- if constraints_e.lsbx_e[i] != 0 %} - lsbx_e[{{ i }}] = {{ constraints_e.lsbx_e[i] }}; - {%- endif %} - {%- if constraints_e.usbx_e[i] != 0 %} - usbx_e[{{ i }}] = {{ constraints_e.usbx_e[i] }}; - {%- endif %} - {%- endfor %} - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); - free(idxsbx_e); - free(lusbx_e); -{% endif %} {% if dims_e.ng_e > 0 %} // set up general constraints for last stage @@ -2148,6 +2111,139 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint_fun_jac_hess); free(luphi_e); {% endif %} + + /* terminal soft constraints */ +{% set_global n_idxs_rev_e = constraints_e.idxs_rev_e | length %} +{% if n_idxs_rev_e > 0 %} + + {% set ni_no_s = dims_e.nbx_e + dims_e.ng_e + dims_e.nh_e + dims_e.nphi_e %} + int* idxs_rev_e = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev_e[{{ i }}] = {{ constraints_e.idxs_rev_e[i] }}; + {%- endfor %} + + double* lus_e = calloc(2*NSN, sizeof(double)); + double* ls_e = lus_e; + double* us_e = lus_e + NSN; + {%- for i in range(end=dims_e.ns_e) %} + {%- if constraints_e.ls_e[i] != 0 %} + ls_e[{{ i }}] = {{ constraints_e.ls_e[i] }}; + {%- endif %} + {%- if constraints_e.us_e[i] != 0 %} + us_e[{{ i }}] = {{ constraints_e.us_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxs_rev", idxs_rev_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ls", ls_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "us", us_e); + free(idxs_rev_e); + free(lus_e); + +{% else %}{# idxs* formulation #} + +{% if dims_e.nsbx_e > 0 %} + // soft bounds on x + int* idxsbx_e = malloc({{ dims_e.nsbx_e }} * sizeof(int)); + {%- for i in range(end=dims_e.nsbx_e) %} + idxsbx_e[{{ i }}] = {{ constraints_e.idxsbx_e[i] }}; + {%- endfor %} + double* lusbx_e = calloc(2*{{ dims_e.nsbx_e }}, sizeof(double)); + double* lsbx_e = lusbx_e; + double* usbx_e = lusbx_e + {{ dims_e.nsbx_e }}; + {%- for i in range(end=dims_e.nsbx_e) %} + {%- if constraints_e.lsbx_e[i] != 0 %} + lsbx_e[{{ i }}] = {{ constraints_e.lsbx_e[i] }}; + {%- endif %} + {%- if constraints_e.usbx_e[i] != 0 %} + usbx_e[{{ i }}] = {{ constraints_e.usbx_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); + free(idxsbx_e); + free(lusbx_e); +{% endif %} + +{% if dims_e.nsg_e > 0 %} + // set up soft bounds for general linear constraints + int* idxsg_e = calloc({{ dims_e.nsg_e }}, sizeof(int)); + {%- for i in range(end=dims_e.nsg_e) %} + idxsg_e[{{ i }}] = {{ constraints_e.idxsg_e[i] }}; + {%- endfor %} + double* lusg_e = calloc(2*{{ dims_e.nsg_e }}, sizeof(double)); + double* lsg_e = lusg_e; + double* usg_e = lusg_e + {{ dims_e.nsg_e }}; + {%- for i in range(end=dims_e.nsg_e) %} + {%- if constraints_e.lsg_e[i] != 0 %} + lsg_e[{{ i }}] = {{ constraints_e.lsg_e[i] }}; + {%- endif %} + {%- if constraints_e.usg_e[i] != 0 %} + usg_e[{{ i }}] = {{ constraints_e.usg_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); + free(idxsg_e); + free(lusg_e); +{%- endif %} + +{% if dims_e.nsh_e > 0 %} + // set up soft bounds for nonlinear constraints + int* idxsh_e = malloc({{ dims_e.nsh_e }} * sizeof(int)); + {%- for i in range(end=dims_e.nsh_e) %} + idxsh_e[{{ i }}] = {{ constraints_e.idxsh_e[i] }}; + {%- endfor %} + double* lush_e = calloc(2*{{ dims_e.nsh_e }}, sizeof(double)); + double* lsh_e = lush_e; + double* ush_e = lush_e + {{ dims_e.nsh_e }}; + {%- for i in range(end=dims_e.nsh_e) %} + {%- if constraints_e.lsh_e[i] != 0 %} + lsh_e[{{ i }}] = {{ constraints_e.lsh_e[i] }}; + {%- endif %} + {%- if constraints_e.ush_e[i] != 0 %} + ush_e[{{ i }}] = {{ constraints_e.ush_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); + free(idxsh_e); + free(lush_e); +{%- endif %} + +{% if dims_e.nsphi_e > 0 %} + // set up soft bounds for convex-over-nonlinear constraints + int* idxsphi_e = malloc({{ dims_e.nsphi_e }} * sizeof(int)); + {%- for i in range(end=dims_e.nsphi_e) %} + idxsphi_e[{{ i }}] = {{ constraints_e.idxsphi_e[i] }}; + {%- endfor %} + double* lusphi_e = calloc(2*{{ dims_e.nsphi_e }}, sizeof(double)); + double* lsphi_e = lusphi_e; + double* usphi_e = lusphi_e + {{ dims_e.nsphi_e }}; + {%- for i in range(end=dims_e.nsphi_e) %} + {%- if constraints_e.lsphi_e[i] != 0 %} + lsphi_e[{{ i }}] = {{ constraints_e.lsphi_e[i] }}; + {%- endif %} + {%- if constraints_e.usphi_e[i] != 0 %} + usphi_e[{{ i }}] = {{ constraints_e.usphi_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); + free(idxsphi_e); + free(lusphi_e); +{%- endif %} + +{% endif %}{# idxs* formulation #} + } 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 8a89d2cd55..24a85a6a12 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 @@ -1594,6 +1594,36 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(luphi_0); {% endif %} + +{% set_global n_idxs_rev_0 = constraints.idxs_rev_0 | length %} +{% if n_idxs_rev_0 > 0 %}{# idxs* formulation initial #} + + {% set ni_no_s = dims.nbu + dims.nbx_0 + dims.ng + dims.nh_0 + dims.nphi_0 %} + int* idxs_rev_0 = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev_0[{{ i }}] = {{ constraints.idxs_rev_0[i] }}; + {%- endfor %} + + double* lus_0 = calloc(2*NS0, sizeof(double)); + double* ls_0 = lus_0; + double* us_0 = lus_0 + NS0; + {%- for i in range(end=dims.ns_0) %} + {%- if constraints.ls_0[i] != 0 %} + ls_0[{{ i }}] = {{ constraints.ls_0[i] }}; + {%- endif %} + {%- if constraints.us_0[i] != 0 %} + us_0[{{ i }}] = {{ constraints.us_0[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxs_rev", idxs_rev_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ls", ls_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "us", us_0); + free(idxs_rev_0); + free(lus_0); + +{% else %}{# idxs* formulation initial #} + {% if dims.nsh_0 > 0 %} // set up soft bounds for nonlinear constraints int* idxsh_0 = malloc(NSH0 * sizeof(int)); @@ -1643,6 +1673,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(idxsphi_0); free(lusphi_0); {%- endif %} +{%- endif %}{# idxs* formulation initial #} /* constraints that are the same for initial and intermediate */ {%- if dims.nbu > 0 %} @@ -1878,6 +1909,37 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(luphi); {%- endif %} + +{% set_global n_idxs_rev = constraints.idxs_rev | length %} +{% if n_idxs_rev > 0 %} + {% set ni_no_s = dims.nbu + dims.nbx + dims.ng + dims.nh + dims.nphi %} + int* idxs_rev = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev[{{ i }}] = {{ constraints.idxs_rev[i] }}; + {%- endfor %} + + double* lus = calloc(2*NS0, sizeof(double)); + double* ls = lus; + double* us = lus + NS0; + {%- for i in range(end=dims.ns) %} + {%- if constraints.ls[i] != 0 %} + ls[{{ i }}] = {{ constraints.ls[i] }}; + {%- endif %} + {%- if constraints.us[i] != 0 %} + us[{{ i }}] = {{ constraints.us[i] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxs_rev", idxs_rev); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ls", ls); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "us", us); + } + free(idxs_rev); + free(lus); + +{% else %}{# idxs* formulation intermediate #} {%- if dims.nsbx > 0 %} {# TODO: introduce nsbx0 #} // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); @@ -1967,6 +2029,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(idxsphi); free(lusphi); {%- endif %} +{%- endif %}{# idxs* formulation intermediate #} {%- endif %}{# solver_options.N_horizon > 0 #} /* terminal constraints */ @@ -2088,6 +2151,35 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu /* terminal soft constraints */ {% endif %} +{% set_global n_idxs_rev_e = constraints.idxs_rev_e | length %} +{% if n_idxs_rev_e > 0 %} + + {% set ni_no_s = dims.nbx_e + dims.ng_e + dims.nh_e + dims.nphi_e %} + int* idxs_rev_e = malloc( {{ ni_no_s }} * sizeof(int)); + {%- for i in range(end=ni_no_s) %} + idxs_rev_e[{{ i }}] = {{ constraints.idxs_rev_e[i] }}; + {%- endfor %} + + double* lus_e = calloc(2*NSN, sizeof(double)); + double* ls_e = lus_e; + double* us_e = lus_e + NSN; + {%- for i in range(end=dims.ns_e) %} + {%- if constraints.ls_e[i] != 0 %} + ls_e[{{ i }}] = {{ constraints.ls_e[i] }}; + {%- endif %} + {%- if constraints.us_e[i] != 0 %} + us_e[{{ i }}] = {{ constraints.us_e[i] }}; + {%- endif %} + {%- endfor %} + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxs_rev", idxs_rev_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ls", ls_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "us", us_e); + free(idxs_rev_e); + free(lus_e); + +{% else %}{# idxs* formulation #} + {% if dims.nsg_e > 0 %} // set up soft bounds for general linear constraints int* idxsg_e = calloc(NSGN, sizeof(int)); @@ -2187,6 +2279,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu free(idxsbx_e); free(lusbx_e); {% endif %} +{% endif %}{# idxs* formulation #} } diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 22b210b60b..f5129d0c3e 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -424,6 +424,11 @@ def J_to_idx_slack(J): return idx +def ns_from_idxs_rev(idxs_rev) -> int: + if is_empty(idxs_rev): + return 0 + return int(np.max(idxs_rev) + 1) + def check_if_nparray_and_flatten(val, name) -> np.ndarray: if not isinstance(val, np.ndarray): raise TypeError(f"{name} must be a numpy array, got {type(val)}") From ef04217501df5fe6b457dfeda9a9886852e56b2d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 21 Aug 2025 14:20:50 +0200 Subject: [PATCH 124/164] Python: add getters for partial condensing fields (#1612) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Tilo Schröder <19371135+tilo-schroeder@users.noreply.github.com> --- .../acados_python/tests/pcond_getters_test.py | 109 +++++++++++++++ interfaces/CMakeLists.txt | 4 + interfaces/acados_c/ocp_nlp_interface.c | 128 ++++++++++++++++++ .../acados_template/acados_ocp_solver.py | 13 +- 4 files changed, 250 insertions(+), 4 deletions(-) create mode 100644 examples/acados_python/tests/pcond_getters_test.py diff --git a/examples/acados_python/tests/pcond_getters_test.py b/examples/acados_python/tests/pcond_getters_test.py new file mode 100644 index 0000000000..af4d6ab67e --- /dev/null +++ b/examples/acados_python/tests/pcond_getters_test.py @@ -0,0 +1,109 @@ +import os +import numpy as np +import casadi as ca +from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver + + + +if __name__ == "__main__": + N = 40 + Tf = 10.0 + cond_N = 10 + + # Problem data + nx, nu = 2, 1 + Q = np.diag([5.0, 1.0]) + R = np.diag([0.2]) + x0 = np.zeros(nx) + x_ref = np.array([10.0, 0.0]) + lbu = np.array([-5.5]) + ubu = np.array([5.5]) + + # Build OCP + ocp = AcadosOcp() + ocp.solver_options.tf = Tf + ocp.solver_options.N_horizon = N + + ocp.model = AcadosModel() + ocp.model.name = "double_integrator" + + s, v = ca.SX.sym("s"), ca.SX.sym("v") + a = ca.SX.sym("a") + x = ca.vertcat(s, v) + u = ca.vertcat(a) + xdot = ca.vertcat(v, a) + + ocp.model.x = x + ocp.model.u = u + ocp.model.xdot = xdot + ocp.model.f_expl_expr = xdot + ocp.model.f_impl_expr = xdot + + # Bounds + ocp.constraints.x0 = x0 + ocp.constraints.idxbu = np.array([0], dtype=int) + ocp.constraints.lbu = lbu + ocp.constraints.ubu = ubu + + # Linear least-squares cost + ny = nx + nu + W = np.zeros((ny, ny)) + W[:nx, :nx] = Q + W[nx:, nx:] = R + + ocp.cost.cost_type = "LINEAR_LS" + ocp.cost.cost_type_0 = "LINEAR_LS" + ocp.cost.cost_type_e = "LINEAR_LS" + ocp.cost.W = W + ocp.cost.W_0 = W + ocp.cost.W_e = Q + + ocp.cost.Vx = np.vstack((np.eye(nx), np.zeros((nu, nx)))) + ocp.cost.Vx_0 = np.vstack((np.eye(nx), np.zeros((nu, nx)))) + ocp.cost.Vx_e = np.eye(nx) + ocp.cost.Vu = np.vstack((np.zeros((nx, nu)), np.eye(nu))) + ocp.cost.Vu_0 = np.vstack((np.zeros((nx, nu)), np.eye(nu))) + + ocp.cost.yref = np.hstack((x_ref, np.zeros(nu))) + ocp.cost.yref_0 = np.hstack((x_ref, np.zeros(nu))) + ocp.cost.yref_e = x_ref + + # Solver options + ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" + ocp.solver_options.qp_solver_cond_N = cond_N + ocp.solver_options.hessian_approx = "EXACT" + ocp.solver_options.reg_epsilon = 0.0 + ocp.solver_options.nlp_solver_type = "SQP_RTI" + + ocp.solver_options.qp_solver_cond_N = cond_N + + ocp.solver_options.qp_solver_cond_block_size = (cond_N) * [N // ((cond_N))] + [N - (cond_N * (N // cond_N))] + # ocp.solver_options.qp_solver_cond_block_size = (cond_N) * [1] + [N-((cond_N))] + + uniq = f"double_integrator_{os.getpid()}" + ocp.code_export_directory = f"c_generated_code/{uniq}" + json_path = f".solver_files/ACADOS_SOLVER_{uniq}.json" + + os.makedirs(os.path.dirname(ocp.code_export_directory), exist_ok=True) + os.makedirs(os.path.dirname(json_path), exist_ok=True) + + solver = AcadosOcpSolver(ocp, json_file=json_path) + + solver.solve() + cond_N = ocp.solver_options.qp_solver_cond_N + + dyn_fields = ["A", "B", "b"] + cost_constr_fields = ["Q", "R", "S", "q", "r", "C", "D", "lg", "ug", "lbx", "ubx", "lbu", "ubu"] + fields = dyn_fields + cost_constr_fields + qp = {"N": cond_N, "stages": []} + for k in range(cond_N + 1): + stage = {} + for field in fields: + if not (k == cond_N and field in dyn_fields): + arr = solver.get_from_qp_in(k, "pcond_" + field) + stage[field] = np.array(arr).tolist() + qp["stages"].append(stage) + + + from pprint import pprint + pprint(qp["stages"]) diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 4e611b99b2..167026eabd 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -316,6 +316,10 @@ add_test(NAME python_pendulum_ocp_example_cmake COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests python armijo_test.py) + add_test(NAME python_pcond_getters_test + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests + python pcond_getters_test.py) + # Test NaN in globalization add_test(NAME python_test_nan_globalization COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 2d53038a15..cc2432e49d 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1140,6 +1140,56 @@ void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, o config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[0]); config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); } + else if (!strcmp(field, "pcond_A")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage+1, "pcond_nx", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); + } + else if (!strcmp(field, "pcond_B")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage+1, "pcond_nx", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[1]); + } + else if (!strcmp(field, "pcond_b")) + { + dims_out[0] = 1; + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage+1, "pcond_nx", &dims_out[1]); + } + else if (!strcmp(field, "pcond_q")) + { + dims_out[0] = 1; + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); + } + else if (!strcmp(field, "pcond_r")) + { + dims_out[0] = 1; + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[1]); + } + else if (!strcmp(field, "pcond_C")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_ng", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nx", &dims_out[1]); + } + else if (!strcmp(field, "pcond_D")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_ng", &dims_out[0]); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nu", &dims_out[1]); + } + else if (!strcmp(field, "pcond_lg") || !strcmp(field, "pcond_ug")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_ng", &dims_out[0]); + dims_out[1] = 1; + } + else if (!strcmp(field, "pcond_lbx") || !strcmp(field, "pcond_ubx")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nbx", &dims_out[0]); + dims_out[1] = 1; + } + else if (!strcmp(field, "pcond_lbu") || !strcmp(field, "pcond_ubu")) + { + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, stage, "pcond_nbu", &dims_out[0]); + dims_out[1] = 1; + } else { printf("\nerror: ocp_nlp_qp_dims_get_from_attr: field %s not available\n", field); @@ -1611,6 +1661,84 @@ void ocp_nlp_get_at_stage(ocp_nlp_solver *solver, int stage, const char *field, ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); d_ocp_qp_get_S(stage, pcond_qp_in, value); } + else if (!strcmp(field, "pcond_A")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_A(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_B")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_B(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_b")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_b(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_q")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_q(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_r")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_r(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_C")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_C(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_D")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_D(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_lg")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_lg(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_ug")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_ug(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_lbx")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_lbx(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_ubx")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_ubx(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_lbu")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_lbu(stage, pcond_qp_in, value); + } + else if (!strcmp(field, "pcond_ubu")) + { + ocp_qp_in *pcond_qp_in; + ocp_nlp_get(solver, "qp_xcond_in", &pcond_qp_in); + d_ocp_qp_get_ubu(stage, pcond_qp_in, value); + } else { char *ptr_module = NULL; diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index b0dbbffc19..c2b5dc6da4 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -305,7 +305,7 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__qp_constraint_fields = {'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'} self.__qp_constraint_int_fields = {'idxs', 'idxb', 'idxs_rev'} self.__qp_pc_hpipm_fields = {'P', 'K', 'Lr', 'p'} - self.__qp_pc_fields = {'pcond_Q', 'pcond_R', 'pcond_S'} + self.__qp_pc_fields = {'pcond_Q', 'pcond_R', 'pcond_S', 'pcond_A', 'pcond_B', 'pcond_b', 'pcond_q', 'pcond_r', 'pcond_C', 'pcond_D', 'pcond_lg', 'pcond_ug', 'pcond_lbx', 'pcond_ubx', 'pcond_lbu', 'pcond_ubu'} self.__all_qp_fields = self.__qp_dynamics_fields | self.__qp_cost_fields | self.__qp_constraint_fields | self.__qp_constraint_int_fields | self.__qp_pc_hpipm_fields | self.__qp_pc_fields self.__relaxed_qp_dynamics_fields = {f'relaxed_{field}' for field in self.__qp_dynamics_fields} @@ -2071,7 +2071,7 @@ def get_from_qp_in(self, stage_: int, field_: str): Note: - additional supported fields are ['P', 'K', 'Lr'], which can be extracted form QP solver PARTIAL_CONDENSING_HPIPM. - - for PARTIAL_CONDENSING_* QP solvers, the following additional fields are available: ['pcond_Q', 'pcond_R', 'pcond_S'] + - for PARTIAL_CONDENSING_* QP solvers, the following additional fields are available: ['pcond_Q', 'pcond_R', 'pcond_S', 'pcond_A', 'pcond_B', 'pcond_b', 'pcond_q', 'pcond_r', 'pcond_C', 'pcond_D', 'pcond_lg', 'pcond_ug', 'pcond_lbx', 'pcond_ubx', 'pcond_lbu', 'pcond_ubu'] """ if not isinstance(stage_, int): raise TypeError("stage should be int") @@ -2086,8 +2086,13 @@ def get_from_qp_in(self, stage_: int, field_: str): raise ValueError(f"field {field_} only works for PARTIAL_CONDENSING_HPIPM QP solver with qp_solver_cond_N == N.") if field_ in ["P", "K", "p"] and stage_ == 0 and self.__nbxe_0 > 0: raise ValueError(f"getting field {field_} at stage 0 only works without x0 elimination (see nbxe_0).") - if field_ in self.__qp_pc_fields and not self.__solver_options["qp_solver"].startswith("PARTIAL_CONDENSING"): - raise ValueError(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") + if field_ in self.__qp_pc_fields: + if not self.__solver_options["qp_solver"].startswith("PARTIAL_CONDENSING"): + raise ValueError(f"field {field_} only works for PARTIAL_CONDENSING QP solvers.") + if field_.split("_", 1)[1] in self.__qp_dynamics_fields and stage_ >= self.__solver_options["qp_solver_cond_N"]: + raise ValueError(f"dynamics field {field_} not available at last stage of partial condensing") + elif stage_ > self.__solver_options["qp_solver_cond_N"]: + raise ValueError(f"stage should be <= qp_solver_cond_N for partial condensing fields") if field_ in self.__all_relaxed_qp_fields and not self.__solver_options["nlp_solver_type"] == "SQP_WITH_FEASIBLE_QP": raise ValueError(f"field {field_} only works for SQP_WITH_FEASIBLE_QP nlp_solver_type.") From c9706300e36d5f5a3ef7c0d5d9b135502b7deca3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 26 Aug 2025 15:18:16 +0200 Subject: [PATCH 125/164] Partial condensing interface followup (#1615) - output full matrix for `pcond_Q`, `pcond_R`, before was just lower triagonal - improve example --- .../acados_python/tests/pcond_getters_test.py | 111 +++++++++++++----- .../acados_template/acados_ocp_solver.py | 2 +- 2 files changed, 83 insertions(+), 30 deletions(-) diff --git a/examples/acados_python/tests/pcond_getters_test.py b/examples/acados_python/tests/pcond_getters_test.py index af4d6ab67e..0420a561fa 100644 --- a/examples/acados_python/tests/pcond_getters_test.py +++ b/examples/acados_python/tests/pcond_getters_test.py @@ -1,23 +1,64 @@ import os import numpy as np import casadi as ca -from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver - - - -if __name__ == "__main__": - N = 40 +from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, latexify_plot +import matplotlib.pyplot as plt + + +def plot_qp_sparsity(qp, fig_filename=None, title=None, with_legend=True): + latexify_plot() + + plt.figure(figsize=(4.0, 4.0)) + nx_total = 0 + nu_total = 0 + for stage in qp["stages"]: + # cost Hessian + nx = stage["Q"].shape[0] + nu = stage["R"].shape[0] + nx_total += nx + nu_total += nu + nv_total = nx_total + nu_total + + H_x = np.zeros((nv_total, nv_total)) + H_u = np.zeros((nv_total, nv_total)) + H_xu = np.zeros((nv_total, nv_total)) + + offset = 0 + for stage in qp["stages"]: + nx = stage["Q"].shape[0] + nu = stage["R"].shape[0] + + H_x[offset:offset + nx, offset:offset + nx] = stage["Q"] + H_u[offset + nx:offset + nx + nu, offset + nx:offset + nx + nu] = stage["R"] + if nx > 0 and nu > 0: + H_xu[offset+nx:offset + nx + nu, offset:offset + nx] = stage["S"] + H_xu[offset:offset + nx, offset+nx:offset + nx + nu] = stage["S"].T + offset += nx + nu + plt.spy(H_xu, markersize=5, label='$S$', color='C2') + plt.spy(H_x, markersize=5, label='$Q$', color='C0') + plt.spy(H_u, markersize=5, label='$R$', color='C1') + # remove xticks + plt.xticks([]) + if with_legend: + plt.legend(handletextpad=0.3) + if title is not None: + plt.title(title) + if fig_filename is not None: + plt.savefig(fig_filename, bbox_inches='tight') + plt.show() + +def main(N=20, cond_N=10, qp_solver_cond_block_size=None, x0_elimination=True, fig_title=None, with_legend=True, fig_filename=None): + N = 20 Tf = 10.0 - cond_N = 10 # Problem data - nx, nu = 2, 1 - Q = np.diag([5.0, 1.0]) - R = np.diag([0.2]) + nx, nu = 5, 2 + Q = np.diag([float(i+1) for i in range(nx)]) + R = np.diag([float(i+1) for i in range(nu)]) x0 = np.zeros(nx) - x_ref = np.array([10.0, 0.0]) - lbu = np.array([-5.5]) - ubu = np.array([5.5]) + x_ref = np.ones(nx) + lbu = -np.ones(nu) + ubu = -lbu # Build OCP ocp = AcadosOcp() @@ -25,23 +66,26 @@ ocp.solver_options.N_horizon = N ocp.model = AcadosModel() - ocp.model.name = "double_integrator" - s, v = ca.SX.sym("s"), ca.SX.sym("v") - a = ca.SX.sym("a") - x = ca.vertcat(s, v) - u = ca.vertcat(a) - xdot = ca.vertcat(v, a) + A = np.eye(nx) + 0.1 * np.ones((nx, nx)) + B = np.ones((nx, nu)) + u = ca.SX.sym('u', nu, 1) + x = ca.SX.sym('x', nx, 1) + f_expl_expr = A @ x + B @ u ocp.model.x = x ocp.model.u = u - ocp.model.xdot = xdot - ocp.model.f_expl_expr = xdot - ocp.model.f_impl_expr = xdot + ocp.model.name = 'model' + ocp.model.f_expl_expr = f_expl_expr # Bounds - ocp.constraints.x0 = x0 - ocp.constraints.idxbu = np.array([0], dtype=int) + if x0_elimination: + ocp.constraints.x0 = x0 + else: + ocp.constraints.lbx_0 = x0 + ocp.constraints.ubx_0 = x0 + ocp.constraints.idxbx_0 = np.arange(nx) + ocp.constraints.idxbu = np.arange(nu) ocp.constraints.lbu = lbu ocp.constraints.ubu = ubu @@ -50,6 +94,9 @@ W = np.zeros((ny, ny)) W[:nx, :nx] = Q W[nx:, nx:] = R + S = 1e-3 * np.ones((nx, nu)) + W[:nx, nx:] = S + W[nx:, :nx] = S.T ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_0 = "LINEAR_LS" @@ -77,8 +124,8 @@ ocp.solver_options.qp_solver_cond_N = cond_N - ocp.solver_options.qp_solver_cond_block_size = (cond_N) * [N // ((cond_N))] + [N - (cond_N * (N // cond_N))] - # ocp.solver_options.qp_solver_cond_block_size = (cond_N) * [1] + [N-((cond_N))] + if qp_solver_cond_block_size is not None: + ocp.solver_options.qp_solver_cond_block_size = qp_solver_cond_block_size uniq = f"double_integrator_{os.getpid()}" ocp.code_export_directory = f"c_generated_code/{uniq}" @@ -101,9 +148,15 @@ for field in fields: if not (k == cond_N and field in dyn_fields): arr = solver.get_from_qp_in(k, "pcond_" + field) - stage[field] = np.array(arr).tolist() + stage[field] = np.array(arr) qp["stages"].append(stage) + plot_qp_sparsity(qp, fig_filename=fig_filename, title=fig_title, with_legend=with_legend) + +if __name__ == "__main__": + # main(N=20, cond_N=20, x0_elimination=False, fig_title="Original QP $N=20$ -- no condensing", fig_filename="sparsity_no_condensing.pdf") + # main(N=20, cond_N=1, with_legend=False, fig_title="Condensing, $N_{\mathrm{cond}}=1$ (fully condensed)", fig_filename="sparsity_fcond.pdf") + # main(N=20, cond_N=5, with_legend=False, fig_title="Partial condensing, $N_{\mathrm{cond}}=5$", fig_filename="sparsity_pcond.pdf") + # main(N=20, cond_N=5, qp_solver_cond_block_size=[6, 5, 4, 3, 2, 0], with_legend=False, fig_title="Condensing, $N_{\mathrm{cond}}=5$, custom block sizes", fig_filename="sparsity_pcond_custom_block_sizes.pdf") - from pprint import pprint - pprint(qp["stages"]) + main(N=20, cond_N=5, qp_solver_cond_block_size=[6, 5, 4, 2, 2, 1]) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index c2b5dc6da4..ba591650ea 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -2119,7 +2119,7 @@ def get_from_qp_in(self, stage_: int, field_: str): # call getter self.__acados_lib.ocp_nlp_get_at_stage(self.nlp_solver, stage, field, out_data_p) - if field_ in ["Q", "R", "relaxed_Q", "relaxed_R"]: + if field_.endswith(("Q", "R")): # make symmetric: copy lower triangular part to upper triangular part out = np.tril(out) + np.tril(out, -1).T From 232cc1a3f6ca000bc93f761442677bc54420fe04 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 26 Aug 2025 17:02:03 +0200 Subject: [PATCH 126/164] Update HPIPM (#1611) - some test modifications where needed, all seem reasonable --- .../acados_matlab_octave/test/simulink_test.m | 2 +- .../non_ocp_nlp/maratos_test_problem.py | 18 +++++++----------- .../non_ocp_nlp/qpscaling_test.py | 1 + .../ocp/minimal_example_ocp_cmake.py | 12 ++++++------ .../ocp/ocp_example_h_init_contraints.py | 16 ++++++++-------- external/hpipm | 2 +- test/ocp_nlp/test_wind_turbine.cpp | 14 +++++++++++++- 7 files changed, 37 insertions(+), 28 deletions(-) diff --git a/examples/acados_matlab_octave/test/simulink_test.m b/examples/acados_matlab_octave/test/simulink_test.m index de276be7c6..d146a728a2 100644 --- a/examples/acados_matlab_octave/test/simulink_test.m +++ b/examples/acados_matlab_octave/test/simulink_test.m @@ -65,7 +65,7 @@ % compare u trajectory err_vs_ref = norm(simulink_u_traj_ref - uvals); -TOL = 1e-8; +TOL = 5e-5; disp(['Norm of control traj. wrt. reference solution is: ',... num2str(err_vs_ref, '%e'), ' test TOL = ', num2str(TOL)]); diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index 8ed7512374..347ca15fe9 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -144,7 +144,8 @@ def solve_maratos_problem_with_setting(setting): ocp.solver_options.levenberg_marquardt = 1e-1 # / (N+1) SQP_max_iter = 300 ocp.solver_options.qp_solver_iter_max = 400 - ocp.solver_options.qp_tol = 5e-7 + ocp.solver_options.qp_tol = 1e-7 + ocp.solver_options.nlp_solver_ext_qp_res = 1 ocp.solver_options.regularize_method = 'MIRROR' # ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.globalization = globalization @@ -207,19 +208,14 @@ def solve_maratos_problem_with_setting(setting): if any(alphas[:iter] != 1.0): raise Exception(f"Expected all alphas = 1.0 when using full step SQP on Maratos problem") elif globalization == 'MERIT_BACKTRACKING': - if max_infeasibility > 0.5: + if max_infeasibility > 3.0: raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") elif globalization_use_SOC == 0: - if iter not in range(56, 61): - raise Exception(f"Expected 56 to 60 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") + if iter not in range(10, 61): + raise Exception(f"Expected 10 to 61 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") elif line_search_use_sufficient_descent == 1: - if iter not in range(29, 37): - # NOTE: got 29 locally and 36 on Github actions. - # On Github actions the inequality constraint was numerically violated in the beginning. - # This leads to very different behavior, since the merit gradient is so different. - # Github actions: merit_grad = -1.669330e+00, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = -1.495535e+00 - # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 - raise Exception(f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") + if iter not in range(9, 37): + raise Exception(f"Expected SQP iterations in range(9, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") else: if iter != 16: raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") diff --git a/examples/acados_python/non_ocp_nlp/qpscaling_test.py b/examples/acados_python/non_ocp_nlp/qpscaling_test.py index 49e9e8c431..88105d09a2 100644 --- a/examples/acados_python/non_ocp_nlp/qpscaling_test.py +++ b/examples/acados_python/non_ocp_nlp/qpscaling_test.py @@ -90,6 +90,7 @@ def create_solver(solver_name: str, nlp_solver_type: str = 'SQP_WITH_FEASIBLE_QP # set options solver_options = ocp.solver_options solver_options.N_horizon = N + solver_options.tol = 1e-5 solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' if qp_tol_scheme == "SUFFICIENTLY_SMALL": diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py index c90a2dd74c..a0521a2fcb 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py @@ -94,8 +94,8 @@ ocp.solver_options.nlp_solver_ext_qp_res = 1 ocp.solver_options.nlp_qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT' ocp.solver_options.qp_solver_iter_max = 1000 -ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-2 - +ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-3 +ocp.solver_options.qp_solver_mu0 = 1e2 # set prediction horizon ocp.solver_options.tf = Tf @@ -112,12 +112,13 @@ sum_qp_iter = sum(ocp_solver.get_stats("qp_iter")) nlp_iter = ocp_solver.get_stats("nlp_iter") print(f'nlp_iter: {nlp_iter}, total qp_iter: {sum_qp_iter}') +ocp_solver.print_statistics() -if sum_qp_iter > 66: - raise Exception(f'number of qp iterations {sum_qp_iter} is too high, expected <= 66.') +if sum_qp_iter > 75: + raise Exception(f'number of qp iterations {sum_qp_iter} is too high, expected <= 75.') if status != 0: - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") + ocp_solver.print_statistics() raise Exception(f'acados returned status {status}.') # get solution @@ -126,6 +127,5 @@ 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") plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=True) diff --git a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_h_init_contraints.py b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_h_init_contraints.py index df809e7e34..a1ed54ea8b 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/ocp_example_h_init_contraints.py +++ b/examples/acados_python/pendulum_on_cart/ocp/ocp_example_h_init_contraints.py @@ -46,9 +46,10 @@ # * non-linear constraint expression + relaxing the initial state constraints # * state bounds constraint CONSTRAINT_VERSIONS = ['nl', 'nl_relxd','bound'] +EXPECTED_STATUS_LIST = [0, 0, 0] def test_initial_h_constraints(constraint_version: str): - print(f'#################################################################################### {constraint_version} constraint ####################################################################################') + print(f'############### {constraint_version} constraint ###############') ocp = AcadosOcp() ocp.model = export_pendulum_ode_model() @@ -144,7 +145,7 @@ def test_initial_h_constraints(constraint_version: str): ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' - ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') + ocp_solver = AcadosOcpSolver(ocp, verbose=False) simX = np.zeros((N+1, nx)) simU = np.zeros((N, nu)) @@ -153,9 +154,7 @@ def test_initial_h_constraints(constraint_version: str): if status != 0: print(f'acados returned status {status}.') - # for debugging - # ocp_solver.print_statistics() - # ocp_solver.dump_last_qp_to_json(f'{constraint_version}_last_qp.json', overwrite=True) + ocp_solver.print_statistics() # get solution for i in range(N): @@ -170,9 +169,10 @@ def test_initial_h_constraints(constraint_version: str): if __name__ == "__main__": # we expect that the OCP with nl constraints will fail because it contains two active constraints at the initial node. - expected_status = [2, 0, 0] for constraint_version in CONSTRAINT_VERSIONS: - if test_initial_h_constraints(constraint_version=constraint_version) != expected_status[CONSTRAINT_VERSIONS.index(constraint_version)]: - raise Exception(f'constraint {constraint_version} failed!') + status = test_initial_h_constraints(constraint_version=constraint_version) + expected_status = EXPECTED_STATUS_LIST[CONSTRAINT_VERSIONS.index(constraint_version)] + if expected_status != status: + raise ValueError(f'constraint {constraint_version} returned status {status}, expected {expected_status}.') else: print(f'constraint {constraint_version} passed!') diff --git a/external/hpipm b/external/hpipm index c24243df52..57c5dba419 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit c24243df52b685278f7032b833f8651596cd0fa9 +Subproject commit 57c5dba41909787ea5460aebf7fa03f384f871be diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index a36be7692c..210409b95a 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -968,6 +968,18 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q ocp_nlp_solver_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); ocp_nlp_solver_opts_set(config, nlp_opts, "tol_comp", &tol_comp); + double qp_tol_stat = 0.5 * tol_stat; + double qp_tol_eq = 0.5 * tol_eq; + double qp_tol_ineq = 0.5 * tol_ineq; + double qp_tol_comp = 0.5 * tol_comp; + ocp_nlp_solver_opts_set(config, nlp_opts, "qp_tol_stat", &qp_tol_stat); + ocp_nlp_solver_opts_set(config, nlp_opts, "qp_tol_eq", &qp_tol_eq); + ocp_nlp_solver_opts_set(config, nlp_opts, "qp_tol_ineq", &qp_tol_ineq); + ocp_nlp_solver_opts_set(config, nlp_opts, "qp_tol_comp", &qp_tol_comp); + + int print_level = 0; + ocp_nlp_solver_opts_set(config, nlp_opts, "print_level", &print_level); + // partial condensing if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) @@ -1093,7 +1105,7 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q printf("electrical power = %f\n", electrical_power); printf("Max residuals = %e\n", max_res); - REQUIRE((status == 0 || status == 1 && MAX_SQP_ITERS == 1)); + REQUIRE(status == 0); REQUIRE(max_res <= TOL); // shift trajectories From 81bf0171f7e99d77f861872db1da20ccefbd7567 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 2 Sep 2025 16:02:22 +0200 Subject: [PATCH 127/164] Compile problem specific solver into static library with CMake (optionally) (#1613) Added the following options to the templated CMake file: - `BUILD_ACADOS_OCP_SOLVER_STATIC_LIB`: compiles a static library, similar to `BUILD_ACADOS_OCP_SOLVER_LIB`, which builds a shared lib. - `BUILD_EXAMPLE_STATIC`: builds the main file by linking to the static lib obtained with `BUILD_ACADOS_OCP_SOLVER_STATIC_LIB`. Mainly for testing. --- .../acados_python/tests/static_lib_test.py | 77 +++++++++++++++++++ interfaces/CMakeLists.txt | 5 ++ .../acados_template/acados_ocp_solver.py | 5 +- .../c_templates_tera/CMakeLists.in.txt | 28 ++++++- 4 files changed, 113 insertions(+), 2 deletions(-) create mode 100644 examples/acados_python/tests/static_lib_test.py diff --git a/examples/acados_python/tests/static_lib_test.py b/examples/acados_python/tests/static_lib_test.py new file mode 100644 index 0000000000..a5c8607be6 --- /dev/null +++ b/examples/acados_python/tests/static_lib_test.py @@ -0,0 +1,77 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# + +import os +import numpy as np +import casadi as ca +from acados_template import AcadosOcp, AcadosModel, AcadosOcpSolver, ocp_get_default_cmake_builder +from subprocess import call + +def export_parametric_nlp() -> AcadosOcp: + + model = AcadosModel() + model.x = ca.SX.sym("x", 1) + model.p_global = ca.SX.sym("p_global", 1) + model.cost_expr_ext_cost_e = (model.x - model.p_global**2)**2 + model.name = "non_ocp" + ocp = AcadosOcp() + ocp.model = model + + ocp.constraints.lbx_e = np.array([-1.0]) + ocp.constraints.ubx_e = np.array([1.0]) + ocp.constraints.idxbx_e = np.array([0]) + + ocp.cost.cost_type_e = "EXTERNAL" + ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" + ocp.solver_options.hessian_approx = "EXACT" + ocp.solver_options.N_horizon = 0 + + ocp.p_global_values = np.zeros((1,)) + ocp.solver_options.nlp_solver_ext_qp_res = 1 + + return ocp + + +def main(): + ocp = export_parametric_nlp() + + json_file = 'test_nlp.json' + cmake_builder = ocp_get_default_cmake_builder() + cmake_builder.options_on = ['BUILD_EXAMPLE_STATIC', 'BUILD_ACADOS_OCP_SOLVER_STATIC_LIB'] + AcadosOcpSolver.generate(ocp, json_file=json_file, cmake_builder=cmake_builder) + code_export_dir = ocp.code_export_directory + cmake_builder.exec(code_export_dir, verbose=True) + + call(os.path.join(os.getcwd(), ocp.code_export_directory, f'main_{ocp.name}')) + + +if __name__ == "__main__": + main() diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 167026eabd..bf8465a08d 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -157,6 +157,9 @@ if(ACADOS_PYTHON) add_test(NAME python_test_reset COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests python reset_test.py) + add_test(NAME python_test_static_lib + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests + python static_lib_test.py) add_test(NAME python_test_reset_timing COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/timing_example python reset_timing.py) @@ -502,6 +505,8 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_regularization_test PROPERTIES DEPENDS python_test_sim_dae) set_tests_properties(python_test_sim_dae PROPERTIES DEPENDS python_sparse_param_test) set_tests_properties(python_sparse_param_test PROPERTIES DEPENDS python_test_detect_constraints) + set_tests_properties(python_test_detect_constraints PROPERTIES DEPENDS python_test_static_lib) + set_tests_properties(python_test_static_lib PROPERTIES DEPENDS python_pcond_getters_test) if(ACADOS_WITH_OSQP) set_tests_properties(python_test_detect_constraints PROPERTIES DEPENDS python_OSQP_test) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index ba591650ea..9b86d90d18 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -90,7 +90,10 @@ def shared_lib(self,): return self.__shared_lib @classmethod - def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None, verbose=True): + def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], + json_file: str, + simulink_opts: Optional[dict]=None, + cmake_builder: Optional[CMakeBuilder] = None, verbose=True): """ Generates the code for an acados OCP solver, given the description in acados_ocp. diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index a9e728f174..4f1a61896a 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -85,7 +85,9 @@ project({{ model.name }}) {% if problem_class != "SIM" %} option(BUILD_ACADOS_SOLVER_LIB "Should the solver library acados_solver_{{ model.name }} be build?" OFF) option(BUILD_ACADOS_OCP_SOLVER_LIB "Should the OCP solver library acados_ocp_solver_{{ model.name }} be build?" OFF) +option(BUILD_ACADOS_OCP_SOLVER_STATIC_LIB "Should the static OCP solver library acados_ocp_solver_{{ model.name }}_static be built?" OFF) option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF) +option(BUILD_EXAMPLE_STATIC "Should the example main_{{ model.name }} be build linking to the static OCP solver library?" OFF) {%- endif %} @@ -134,7 +136,7 @@ add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) {% if problem_class != "SIM" %} # optimal control problem - mostly CasADi exports -if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMPLE}) +if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_STATIC_LIB} OR ${BUILD_EXAMPLE}) set(OCP_SRC {% for filename in external_function_files_ocp %} {{ filename }} @@ -258,6 +260,20 @@ if(${BUILD_ACADOS_OCP_SOLVER_LIB}) install(TARGETS ${LIB_ACADOS_OCP_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) endif(${BUILD_ACADOS_OCP_SOLVER_LIB}) +# ocp_static_lib +if(${BUILD_ACADOS_OCP_SOLVER_STATIC_LIB}) + set(LIB_ACADOS_OCP_SOLVER_STATIC acados_ocp_solver_{{ model.name }}_static) + add_library(${LIB_ACADOS_OCP_SOLVER_STATIC} STATIC + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $ + ) + target_link_libraries(${LIB_ACADOS_OCP_SOLVER_STATIC} PRIVATE ${EXTERNAL_LIB}) + target_link_directories(${LIB_ACADOS_OCP_SOLVER_STATIC} PRIVATE ${EXTERNAL_DIR}) + install(TARGETS ${LIB_ACADOS_OCP_SOLVER_STATIC} DESTINATION ${CMAKE_INSTALL_PREFIX}) +endif(${BUILD_ACADOS_OCP_SOLVER_STATIC_LIB}) + # example if(${BUILD_EXAMPLE}) add_executable(${EX_EXE} ${EX_SRC} @@ -271,6 +287,14 @@ if(${BUILD_EXAMPLE}) ) install(TARGETS ${EX_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX}) endif(${BUILD_EXAMPLE}) + +# example linking to static lib +if(${BUILD_EXAMPLE_STATIC}) + add_executable(${EX_EXE} ${EX_SRC} + ) + install(TARGETS ${EX_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX}) + target_link_libraries(${EX_EXE} PRIVATE ${LIB_ACADOS_OCP_SOLVER_STATIC} ${EXTERNAL_LIB}) +endif(${BUILD_EXAMPLE_STATIC}) {%- endif %} {% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} @@ -294,7 +318,9 @@ endif(${BUILD_ACADOS_SIM_SOLVER_LIB}) {%- if problem_class != "SIM" %} unset(BUILD_ACADOS_SOLVER_LIB CACHE) unset(BUILD_ACADOS_OCP_SOLVER_LIB CACHE) +unset(BUILD_ACADOS_OCP_SOLVER_STATIC_LIB CACHE) unset(BUILD_EXAMPLE CACHE) +unset(BUILD_EXAMPLE_STATIC CACHE) {%- endif %} {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} unset(BUILD_SIM_EXAMPLE CACHE) From 4a7d48e1f4f0f6a8867f854c6be0d94eadde7896 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 2 Sep 2025 16:45:11 +0200 Subject: [PATCH 128/164] Improve condensing plot (#1617) - set markersize according matrix size to show sparsity correctly --- examples/acados_python/tests/pcond_getters_test.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/acados_python/tests/pcond_getters_test.py b/examples/acados_python/tests/pcond_getters_test.py index 0420a561fa..f7cf03a769 100644 --- a/examples/acados_python/tests/pcond_getters_test.py +++ b/examples/acados_python/tests/pcond_getters_test.py @@ -34,9 +34,10 @@ def plot_qp_sparsity(qp, fig_filename=None, title=None, with_legend=True): H_xu[offset+nx:offset + nx + nu, offset:offset + nx] = stage["S"] H_xu[offset:offset + nx, offset+nx:offset + nx + nu] = stage["S"].T offset += nx + nu - plt.spy(H_xu, markersize=5, label='$S$', color='C2') - plt.spy(H_x, markersize=5, label='$Q$', color='C0') - plt.spy(H_u, markersize=5, label='$R$', color='C1') + markersize = 5 * (40 / nv_total) + plt.spy(H_xu, markersize=markersize, label='$S$', color='C2') + plt.spy(H_x, markersize=markersize, label='$Q$', color='C0') + plt.spy(H_u, markersize=markersize, label='$R$', color='C1') # remove xticks plt.xticks([]) if with_legend: From 19bd6f3d079ea8b5a7e0b679526e7cd40130c264 Mon Sep 17 00:00:00 2001 From: Leonard Fichtner <30325218+Confectio@users.noreply.github.com> Date: Wed, 3 Sep 2025 21:47:38 +0200 Subject: [PATCH 129/164] Improve python minimal closed-loop example (#1618) Makes ``minimal_example_closed_loop.py`` more minimal. --------- Co-authored-by: Jonathan Frey --- docs/features/index.md | 2 +- .../minimal_example_closed_loop.py | 97 ++++++------------- 2 files changed, 29 insertions(+), 70 deletions(-) diff --git a/docs/features/index.md b/docs/features/index.md index f22b7deb0c..766935c1eb 100644 --- a/docs/features/index.md +++ b/docs/features/index.md @@ -8,7 +8,7 @@ If you are new to `acados`, we highly recommend you to start with the `getting_s ### Getting started with Model Predictive Control In particular, the **closed-loop** examples are a great starting point to develop model predictive control (MPC) in a simulation. Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs and an integrator (`AcadosSimSolver`) is used to simulate the real system. -- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started/minimal_example_closed_loop.py) +- Python: [minimal](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started/minimal_example_closed_loop.py), [with real-time algorithms](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m) ## Simulation and Sensitivity propagation diff --git a/examples/acados_python/getting_started/minimal_example_closed_loop.py b/examples/acados_python/getting_started/minimal_example_closed_loop.py index 8c22a20499..1547adc6b1 100644 --- a/examples/acados_python/getting_started/minimal_example_closed_loop.py +++ b/examples/acados_python/getting_started/minimal_example_closed_loop.py @@ -29,14 +29,14 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver, AcadosSim from pendulum_model import export_pendulum_ode_model from utils import plot_pendulum import numpy as np import scipy.linalg from casadi import vertcat -def setup(x0, Fmax, N_horizon, Tf, RTI=False): +def setup_ocp_solver(x0, Fmax, N_horizon, Tf): # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -49,8 +49,7 @@ def setup(x0, Fmax, N_horizon, Tf, RTI=False): ny = nx + nu ny_e = nx - - # set cost module + # set cost ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' @@ -62,7 +61,7 @@ def setup(x0, Fmax, N_horizon, Tf, RTI=False): ocp.model.cost_y_expr = vertcat(model.x, model.u) ocp.model.cost_y_expr_e = model.x - ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) # set constraints @@ -76,31 +75,28 @@ def setup(x0, Fmax, N_horizon, Tf, RTI=False): ocp.solver_options.N_horizon = N_horizon ocp.solver_options.tf = Tf - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + # set ocp options ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'IRK' - ocp.solver_options.sim_method_newton_iter = 10 - - if RTI: - ocp.solver_options.nlp_solver_type = 'SQP_RTI' - else: - ocp.solver_options.nlp_solver_type = 'SQP' - ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization - ocp.solver_options.nlp_solver_max_iter = 150 + ocp.solver_options.qp_tol = 1e-8 - ocp.solver_options.qp_solver_cond_N = N_horizon + ocp.code_export_directory = 'c_generated_code_ocp' + acados_ocp_solver = AcadosOcpSolver(ocp) + return acados_ocp_solver - solver_json = 'acados_ocp_' + model.name + '.json' - acados_ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json) - # create an integrator with the same settings as used in the OCP solver. - acados_integrator = AcadosSimSolver(ocp, json_file = solver_json) +def setup_integrator(dt): + sim = AcadosSim() + sim.model = export_pendulum_ode_model() - return acados_ocp_solver, acados_integrator + sim.solver_options.T = dt # simulation time + sim.solver_options.num_steps = 2 # Make extra integrator more precise than ocp-internal integrator + sim.code_export_directory = 'c_generated_code_sim' + acados_integrator = AcadosSimSolver(sim) + return acados_integrator -def main(use_RTI=False): +def main(): x0 = np.array([0.0, np.pi, 0.0, 0.0]) Fmax = 80 @@ -108,7 +104,8 @@ def main(use_RTI=False): Tf = .8 N_horizon = 40 - ocp_solver, integrator = setup(x0, Fmax, N_horizon, Tf, use_RTI) + ocp_solver = setup_ocp_solver(x0, Fmax, N_horizon, Tf) + integrator = setup_integrator(Tf/N_horizon) nx = ocp_solver.acados_ocp.dims.nx nu = ocp_solver.acados_ocp.dims.nu @@ -119,60 +116,23 @@ def main(use_RTI=False): simX[0,:] = x0 - if use_RTI: - t_preparation = np.zeros((Nsim)) - t_feedback = np.zeros((Nsim)) - - else: - t = np.zeros((Nsim)) - - # do some initial iterations to start with a good initial guess - num_iter_initial = 5 - for _ in range(num_iter_initial): - ocp_solver.solve_for_x0(x0_bar = x0) + t = np.zeros((Nsim)) # closed loop for i in range(Nsim): - if use_RTI: - # preparation phase - ocp_solver.options_set('rti_phase', 1) - status = ocp_solver.solve() - t_preparation[i] = ocp_solver.get_stats('time_tot') - - # set initial state - ocp_solver.set(0, "lbx", simX[i, :]) - ocp_solver.set(0, "ubx", simX[i, :]) - - # feedback phase - ocp_solver.options_set('rti_phase', 2) - status = ocp_solver.solve() - t_feedback[i] = ocp_solver.get_stats('time_tot') - - simU[i, :] = ocp_solver.get(0, "u") - - else: - # solve ocp and get next control input - simU[i,:] = ocp_solver.solve_for_x0(x0_bar = simX[i, :]) + # solve ocp and get next control input + simU[i,:] = ocp_solver.solve_for_x0(x0_bar = simX[i, :]) - t[i] = ocp_solver.get_stats('time_tot') + t[i] = ocp_solver.get_stats('time_tot') # simulate system simX[i+1, :] = integrator.simulate(x=simX[i, :], u=simU[i,:]) # evaluate timings - if use_RTI: - # scale to milliseconds - t_preparation *= 1000 - t_feedback *= 1000 - print(f'Computation time in preparation phase in ms: \ - min {np.min(t_preparation):.3f} median {np.median(t_preparation):.3f} max {np.max(t_preparation):.3f}') - print(f'Computation time in feedback phase in ms: \ - min {np.min(t_feedback):.3f} median {np.median(t_feedback):.3f} max {np.max(t_feedback):.3f}') - else: - # scale to milliseconds - t *= 1000 - print(f'Computation time in ms: min {np.min(t):.3f} median {np.median(t):.3f} max {np.max(t):.3f}') + # scale to milliseconds + t *= 1000 + print(f'Computation time in ms: min {np.min(t):.3f} median {np.median(t):.3f} max {np.max(t):.3f}') # plot results model = ocp_solver.acados_ocp.model @@ -182,5 +142,4 @@ def main(use_RTI=False): if __name__ == '__main__': - main(use_RTI=False) - main(use_RTI=True) + main() From bef4371d92e6bdcee7f6a9615c1f82631cdd686d Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Fri, 5 Sep 2025 16:18:03 +0200 Subject: [PATCH 130/164] `AcadosCasadiOcp`: Add support for updating `y_ref` (#1616) - modify `AcadosCasadiOcp` for supporting updating `y_ref` by adding `y_ref` into parameter `p` - modify `AcadosCasadiOcpSolver` for setting `y_ref` - add a simple test for testing the support --------- Co-authored-by: Jonathan Frey --- .../casadi_tests/test_casadi_closed_loop.py | 148 ++++++++++++++++++ examples/acados_python/cstr/cstr_model.py | 8 +- examples/acados_python/cstr/cstr_utils.py | 2 - .../cstr/integrator_experiment.py | 2 - examples/acados_python/cstr/main.py | 90 ++++++++--- .../cstr/setup_acados_ocp_solver.py | 48 +++--- interfaces/CMakeLists.txt | 4 + .../acados_casadi_ocp_solver.py | 147 +++++++++++------ .../acados_template/acados_ocp.py | 18 +-- 9 files changed, 361 insertions(+), 106 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_closed_loop.py diff --git a/examples/acados_python/casadi_tests/test_casadi_closed_loop.py b/examples/acados_python/casadi_tests/test_casadi_closed_loop.py new file mode 100644 index 0000000000..43580b674c --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_closed_loop.py @@ -0,0 +1,148 @@ +import sys +sys.path.insert(0, '../getting_started') + +import numpy as np +import casadi as ca + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver, AcadosSim, AcadosSimSolver +from pendulum_model import export_pendulum_ode_model +from utils import plot_pendulum + +PLOT = False + +def formulate_ocp_and_sim(Tf: float = 1.0, N: int = 20, dt: float = 0.05)-> tuple[AcadosOcp, AcadosSimSolver]: + ### create ocp + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + ### set integrator + sim = AcadosSim() + sim.model = model + sim.solver_options.T = dt + sim.solver_options.num_stages = 4 + sim.solver_options.num_steps = 2 + sim.solver_options.integrator_type = 'ERK' + + integrator = AcadosSimSolver(sim, verbose=False) + + return ocp, integrator + +def main(): + Tsim = 2.0 + Tf = 0.5 + dt = 0.05 + Nsim = int(Tsim/dt) + N_horizon = int(Tf/dt) + ocp, integrator = formulate_ocp_and_sim(Tf, N_horizon, dt) + ocp.make_consistent() + + simX = np.zeros((Nsim+1, ocp.dims.nx)) + simU = np.zeros((Nsim, ocp.dims.nu)) + simX_casadi = np.zeros((Nsim+1, ocp.dims.nx)) + simU_casadi = np.zeros((Nsim, ocp.dims.nu)) + simX[0,:] = ocp.constraints.x0 + simX_casadi[0,:] = ocp.constraints.x0 + + # steady-state + xs = np.array([[0.0, 0.0, 0.0, 0.0]]).T + us = np.array([[0.0]]).T + + # constant ref + X_ref = np.tile(xs, Nsim + 1).T + U_ref = np.tile(us, Nsim).T + + # reference jump + xs2 = np.array([0.5, 0, 0, 0]) + Njump = int(Nsim / 4) + X_ref[Njump : 3*Njump, :] = xs2 + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + # do some initial iterations to start with a good initial guess + num_iter_initial = 10 + for _ in range(num_iter_initial): + ocp_solver.solve_for_x0(x0_bar = simX[0,:], fail_on_nonzero_status=False) + # closed loop with acados + for i in range(Nsim): + yref = np.concatenate((X_ref[i, :], U_ref[i, :])) + for stage in range(ocp.solver_options.N_horizon): + ocp_solver.set(stage, "yref", yref) + ocp_solver.set(ocp.solver_options.N_horizon, "yref", X_ref[i, :]) + simU[i, :] = ocp_solver.solve_for_x0(simX[i, :]) + simX[i+1, :] = integrator.simulate(simX[i, :], simU[i, :]) + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp, solver="ipopt", verbose=False) + # do some initial iterations to start with a good initial guess + num_iter_initial = 10 + for _ in range(num_iter_initial): + casadi_ocp_solver.solve_for_x0(x0_bar = simX_casadi[0,:]) + for i in range(Nsim): + yref = np.concatenate((X_ref[i, :], U_ref[i, :])) + for stage in range(ocp.solver_options.N_horizon): + casadi_ocp_solver.set(stage, "yref", yref) + casadi_ocp_solver.set(ocp.solver_options.N_horizon, "yref", X_ref[i, :]) + simU_casadi[i, :] = casadi_ocp_solver.solve_for_x0(simX_casadi[i, :]) + simX_casadi[i+1, :] = integrator.simulate(simX_casadi[i, :], simU_casadi[i, :]) + + diff_x = np.linalg.norm(simX - simX_casadi) + diff_u = np.linalg.norm(simU - simU_casadi) + print(f"Difference in state trajectories: {diff_x}") + print(f"Difference in control inputs: {diff_u}") + if diff_x > 1e-4 or diff_u > 1e-4: + raise Exception("Results with acados and casadi are different!") + + if PLOT: + Fmax = 80 + acados_u = simU + acados_x = simX + casadi_u = simU_casadi + casadi_x = simX_casadi + plot_pendulum(np.linspace(0, Tsim, Nsim+1), Fmax, acados_u, acados_x, latexify=False) + plot_pendulum(np.linspace(0, Tsim, Nsim+1), Fmax, casadi_u, casadi_x, latexify=False) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/examples/acados_python/cstr/cstr_model.py b/examples/acados_python/cstr/cstr_model.py index 5ec24330af..947c7c22a1 100644 --- a/examples/acados_python/cstr/cstr_model.py +++ b/examples/acados_python/cstr/cstr_model.py @@ -29,10 +29,8 @@ # POSSIBILITY OF SUCH DAMAGE.; # -# authors: Katrin Baumgaertner, Jonathan Frey - import numpy as np -from dataclasses import dataclass +from dataclasses import dataclass, field from acados_template import AcadosModel from casadi import SX, vertcat, exp from cstr_utils import compute_lqr_gain @@ -53,8 +51,8 @@ class CstrParameters: dH: float = -5 * 1e4 # kJ / kmol # to avoid division by zero eps: float = 1e-5 # m - xs: np.ndarray = np.array([0.878, 324.5, 0.659]) - us: np.ndarray = np.array([300, 0.1]) + xs: np.ndarray = field(default_factory=lambda: np.array([0.878, 324.5, 0.659])) + us: np.ndarray = field(default_factory=lambda: np.array([300, 0.1])) def setup_cstr_model(params: CstrParameters): diff --git a/examples/acados_python/cstr/cstr_utils.py b/examples/acados_python/cstr/cstr_utils.py index 5430393a89..69a8188562 100644 --- a/examples/acados_python/cstr/cstr_utils.py +++ b/examples/acados_python/cstr/cstr_utils.py @@ -29,8 +29,6 @@ # POSSIBILITY OF SUCH DAMAGE.; # -# authors: Katrin Baumgaertner, Jonathan Frey - import matplotlib.pyplot as plt import numpy as np from acados_template import latexify_plot diff --git a/examples/acados_python/cstr/integrator_experiment.py b/examples/acados_python/cstr/integrator_experiment.py index 702c30f2a1..e3b3fb1098 100644 --- a/examples/acados_python/cstr/integrator_experiment.py +++ b/examples/acados_python/cstr/integrator_experiment.py @@ -29,8 +29,6 @@ # POSSIBILITY OF SUCH DAMAGE.; # -# authors: Katrin Baumgaertner, Jonathan Frey - from cstr_model import CstrParameters, setup_cstr_model from setup_acados_ocp_solver import MpcCstrParameters from setup_acados_integrator import setup_acados_integrator diff --git a/examples/acados_python/cstr/main.py b/examples/acados_python/cstr/main.py index 02a1463f37..343e12467e 100644 --- a/examples/acados_python/cstr/main.py +++ b/examples/acados_python/cstr/main.py @@ -29,23 +29,22 @@ # POSSIBILITY OF SUCH DAMAGE.; # -# authors: Katrin Baumgaertner, Jonathan Frey - from cstr_model import CstrParameters, setup_cstr_model, setup_linearized_model from setup_acados_ocp_solver import ( MpcCstrParameters, setup_acados_ocp_solver, AcadosOcpSolver, + AcadosCasadiOcpSolver ) from setup_acados_integrator import setup_acados_integrator, AcadosSimSolver import numpy as np import casadi as ca from cstr_utils import plot_cstr -from typing import Optional +from typing import Union def simulate( - controller: Optional[AcadosOcpSolver], + controller: Union[AcadosOcpSolver, AcadosCasadiOcpSolver, None], plant: AcadosSimSolver, x0: np.ndarray, Nsim: int, @@ -64,6 +63,8 @@ def simulate( # closed loop xcurrent = x0 + if controller is not None: + ocp = controller.acados_ocp if isinstance(controller, AcadosOcpSolver) else controller.ocp X[0, :] = xcurrent for i in range(Nsim): @@ -73,14 +74,14 @@ def simulate( else: if with_reference_profile: t0 = i * plant.acados_sim.solver_options.T - for stage, dt in enumerate(controller.acados_ocp.solver_options.shooting_nodes): + for stage, dt in enumerate(ocp.solver_options.shooting_nodes): t = t0 + dt controller.set_params_sparse(stage, np.array([1]), np.array([t])) else: yref = np.concatenate((X_ref[i, :], U_ref[i, :])) - for stage in range(controller.acados_ocp.solver_options.N_horizon): + for stage in range(ocp.solver_options.N_horizon): controller.set(stage, "yref", yref) - controller.set(controller.acados_ocp.solver_options.N_horizon, "yref", X_ref[i, :]) + controller.set(ocp.solver_options.N_horizon, "yref", X_ref[i, :]) # solve ocp U[i, :] = controller.solve_for_x0(xcurrent) @@ -96,15 +97,18 @@ def simulate( def main(): with_nmpc = True + with_nmpc_ipopt = True with_timevar_ref_nmpc = True + with_timevar_ref_nmpc_ipopt = True with_linear_mpc = True with_nmpc_rti = True Tsim = 25 dt_plant = 0.25 # [min] + N_horizon = 10 cstr_params = CstrParameters() - mpc_params = MpcCstrParameters(xs=cstr_params.xs, us=cstr_params.us) + mpc_params = MpcCstrParameters(xs=cstr_params.xs, us=cstr_params.us, N=N_horizon, Tf=N_horizon*dt_plant) model = setup_cstr_model(cstr_params) linearized_model = setup_linearized_model(model, cstr_params, mpc_params) plant_model = setup_cstr_model(cstr_params) @@ -137,11 +141,13 @@ def main(): U_all = [] labels_all = [] timings_solver_all = [] + diff_all = [] # simulation with constant reference input label = "constant reference input" print(f"\n\nRunning simulation with {label}\n\n") X, U, timings_solver, _ = simulate(None, integrator, x0, Nsim, X_ref, U_ref) + diff_all.append(None) X_all.append(X) U_all.append(U) timings_solver_all.append(timings_solver) @@ -151,11 +157,31 @@ def main(): if with_nmpc: label = "NMPC" print(f"\n\nRunning simulation with {label}\n\n") - ocp_solver = setup_acados_ocp_solver(model, mpc_params, cstr_params=cstr_params) + ocp_solver = setup_acados_ocp_solver(model, solver_type='SQP', mpc_params=mpc_params, cstr_params=cstr_params) X, U, timings_solver, _ = simulate( ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref ) + diff_all.append(None) + X_all.append(X) + U_all.append(U) + timings_solver_all.append(timings_solver) + labels_all.append(label) + ocp_solver = None + + # simulation with NMPC controller (IPOPT) + if with_nmpc_ipopt: + label = "NMPC with IPOPT" + print(f"\n\nRunning simulation with {label}\n\n") + ocp_solver = setup_acados_ocp_solver(model, solver_type='IPOPT', mpc_params=mpc_params, cstr_params=cstr_params) + + X, U, timings_solver, _ = simulate( + ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref + ) + if with_nmpc: + diff_all.append(np.linalg.norm(X - X_all[-1])) + else: + diff_all.append(None) X_all.append(X) U_all.append(U) timings_solver_all.append(timings_solver) @@ -166,22 +192,42 @@ def main(): if with_timevar_ref_nmpc: label = "NMPC with time-varying reference" ocp_model = setup_cstr_model(cstr_params) - ocp_model.t = ca.SX.sym("t") - t0 = ca.SX.sym("t0") - ocp_model.p = ca.vertcat(ocp_model.p, t0) - t = t0 + ocp_model.t + ocp_model.p = ca.vertcat(ocp_model.p, ca.SX.sym("t")) tjump1 = Njump * dt_plant tjump2 = 2 * Njump * dt_plant - reference_profile = ca.if_else(t < tjump1, ca.vertcat(xs, us), ca.vertcat(xs2, us2)) - reference_profile = ca.if_else(t < tjump2, reference_profile, ca.vertcat(xs, us)) + reference_profile = ca.if_else(ocp_model.p[-1] < tjump1, ca.vertcat(xs, us), ca.vertcat(xs2, us2)) + reference_profile = ca.if_else(ocp_model.p[-1] < tjump2, reference_profile, ca.vertcat(xs, us)) print(f"\n\nRunning simulation with {label}\n\n") - ocp_solver = setup_acados_ocp_solver(ocp_model, mpc_params, cstr_params=cstr_params, reference_profile=reference_profile, cost_integration=True) + ocp_solver = setup_acados_ocp_solver(ocp_model, solver_type='SQP', mpc_params=mpc_params, + cstr_params=cstr_params, reference_profile=reference_profile) X, U, timings_solver, _ = simulate( ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref, with_reference_profile=True ) + diff_all.append(None) + X_all.append(X) + U_all.append(U) + timings_solver_all.append(timings_solver) + labels_all.append(label) + ocp_solver = None + + # simulation with time varying reference NMPC controller (IPOPT) + if with_timevar_ref_nmpc_ipopt: + label = "NMPC with time-varying reference with IPOPT" + print(f"\n\nRunning simulation with {label}\n\n") + + ocp_solver = setup_acados_ocp_solver(ocp_model, solver_type='IPOPT', mpc_params=mpc_params, + cstr_params=cstr_params, reference_profile=reference_profile) + + X, U, timings_solver, _ = simulate( + ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref, with_reference_profile=True + ) + if with_timevar_ref_nmpc: + diff_all.append(np.linalg.norm(X - X_all[-1])) + else: + diff_all.append(None) X_all.append(X) U_all.append(U) timings_solver_all.append(timings_solver) @@ -194,13 +240,14 @@ def main(): print(f"\n\nRunning simulation with {label}\n\n") mpc_params.linear_mpc = True ocp_solver = setup_acados_ocp_solver( - linearized_model, mpc_params, cstr_params=cstr_params, use_rti=True + linearized_model, solver_type='SQP_RTI', mpc_params=mpc_params, cstr_params=cstr_params ) mpc_params.linear_mpc = False X, U, timings_solver, _ = simulate( ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref ) + diff_all.append(None) X_all.append(X) U_all.append(U) timings_solver_all.append(timings_solver) @@ -212,12 +259,13 @@ def main(): label = "NMPC-RTI" print(f"\n\nRunning simulation with {label}\n\n") ocp_solver = setup_acados_ocp_solver( - model, mpc_params, cstr_params=cstr_params, use_rti=True + model, solver_type='SQP_RTI', mpc_params=mpc_params, cstr_params=cstr_params ) X, U, timings_solver, _ = simulate( ocp_solver, integrator, x0, Nsim, X_ref=X_ref, U_ref=U_ref ) + diff_all.append(None) X_all.append(X) U_all.append(U) timings_solver_all.append(timings_solver) @@ -235,6 +283,12 @@ def main(): f"{label:{max_label_length}} {f'{np.min(timings_solver):.3f}':>10} {f'{np.mean(timings_solver):.3f}':>10} {f'{np.max(timings_solver):.3f}':>10}" ) + print(f"\n{'Differences in x trajectory between IPOPT and acados:'} \n------------------") + for i in range(len(labels_all)): + label = labels_all[i] + if diff_all[i] is not None: + print(f"{label:{max_label_length}} {f'{diff_all[i]:.3f}':>10}") + # plot results plot_cstr( dt_plant, diff --git a/examples/acados_python/cstr/setup_acados_ocp_solver.py b/examples/acados_python/cstr/setup_acados_ocp_solver.py index bbdacc58ed..68e7bc1909 100644 --- a/examples/acados_python/cstr/setup_acados_ocp_solver.py +++ b/examples/acados_python/cstr/setup_acados_ocp_solver.py @@ -31,11 +31,11 @@ # authors: Katrin Baumgaertner, Jonathan Frey -from acados_template import AcadosOcp, AcadosOcpSolver +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver from scipy.linalg import block_diag import numpy as np from casadi import vertcat - +from typing import Union class MpcCstrParameters: def __init__(self, xs: np.ndarray, us: np.ndarray, dt: float = 0.25, linear_mpc: bool = False, N: int = 16, Tf: float = 4.0): @@ -58,10 +58,13 @@ def __init__(self, xs: np.ndarray, us: np.ndarray, dt: float = 0.25, linear_mpc: def setup_acados_ocp_solver( model, mpc_params: MpcCstrParameters, cstr_params, - use_rti=False, reference_profile=None, - cost_integration=False -): + solver_type: str = "SQP", +) -> Union[AcadosOcpSolver, AcadosCasadiOcpSolver]: + + # sanity checks + if solver_type not in ["SQP", "SQP_RTI", "IPOPT"]: + raise ValueError(f"solver_type '{solver_type}' not supported. Choose 'SQP', 'SQP_RTI' or 'IPOPT'.") ocp = AcadosOcp() @@ -94,8 +97,6 @@ def setup_acados_ocp_solver( ocp.model.cost_y_expr -= reference_profile ocp.model.cost_y_expr_e -= reference_profile[:nx] ocp.parameter_values = np.concatenate((ocp.parameter_values, np.array([0.0]))) - if cost_integration: - ocp.solver_options.cost_discretization = "INTEGRATOR" ocp.cost.yref = np.zeros((nx + nu,)) ocp.cost.yref_e = np.zeros((nx,)) @@ -107,32 +108,31 @@ def setup_acados_ocp_solver( ocp.constraints.x0 = cstr_params.xs - # 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.qp_solver = 'FULL_CONDENSING_QPOASES' - ocp.solver_options.qp_solver_cond_N = mpc_params.N # for partial condensing - - ocp.solver_options.hessian_approx = "GAUSS_NEWTON" - # ocp.solver_options.print_level = 1 - if use_rti: - ocp.solver_options.nlp_solver_type = "SQP_RTI" # SQP_RTI, SQP - else: - ocp.solver_options.nlp_solver_type = "SQP" # SQP_RTI, SQP - + # discretization options if mpc_params.linear_mpc: ocp.solver_options.integrator_type = "DISCRETE" else: - ocp.solver_options.integrator_type = "IRK" + ocp.solver_options.integrator_type = "ERK" ocp.solver_options.sim_method_num_stages = 4 ocp.solver_options.sim_method_num_steps = 1 # 5 + # solver options + ocp.solver_options.qp_solver_cond_N = mpc_params.N # for partial condensing + ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" + + ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + # ocp.solver_options.print_level = 1 + if solver_type == "SQP_RTI": + ocp.solver_options.nlp_solver_type = "SQP_RTI" + else: + ocp.solver_options.nlp_solver_type = "SQP" ocp.solver_options.levenberg_marquardt = 1e-5 - # ocp.solver_options.tol = 1e-3 # create - ocp_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") + if solver_type == "IPOPT": + ocp_solver = AcadosCasadiOcpSolver(ocp) + else: + ocp_solver = AcadosOcpSolver(ocp) return ocp_solver diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index bf8465a08d..129f59bb80 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -396,6 +396,9 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_test_casadi_LICQ_violation COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests python test_casadi_LICQ_violation.py) + add_test(NAME python_test_casadi_closed_loop + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_closed_loop.py) # Sim add_test(NAME python_pendulum_ext_sim_example @@ -482,6 +485,7 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_test_casadi_slack_in_h PROPERTIES DEPENDS python_test_casadi_constraint) set_tests_properties(python_test_casadi_single_shooting PROPERTIES DEPENDS python_test_casadi_slack_in_h) set_tests_properties(python_test_casadi_LICQ_violation PROPERTIES DEPENDS python_test_casadi_single_shooting) + set_tests_properties(python_test_casadi_closed_loop PROPERTIES DEPENDS python_test_casadi_LICQ_violation) # Directory getting_started set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index f72240d635..3ae0c37a19 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -35,7 +35,7 @@ import numpy as np -from .utils import casadi_length, is_casadi_SX +from .utils import casadi_length, is_casadi_SX, is_empty from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpFlattenedIterate @@ -61,6 +61,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): # indices of parameters within p_nlp 'p_in_p_nlp': [], 'p_global_in_p_nlp': [], + 'yref_in_p_nlp': [], # indices of state bounds and control bounds within lam_x(lam_w) in casadi formulation 'lam_bx_in_lam_w':[], 'lam_bu_in_lam_w': [], @@ -75,6 +76,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): self.offset_w = 0 # offset for the indices in index_map self.offset_gnl = 0 self.offset_lam = 0 + self.offset_p = 0 # unpack model = ocp.model @@ -102,19 +104,16 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): su_nodes = [] if multiple_shooting: for i in range(N_horizon+1): - self._append_node('x', ca_symbol, xtraj_nodes, i, dims) - self._append_node('u', ca_symbol, utraj_nodes, i, dims) - self._append_node('sl', ca_symbol, sl_nodes, i, dims) - self._append_node('su', ca_symbol, su_nodes, i, dims) + self._append_node_for_variables('x', ca_symbol, xtraj_nodes, i, dims) + self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) + self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) + self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) else: # single_shooting self._x_traj_fun = [] for i in range(N_horizon): - self._append_node('u', ca_symbol, utraj_nodes, i, dims) - self._append_node('sl', ca_symbol, sl_nodes, i, dims) - self._append_node('su', ca_symbol, su_nodes, i, dims) - - # parameters - ptraj_nodes = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon+1)] + self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) + self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) + self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) # setup state and control bounds lb_xtraj_nodes = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] @@ -143,8 +142,6 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): lbw_list = [] ubw_list = [] w0_list = [] - p_list = [] - offset_p = 0 x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) if multiple_shooting: for i in range(N_horizon+1): @@ -152,9 +149,6 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): if i < N_horizon: self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) - p_list.append(ocp.parameter_values) - self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) - offset_p += dims.np else: # single_shooting xtraj_nodes.append(x_guess) self._x_traj_fun.append(x_guess) @@ -162,15 +156,18 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): if i < N_horizon: self._append_variables_and_bounds('u', w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) - p_list.append(ocp.parameter_values) - self._index_map['p_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np))) - offset_p += dims.np - p_list.append(ocp.p_global_values) - self._index_map['p_global_in_p_nlp'].append(list(range(offset_p, offset_p+dims.np_global))) - offset_p += dims.np_global nw = self.offset_w # number of primal variables + # setup parameter nodes and value + ptraj_nodes = [] + p_list = [] + for i in range(N_horizon+1): + self._append_node_and_value_for_params(ca_symbol, p_list, ptraj_nodes, i, ocp) + p_list.append(ocp.p_global_values) + self._index_map['p_global_in_p_nlp'].append(list(range(self.offset_p, self.offset_p+dims.np_global))) + self.offset_p += dims.np_global + # vectorize w = ca.vertcat(*w_sym_list) lbw = ca.vertcat(*lbw_list) @@ -199,9 +196,9 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): if multiple_shooting: if i < N_horizon: if solver_options.integrator_type == "DISCRETE": - dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i], model.p_global) + dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) elif solver_options.integrator_type == "ERK": - param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i], model.p_global) + param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], param, solver_options.time_steps[i]) self._append_constraints(i, 'dyn', g, lbg, ubg, g_expr = dyn_equality, @@ -219,9 +216,9 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): if i < N_horizon: x_current = xtraj_nodes[i] if solver_options.integrator_type == "DISCRETE": - x_next = f_discr_fun(x_current, utraj_nodes[i], ptraj_nodes[i], model.p_global) + x_next = f_discr_fun(x_current, utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) elif solver_options.integrator_type == "ERK": - param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i], model.p_global) + param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) x_next = f_discr_fun(x_current, param, solver_options.time_steps[i]) xtraj_nodes.append(x_next) self._x_traj_fun.append(f_discr_fun) @@ -282,7 +279,7 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): # add convex-over-nonlinear constraints if nphi > 0: self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = conl_constr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i], model.p_global), + g_expr = conl_constr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global), lbg_expr = lphi, ubg_expr = uphi, cons_dim=nphi) @@ -299,11 +296,12 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): ### Cost nlp_cost = 0 for i in range(N_horizon+1): - xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, ns, zl, Zl, zu, Zu = \ - self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, ptraj_nodes, sl_nodes, su_nodes, ocp, dims, cost) + xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, p_for_model, ns, zl, Zl, zu, Zu = \ + self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, p_nlp, sl_nodes, su_nodes, ocp, dims, cost) - cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, model.p, model.p_global], [cost_expr_i]) - nlp_cost += solver_options.cost_scaling[i] * cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) + cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, p_for_model, model.p_global], [cost_expr_i]) + cost_i = cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) + nlp_cost += solver_options.cost_scaling[i] * cost_i if ns: penalty_expr_i = 0.5 * ca.mtimes(sl_node_i.T, ca.mtimes(np.diag(Zl), sl_node_i)) + \ ca.mtimes(zl.reshape(-1, 1).T, sl_node_i) + \ @@ -339,20 +337,27 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): self.__nlp_hess_l_custom = nlp_hess_l_custom self.__hess_approx_expr = hess_l - def _append_node(self, _field, ca_symbol, node:list, i, dims): + def _append_node_for_variables(self, _field, ca_symbol, node:list, i, dims): """ Helper function to append a node to the NLP formulation. """ if i == 0: ns = dims.ns_0 + nx = dims.nx + nu = dims.nu elif i < dims.N: ns = dims.ns + nx = dims.nx + nu = dims.nu else: ns = dims.ns_e + nx = dims.nx + nu = 0 + if _field == 'x': - node.append(ca_symbol(f'x{i}', dims.nx, 1)) + node.append(ca_symbol(f'x{i}', nx, 1)) elif _field == 'u': - node.append(ca_symbol(f'u{i}', dims.nu, 1)) + node.append(ca_symbol(f'u{i}', nu, 1)) elif _field == 'sl': if ns > 0: node.append(ca_symbol(f'sl{i}', ns, 1)) @@ -364,6 +369,28 @@ def _append_node(self, _field, ca_symbol, node:list, i, dims): else: node.append([]) + def _append_node_and_value_for_params(self, ca_symbol, p_list, node, stage, ocp:AcadosOcp): + """ + Helper function to append parameter values to the NLP formulation. + """ + if stage == 0: + ny = ocp.dims.ny_0 + yref = ocp.cost.yref_0 + elif stage < ocp.solver_options.N_horizon: + ny = ocp.dims.ny + yref = ocp.cost.yref + else: + ny = ocp.dims.ny_e + yref = ocp.cost.yref_e + + node.append(ca.vertcat(ca_symbol(f'p{stage}', ocp.dims.np, 1), ca_symbol(f'yref{stage}', ny, 1))) + p_list.append(ocp.parameter_values) + self._index_map['p_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ocp.dims.np))) + self.offset_p += ocp.dims.np + p_list.append(yref) + self._index_map['yref_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ny))) + self.offset_p += ny + def _set_bounds_indices(self, _field, i, lb_node, ub_node, constraints, dims): """ Helper function to set bounds and indices for the primal variables. @@ -479,33 +506,42 @@ def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr self._index_map['lam_su_in_lam_g'][i].append(self.offset_gnl) self.offset_gnl += 1 - def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, sl_node, su_node, ocp, dims, cost): + def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, p_nlp, sl_node, su_node, ocp: AcadosOcp, dims, cost): """ Helper function to get the cost node for a given stage. """ if i == 0: + p_index = self._index_map['p_in_p_nlp'][0] + yref_index = self._index_map['yref_in_p_nlp'][0] return (xtraj_node[0], utraj_node[0], - ptraj_node[0], + p_nlp[p_index + yref_index], sl_node[0], su_node[0], - ocp.get_initial_cost_expression(), + ocp.get_initial_cost_expression(p_nlp[yref_index]), + ca.vertcat(ocp.model.p, p_nlp[yref_index]), dims.ns_0, cost.zl_0, cost.Zl_0, cost.zu_0, cost.Zu_0) elif i < N_horizon: + p_index = self._index_map['p_in_p_nlp'][i] + yref_index = self._index_map['yref_in_p_nlp'][i] return (xtraj_node[i], utraj_node[i], - ptraj_node[i], + p_nlp[p_index + yref_index], sl_node[i], su_node[i], - ocp.get_path_cost_expression(), + ocp.get_path_cost_expression(p_nlp[yref_index]), + ca.vertcat(ocp.model.p, p_nlp[yref_index]), dims.ns, cost.zl, cost.Zl, cost.zu, cost.Zu) else: + p_index = self._index_map['p_in_p_nlp'][-1] + yref_index = self._index_map['yref_in_p_nlp'][-1] return (xtraj_node[-1], [], - ptraj_node[-1], + p_nlp[p_index + yref_index], sl_node[-1], su_node[-1], - ocp.get_terminal_cost_expression(), + ocp.get_terminal_cost_expression(p_nlp[yref_index]), + ca.vertcat(ocp.model.p, p_nlp[yref_index]), dims.ns_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims): @@ -527,7 +563,7 @@ def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) # nonlinear function h_fun = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) # compound nonlinear constraint conl_constr_fun = None if dims.nphi_0 > 0: @@ -547,7 +583,7 @@ def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, D = constraints.D linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i], model.p_global) + h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) conl_constr_fun = None if dims.nphi > 0: conl_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) @@ -565,7 +601,7 @@ def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, C = constraints.C_e linear_constr_expr = ca.mtimes(C, xtraj_node[i]) h_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) - h_i_nlp_expr = h_fun(xtraj_node[i], ptraj_node[i], model.p_global) + h_i_nlp_expr = h_fun(xtraj_node[i], ptraj_node[i][:dims.np], model.p_global) conl_constr_fun = None if dims.nphi_e > 0: conl_expr = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) @@ -690,10 +726,17 @@ def __init__(self, ocp: AcadosOcp, solver: str = "ipopt", verbose=True, self.lam_g0 = np.zeros(self.casadi_nlp['g'].shape).flatten() self.nlp_sol = None + def solve_for_x0(self, x0_bar): + """ + Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0. + """ + self.set(0, 'lbx', x0_bar) + self.set(0, 'ubx', x0_bar) - def solve_for_x0(self, x0_bar, fail_on_nonzero_status=True, print_stats_on_failure=True): - raise NotImplementedError() + status = self.solve() + u0 = self.get(0, "u") + return u0 def solve(self) -> int: """ @@ -1030,9 +1073,21 @@ def set(self, stage: int, field: str, value_: np.ndarray): self.lam_g0[self.index_map['lam_sl_in_lam_g'][stage]] = -lg_lam_soft self.lam_g0[self.index_map['lam_su_in_lam_g'][stage]] = ug_lam_soft self.lam_x0[self.index_map['sl_in_w'][stage]+self.index_map['su_in_w'][stage]] = -soft_lam + elif field == 'lbx': + self.bounds['lbx'][self.index_map['lam_bx_in_lam_w'][stage]] = value_.flatten() + elif field == 'ubx': + self.bounds['ubx'][self.index_map['lam_bx_in_lam_w'][stage]] = value_.flatten() + elif field == 'yref': + self.p[self.index_map['yref_in_p_nlp'][stage]] = value_.flatten() else: raise NotImplementedError(f"Field '{field}' is not yet implemented in set().") + def set_params_sparse(self, stage_: int, idx_values_: np.ndarray, param_values_: np.ndarray): + if not isinstance(stage_, int): + raise TypeError('stage should be integer.') + index = np.asarray(self.index_map['p_in_p_nlp'][stage_])[idx_values_] + self.p[index] = param_values_.flatten() + def cost_get(self, stage_: int, field_: str) -> np.ndarray: raise NotImplementedError() diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 80ea786576..60ad02be12 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2295,7 +2295,7 @@ def ensure_solution_sensitivities_available(self, parametric=True) -> None: has_custom_hess ) - def get_initial_cost_expression(self): + def get_initial_cost_expression(self, yref: Optional[ca.SX]=None): model = self.model if self.cost.cost_type == "LINEAR_LS": if is_empty(self.cost.Vx_0): @@ -2305,11 +2305,11 @@ def get_initial_cost_expression(self): if not is_empty(self.cost.Vz_0): y += self.cost.Vz @ model.z - residual = y - self.cost.yref_0 + residual = y - (self.cost.yref_0 if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W_0 @ residual) elif self.cost.cost_type == "NONLINEAR_LS": - residual = model.cost_y_expr_0 - self.cost.yref_0 + residual = model.cost_y_expr_0 - (self.cost.yref_0 if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W_0 @ residual) elif self.cost.cost_type == "EXTERNAL": @@ -2324,7 +2324,7 @@ def get_initial_cost_expression(self): return cost_dot - def get_path_cost_expression(self): + def get_path_cost_expression(self, yref: Optional[ca.SX]=None): model = self.model if self.cost.cost_type == "LINEAR_LS": if is_empty(self.cost.Vx): @@ -2334,11 +2334,11 @@ def get_path_cost_expression(self): if not is_empty(self.cost.Vz): y += self.cost.Vz @ model.z - residual = y - self.cost.yref + residual = y - (self.cost.yref if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W @ residual) elif self.cost.cost_type == "NONLINEAR_LS": - residual = model.cost_y_expr - self.cost.yref + residual = model.cost_y_expr - (self.cost.yref if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W @ residual) elif self.cost.cost_type == "EXTERNAL": @@ -2353,17 +2353,17 @@ def get_path_cost_expression(self): return cost_dot - def get_terminal_cost_expression(self): + def get_terminal_cost_expression(self, yref: Optional[ca.SX]=None): model = self.model if self.cost.cost_type_e == "LINEAR_LS": if is_empty(self.cost.Vx_e): return 0.0 y = self.cost.Vx_e @ model.x - residual = y - self.cost.yref_e + residual = y - (self.cost.yref_e if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) elif self.cost.cost_type_e == "NONLINEAR_LS": - residual = model.cost_y_expr_e - self.cost.yref_e + residual = model.cost_y_expr_e - (self.cost.yref_e if yref is None else yref) cost_dot = 0.5 * (residual.T @ self.cost.W_e @ residual) elif self.cost.cost_type_e == "EXTERNAL": From af3f22068e40f4c88883180ff0cabcd357a9c360 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 8 Sep 2025 15:33:08 +0200 Subject: [PATCH 131/164] Add opts to templated CMake (#1619) - Add CMake variable `ACADOS_LINK_LIBS` to define which libs to link to - Add CMake variable `ACADOS_INCLUDE_PATHS` to set the include paths manually - Add `additional_cmake_options` to `CmakeBuilder` --- .../ocp/minimal_example_ocp_cmake.py | 165 ++++++++++-------- .../acados_template/builders.py | 4 +- .../c_templates_tera/CMakeLists.in.txt | 12 +- .../c_templates_tera/multi_CMakeLists.in.txt | 10 +- 4 files changed, 115 insertions(+), 76 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py index a0521a2fcb..4e9c0c7e1b 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_ocp_cmake.py @@ -38,94 +38,123 @@ import scipy.linalg from utils import plot_pendulum -# create ocp object to formulate the OCP -ocp = AcadosOcp() - -# set model -model = export_pendulum_ode_model() -ocp.model = model - -Tf = 1.0 -nx = model.x.rows() -nu = model.u.rows() -ny = nx + nu -ny_e = nx +FMAX = 80 +T_HORIZON = 1.0 N = 20 -# set dimensions -ocp.solver_options.N_horizon = N +def create_ocp() -> AcadosOcp: + # create ocp object to formulate the OCP + ocp = AcadosOcp() -# set cost -Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) -R = 2*np.diag([1e-2]) + # set model + model = export_pendulum_ode_model() + ocp.model = model -ocp.cost.W_e = Q -ocp.cost.W = scipy.linalg.block_diag(Q, R) + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx -ocp.cost.cost_type = 'LINEAR_LS' -ocp.cost.cost_type_e = 'LINEAR_LS' + # set dimensions + ocp.solver_options.N_horizon = N -ocp.cost.Vx = np.zeros((ny, nx)) -ocp.cost.Vx[:nx,:nx] = np.eye(nx) + # set cost + Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R = 2*np.diag([1e-2]) -Vu = np.zeros((ny, nu)) -Vu[4,0] = 1.0 -ocp.cost.Vu = Vu + ocp.cost.W_e = Q + ocp.cost.W = scipy.linalg.block_diag(Q, R) -ocp.cost.Vx_e = np.eye(nx) + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' -ocp.cost.yref = np.zeros((ny, )) -ocp.cost.yref_e = np.zeros((ny_e, )) + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vx[:nx,:nx] = np.eye(nx) -# set constraints -Fmax = 80 -ocp.constraints.lbu = np.array([-Fmax]) -ocp.constraints.ubu = np.array([+Fmax]) -ocp.constraints.idxbu = np.array([0]) + Vu = np.zeros((ny, nu)) + Vu[4,0] = 1.0 + ocp.cost.Vu = Vu -ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + ocp.cost.Vx_e = np.eye(nx) -# set options -ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' -# ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_OSQP' -ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' -ocp.solver_options.integrator_type = 'ERK' -ocp.solver_options.nlp_solver_type = 'SQP' -ocp.solver_options.nlp_solver_ext_qp_res = 1 -ocp.solver_options.nlp_qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT' -ocp.solver_options.qp_solver_iter_max = 1000 -ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-3 -ocp.solver_options.qp_solver_mu0 = 1e2 -# set prediction horizon -ocp.solver_options.tf = Tf + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) -# use the CMake build pipeline -cmake_builder = ocp_get_default_cmake_builder() + # set constraints + ocp.constraints.lbu = np.array([-FMAX]) + ocp.constraints.ubu = np.array([+FMAX]) + ocp.constraints.idxbu = np.array([0]) -ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', cmake_builder=cmake_builder) + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) -simX = np.zeros((N+1, nx)) -simU = np.zeros((N, nu)) + # set options + ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' + # ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_OSQP' + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.nlp_solver_ext_qp_res = 1 + ocp.solver_options.nlp_qp_tol_strategy = 'ADAPTIVE_CURRENT_RES_JOINT' + ocp.solver_options.qp_solver_iter_max = 1000 + ocp.solver_options.nlp_qp_tol_reduction_factor = 1e-3 + ocp.solver_options.qp_solver_mu0 = 1e2 + # set prediction horizon + ocp.solver_options.tf = T_HORIZON -status = ocp_solver.solve() + return ocp -sum_qp_iter = sum(ocp_solver.get_stats("qp_iter")) -nlp_iter = ocp_solver.get_stats("nlp_iter") -print(f'nlp_iter: {nlp_iter}, total qp_iter: {sum_qp_iter}') -ocp_solver.print_statistics() -if sum_qp_iter > 75: - raise Exception(f'number of qp iterations {sum_qp_iter} is too high, expected <= 75.') +def test_cmake_link_libs(): + ocp = create_ocp() -if status != 0: + cmake_builder = ocp_get_default_cmake_builder() + # do not link against blasfeo, hpipm, m -> this should fail + cmake_builder.additional_cmake_options = '-DACADOS_LINK_LIBS=""' + try: + ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', cmake_builder=cmake_builder) + raise Exception('should have failed') + except Exception as e: + print(f'expected exception: {e}') + # remove codegen dir, to get rid of cmake cache + import shutil + shutil.rmtree('c_generated_code') + + +def test_cmake(): + ocp = create_ocp() + + # use the CMake build pipeline + cmake_builder = ocp_get_default_cmake_builder() + ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', cmake_builder=cmake_builder) + + nx = ocp.model.x.rows() + nu = ocp.model.u.rows() + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) + + status = ocp_solver.solve() + + sum_qp_iter = sum(ocp_solver.get_stats("qp_iter")) + nlp_iter = ocp_solver.get_stats("nlp_iter") + print(f'nlp_iter: {nlp_iter}, total qp_iter: {sum_qp_iter}') ocp_solver.print_statistics() - raise Exception(f'acados returned status {status}.') -# get solution -for i in range(N): - simX[i,:] = ocp_solver.get(i, "x") - simU[i,:] = ocp_solver.get(i, "u") -simX[N,:] = ocp_solver.get(N, "x") + if sum_qp_iter > 75: + raise Exception(f'number of qp iterations {sum_qp_iter} is too high, expected <= 75.') + + if status != 0: + ocp_solver.print_statistics() + raise Exception(f'acados returned status {status}.') + + # get solution + for i in range(N): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N,:] = ocp_solver.get(N, "x") + + plot_pendulum(np.linspace(0, T_HORIZON, N+1), FMAX, simU, simX, latexify=True) -plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=True) +if __name__ == "__main__": + test_cmake_link_libs() + test_cmake() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/builders.py b/interfaces/acados_template/acados_template/builders.py index eb0b143347..969928cc4e 100644 --- a/interfaces/acados_template/acados_template/builders.py +++ b/interfaces/acados_template/acados_template/builders.py @@ -56,6 +56,8 @@ def __init__(self): """A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`.""" self.options_on = None """List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`.""" + self.additional_cmake_options = None + """Additional cmake options as a single string, e.g. '-D CMAKE_CXX_FLAGS="-O3 -Wall"'; default: `None`.""" # Generate the command string for handling the cmake command. def get_cmd1_cmake(self): @@ -69,7 +71,7 @@ def get_cmd1_cmake(self): host_str = '' if self.host is not None: host_str = f' -A"{self.host}"' - return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str}{host_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"' + return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str} {self.additional_cmake_options} {generator_str}{host_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"' # Generate the command string for handling the build. def get_cmd2_build(self): diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 4f1a61896a..ee6be77e7f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -205,7 +205,7 @@ set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_co ") #-fno-diagnostics-show-line-numbers -g -include_directories( +set(ACADOS_INCLUDE_PATHS ${ACADOS_INCLUDE_PATH} ${ACADOS_INCLUDE_PATH}/acados ${ACADOS_INCLUDE_PATH}/blasfeo/include @@ -218,16 +218,20 @@ include_directories( {%- endif %} ) +include_directories(${ACADOS_INCLUDE_PATHS}) + # linker flags link_directories(${ACADOS_LIB_PATH}) # link to libraries if(UNIX) - link_libraries(acados hpipm blasfeo m {{ link_libs }}) + set(ACADOS_LINK_LIBS acados hpipm blasfeo m {{ link_libs }} CACHE STRING "Define what libs to link with acados solver") else() - link_libraries(acados hpipm blasfeo {{ link_libs }}) + set(ACADOS_LINK_LIBS acados hpipm blasfeo {{ link_libs }} CACHE STRING "Define what libs to link with acados solver") endif() +link_libraries(${ACADOS_LINK_LIBS}) + # the targets {%- if problem_class != "SIM" %} @@ -240,7 +244,7 @@ if(${BUILD_ACADOS_SOLVER_LIB}) {%- endif %} $ {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} - $ + $ {%- endif -%} ) install(TARGETS ${LIB_ACADOS_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX}) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt index 40472cde8d..503b029a5b 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/multi_CMakeLists.in.txt @@ -310,7 +310,7 @@ set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_co ") #-fno-diagnostics-show-line-numbers -g -include_directories( +set(ACADOS_INCLUDE_PATHS ${ACADOS_INCLUDE_PATH} ${ACADOS_INCLUDE_PATH}/acados ${ACADOS_INCLUDE_PATH}/blasfeo/include @@ -323,16 +323,20 @@ include_directories( {%- endif %} ) +include_directories(${ACADOS_INCLUDE_PATHS}) + # linker flags link_directories(${ACADOS_LIB_PATH}) # link to libraries if(UNIX) - link_libraries(acados hpipm blasfeo m {{ link_libs }}) + set(ACADOS_LINK_LIBS acados hpipm blasfeo m {{ link_libs }} CACHE STRING "Define what libs to link with acados solver") else() - link_libraries(acados hpipm blasfeo {{ link_libs }}) + set(ACADOS_LINK_LIBS acados hpipm blasfeo {{ link_libs }} CACHE STRING "Define what libs to link with acados solver") endif() +link_libraries(${ACADOS_LINK_LIBS}) + # the targets # ocp_shared_lib From 7a6a796b87f3ac232ae0e4540d1558827880e900 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 10 Sep 2025 15:35:40 +0200 Subject: [PATCH 132/164] Matlab checks (#1624) - check that symbolics `x`, `u`, `p`, etc are valid inputs for casadi functions and raise proper error. - avoid warnings about non used non default fields in MOCP example --- .../formulate_double_integrator_ocp.m | 17 ++++++++++++----- .../formulate_single_integrator_ocp.m | 10 ++++++++-- .../main_multiphase_ocp.m | 4 ++-- interfaces/acados_matlab_octave/AcadosModel.m | 16 +++++++++++++--- 4 files changed, 35 insertions(+), 12 deletions(-) diff --git a/examples/acados_matlab_octave/mocp_transition_example/formulate_double_integrator_ocp.m b/examples/acados_matlab_octave/mocp_transition_example/formulate_double_integrator_ocp.m index 2c19f50311..2e78bd3311 100644 --- a/examples/acados_matlab_octave/mocp_transition_example/formulate_double_integrator_ocp.m +++ b/examples/acados_matlab_octave/mocp_transition_example/formulate_double_integrator_ocp.m @@ -28,20 +28,27 @@ % POSSIBILITY OF SUCH DAMAGE.; -function ocp = formulate_double_integrator_ocp(settings) +function ocp = formulate_double_integrator_ocp(settings, first_phase_ocp) + if nargin < 2 + first_phase_ocp = 0; + end ocp = AcadosOcp(); ocp.model = get_double_integrator_model(); ocp.cost.cost_type = 'NONLINEAR_LS'; - ocp.cost.cost_type_e = 'NONLINEAR_LS'; ocp.cost.W = diag([settings.L2_COST_P, settings.L2_COST_V, settings.L2_COST_A]); - ocp.cost.W_e = diag([1e1, 1e1]); ocp.cost.yref = [0.0; 0.0; 0.0]; - ocp.cost.yref_e = [0.0; 0.0]; ocp.model.cost_y_expr = vertcat(ocp.model.x, ocp.model.u); - ocp.model.cost_y_expr_e = ocp.model.x; + + % terminal cost - not needed when formulating first phase of MOCP + if ~first_phase_ocp + ocp.cost.cost_type_e = 'NONLINEAR_LS'; + ocp.model.cost_y_expr_e = ocp.model.x; + ocp.cost.yref_e = [0.0; 0.0]; + ocp.cost.W_e = diag([1e1, 1e1]); + end u_max = 50.0; ocp.constraints.lbu = [-u_max]; diff --git a/examples/acados_matlab_octave/mocp_transition_example/formulate_single_integrator_ocp.m b/examples/acados_matlab_octave/mocp_transition_example/formulate_single_integrator_ocp.m index 20603e2bc4..05c73254fc 100644 --- a/examples/acados_matlab_octave/mocp_transition_example/formulate_single_integrator_ocp.m +++ b/examples/acados_matlab_octave/mocp_transition_example/formulate_single_integrator_ocp.m @@ -28,7 +28,10 @@ % POSSIBILITY OF SUCH DAMAGE.; -function ocp = formulate_single_integrator_ocp(settings) +function ocp = formulate_single_integrator_ocp(settings, terminal_phase_ocp) + if nargin < 2 + terminal_phase_ocp = 0; + end ocp = AcadosOcp(); ocp.model = get_single_integrator_model(); @@ -58,7 +61,10 @@ ocp.constraints.idxbx_e = [0]; end - ocp.constraints.x0 = settings.X0(1); + % initial state constraint, not needed for terminal phase OCP. + if ~terminal_phase_ocp + ocp.constraints.x0 = settings.X0(1); + end end 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 ebcbfc00a1..7452ca4846 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 @@ -37,7 +37,7 @@ % create_multiphase_ocp_solver ocp = AcadosMultiphaseOcp(N_list); -phase_1 = formulate_double_integrator_ocp(settings); +phase_1 = formulate_double_integrator_ocp(settings, 1); ocp.set_phase(phase_1, 1); phase_2 = AcadosOcp(); @@ -50,7 +50,7 @@ phase_2.cost.yref = zeros(2, 1); ocp.set_phase(phase_2, 2); -phase_3 = formulate_single_integrator_ocp(settings); +phase_3 = formulate_single_integrator_ocp(settings, 1); ocp.set_phase(phase_3, 3); % set mocp specific options diff --git a/interfaces/acados_matlab_octave/AcadosModel.m b/interfaces/acados_matlab_octave/AcadosModel.m index 094bb1d202..9f5d7b5ed4 100644 --- a/interfaces/acados_matlab_octave/AcadosModel.m +++ b/interfaces/acados_matlab_octave/AcadosModel.m @@ -227,9 +227,6 @@ function make_consistent(obj, dims) error('model.p_global should be column vector.'); end - if dims.np_global > 0 && ~(isa(obj.p_global, 'casadi.MX') || isa(obj.p_global, 'casadi.SX')) - error('model.p_global needs to be casadi.MX or casadi.SX') - end if isempty(obj.xdot) obj.xdot = empty_var; elseif ~(isa(obj.xdot, 'casadi.SX') == isSX && length(obj.xdot) == 0) && (~iscolumn(obj.xdot) || size(obj.xdot, 1) ~= dims.nx) @@ -253,6 +250,19 @@ function make_consistent(obj, dims) error('model.u should be column vector.'); end + % sanity checks + vars_and_names = {obj.x, 'x'; obj.xdot, 'xdot'; obj.u, 'u'; obj.z, 'z'; obj.p, 'p'; obj.p_global, 'p_global'}; + for i = 1:size(vars_and_names, 1) + symbol = vars_and_names{i, 1}; + var_name = vars_and_names{i, 2}; + if ~(isa(symbol, 'casadi.MX') || isa(symbol, 'casadi.SX')) + error(['model.' var_name ' must be casadi.MX or casadi.SX, got ' class(symbol)]); + end + if ~symbol.is_valid_input() + error(['model.' var_name ' must be valid CasADi symbol to be used as input for functions, got ' str(symbol)]); + end + end + % model output dimension nx_next: dimension of the next state if isa(dims, 'AcadosOcpDims') if ~isempty(obj.disc_dyn_expr) From 15794d6f6e7a6817473ead756f0eb77bcf9746df Mon Sep 17 00:00:00 2001 From: David Kiessling <74051259+david0oo@users.noreply.github.com> Date: Thu, 11 Sep 2025 17:35:42 +0200 Subject: [PATCH 133/164] Added Byrd-Omojokun relaxation factor of shifted bounds (#1626) Added option `byrd_omojokon_slack_relaxation_factor` that multiplies the slack variables in the Byrd-Omojokun multiplication with a scalar >1. This can avoid the LICQ violation of the Byrd-Omojkokun QP. --- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 12 +++++++---- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 1 + .../inconsistent_qp_linearization_test.py | 1 + .../acados_template/acados_ocp_options.py | 21 +++++++++++++++++++ .../c_templates_tera/acados_multi_solver.in.c | 3 +++ .../c_templates_tera/acados_solver.in.c | 3 +++ 6 files changed, 37 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 3d79a7423c..9374219f7c 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -117,6 +117,7 @@ void ocp_nlp_sqp_wfqp_opts_initialize_default(void *config_, void *dims_, void * opts->search_direction_mode = NOMINAL_QP; opts->watchdog_zero_slacks_max = 2; opts->allow_direction_mode_switch_to_nominal = true; + opts->byrd_omojokon_slack_relaxation_factor = 1.00001; opts->feasibility_qp_hessian_scalar = 1e-4; opts->log_pi_norm_inf = true; opts->log_lam_norm_inf = true; @@ -151,7 +152,6 @@ void ocp_nlp_sqp_wfqp_opts_set(void *config_, void *opts_, const char *field, vo char module[MAX_STR_LEN]; char *ptr_module = NULL; int module_length = 0; - // extract module name char *char_ = strchr(field, '_'); if (char_!=NULL) @@ -162,7 +162,6 @@ void ocp_nlp_sqp_wfqp_opts_set(void *config_, void *opts_, const char *field, vo module[module_length] = '\0'; // add end of string ptr_module = module; } - // pass options to QP module if ( ptr_module!=NULL && (!strcmp(ptr_module, "qp")) ) { @@ -175,6 +174,11 @@ void ocp_nlp_sqp_wfqp_opts_set(void *config_, void *opts_, const char *field, vo bool* use_constraint_hessian_in_feas_qp = (bool *) value; opts->use_constraint_hessian_in_feas_qp = *use_constraint_hessian_in_feas_qp; } + else if (!strcmp(field, "byrd_omojokon_slack_relaxation_factor")) + { + double* byrd_omojokon_slack_relaxation_factor = (double *) value; + opts->byrd_omojokon_slack_relaxation_factor = *byrd_omojokon_slack_relaxation_factor; + } else if (!strcmp(field, "search_direction_mode")) { bool* search_direction_mode = (bool *) value; @@ -1111,7 +1115,7 @@ static void setup_byrd_omojokun_bounds(ocp_nlp_dims *dims, ocp_nlp_memory *nlp_m // get lower slack tmp_lower = BLASFEO_DVECEL(relaxed_qp_out->ux + i, nx[i]+nu[i]+slack_index); // lower_bound - value - BLASFEO_DVECEL(nominal_qp_in->d+i, constr_index) -= tmp_lower; + BLASFEO_DVECEL(nominal_qp_in->d+i, constr_index) -= opts->byrd_omojokon_slack_relaxation_factor*tmp_lower; // get upper slack tmp_upper = BLASFEO_DVECEL(relaxed_qp_out->ux + i, nx[i]+nu[i]+ns[i]+nns[i] + slack_index); @@ -1120,7 +1124,7 @@ static void setup_byrd_omojokun_bounds(ocp_nlp_dims *dims, ocp_nlp_memory *nlp_m // for the slacks with upper bound we have value - slack, therefore // value <= -upper_bound + slack, // we store upper_bound - slack - BLASFEO_DVECEL(nominal_qp_in->d+i, nb[i] + ng[i] + ni_nl[i] + constr_index) -= tmp_upper; + BLASFEO_DVECEL(nominal_qp_in->d+i, nb[i] + ng[i] + ni_nl[i] + constr_index) -= opts->byrd_omojokon_slack_relaxation_factor*tmp_upper; } } } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index e036fab389..b9d2847058 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -62,6 +62,7 @@ typedef struct int watchdog_zero_slacks_max; // number of consecutive BYRD_OMOJOKUN iterations with zero slacks before switching back to NOMINAL_QP bool allow_direction_mode_switch_to_nominal; // if true, mode can switch from Byrd-Omojokun to nominal mode double feasibility_qp_hessian_scalar; // multiplication factor of feasibility QP Hessian + double byrd_omojokon_slack_relaxation_factor; // multiplication factor of slack variables in Byrd-Omojokun bound factor } ocp_nlp_sqp_wfqp_opts; diff --git a/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py b/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py index f93246e2c6..c5388392d8 100644 --- a/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py +++ b/examples/acados_python/inconsistent_qp_linearization/inconsistent_qp_linearization_test.py @@ -139,6 +139,7 @@ def create_solver_opts(N=1, solver_options.search_direction_mode = search_direction_mode solver_options.use_constraint_hessian_in_feas_qp = False solver_options.store_iterates = True + solver_options.byrd_omojokon_slack_relaxation_factor = 1.0 # set prediction horizon solver_options.tf = Tf diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 77bfcfeada..6a72dc3460 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -97,6 +97,7 @@ def __init__(self): self.__exact_hess_constr = 1 self.__eval_residual_at_max_iter = None self.__use_constraint_hessian_in_feas_qp = False + self.__byrd_omojokon_slack_relaxation_factor = 1.00001 self.__search_direction_mode = 'NOMINAL_QP' self.__allow_direction_mode_switch_to_nominal = True self.__fixed_hess = 0 @@ -1153,6 +1154,16 @@ def use_constraint_hessian_in_feas_qp(self): """ return self.__use_constraint_hessian_in_feas_qp + @property + def byrd_omojokon_slack_relaxation_factor(self): + """ + Multiplication factor in Byrd-Omojokun bounds setup. Reduces ill-conditioning, + but can allow convergence to unwanted infeasible stationary points. + Type: double, >=1 + Default: 1.00001 + """ + return self.__byrd_omojokon_slack_relaxation_factor + @property def search_direction_mode(self): """ @@ -1729,6 +1740,16 @@ def use_constraint_hessian_in_feas_qp(self, use_constraint_hessian_in_feas_qp): else: raise TypeError(f'Invalid datatype for use_constraint_hessian_in_feas_qp. Should be bool, got {type(use_constraint_hessian_in_feas_qp)}') + @byrd_omojokon_slack_relaxation_factor.setter + def byrd_omojokon_slack_relaxation_factor(self, byrd_omojokon_slack_relaxation_factor): + if isinstance(byrd_omojokon_slack_relaxation_factor, float): + if byrd_omojokon_slack_relaxation_factor >= 1.0: + self.__byrd_omojokon_slack_relaxation_factor = byrd_omojokon_slack_relaxation_factor + else: + raise ValueError(f'Invalid float for byrd_omojokon_slack_relaxation_factor. Must be >=1.0, got {byrd_omojokon_slack_relaxation_factor}') + else: + raise TypeError(f'Invalid datatype for search_direction_mode. Should be float, got {type(byrd_omojokon_slack_relaxation_factor)}') + @search_direction_mode.setter def search_direction_mode(self, search_direction_mode): search_direction_modes = ('NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP') 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 589dc39481..5e325f84ce 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 @@ -2427,6 +2427,9 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) {%- endif %} {%- if solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} +double byrd_omojokon_slack_relaxation_factor = {{ solver_options.byrd_omojokon_slack_relaxation_factor }}; +ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "byrd_omojokon_slack_relaxation_factor", &byrd_omojokon_slack_relaxation_factor); + bool use_constraint_hessian_in_feas_qp = {{ solver_options.use_constraint_hessian_in_feas_qp }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "use_constraint_hessian_in_feas_qp", &use_constraint_hessian_in_feas_qp); 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 24a85a6a12..fab3ebfe2f 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 @@ -2571,6 +2571,9 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {%- endif %} {%- if solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} + double byrd_omojokon_slack_relaxation_factor = {{ solver_options.byrd_omojokon_slack_relaxation_factor }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "byrd_omojokon_slack_relaxation_factor", &byrd_omojokon_slack_relaxation_factor); + bool use_constraint_hessian_in_feas_qp = {{ solver_options.use_constraint_hessian_in_feas_qp }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "use_constraint_hessian_in_feas_qp", &use_constraint_hessian_in_feas_qp); From 3adaf09c6c524524a893fff87e6c029af23cb8a0 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 12 Sep 2025 09:10:02 +0200 Subject: [PATCH 134/164] Zoro: check data size of custom update function (#1628) --- interfaces/acados_matlab_octave/AcadosOcp.m | 2 +- .../acados_matlab_octave/ZoroDescription.m | 26 +++-- .../acados_template/acados_ocp.py | 4 +- .../custom_update_function_zoro_template.in.c | 9 +- .../acados_template/zoro_description.py | 95 +++++++++++-------- 5 files changed, 86 insertions(+), 50 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 6484dad8ab..aac5659214 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -1320,7 +1320,7 @@ function make_consistent(self, is_mocp_phase) if opts.N_horizon == 0 error('ZORO only supported for N_horizon > 0.'); end - self.zoro_description.process(); + self.zoro_description.make_consistent(self.dims); end % Anderson acceleration diff --git a/interfaces/acados_matlab_octave/ZoroDescription.m b/interfaces/acados_matlab_octave/ZoroDescription.m index 3faf39e76a..c9ab4f9d0f 100644 --- a/interfaces/acados_matlab_octave/ZoroDescription.m +++ b/interfaces/acados_matlab_octave/ZoroDescription.m @@ -45,6 +45,7 @@ nuh_t nlh_e_t nuh_e_t + data_size end methods @@ -52,7 +53,7 @@ % Constructor - initialize the object if needed end - function obj = process(obj) + function obj = make_consistent(obj, dims) [nw, ~] = size(obj.W_mat); obj.nw = nw; if isempty(obj.unc_jac_G_mat) @@ -77,28 +78,41 @@ error('Only one of input_P0_diag and input_P0 can be True'); end + data_size = 0; % Print input note: fprintf('\nThe data of the generated custom update function consists of the concatenation of:\n'); i_component = 1; if obj.input_P0_diag - fprintf('%d) input: diag(P0)\n', i_component); + size_i = dims.nx + fprintf('%d) input: diag(P0), size: [nx] = %d\n', i_component, size_i); i_component = i_component + 1; + data_size = data_size + size_i; end if obj.input_P0 - fprintf('%d) input: P0; full matrix in column-major format\n', i_component); + size_i = dims.nx * dims.nx + fprintf('%d) input: P0; full matrix in column-major format, size: [nx*nx] = %d\n', i_component, size_i); i_component = i_component + 1; + data_size = data_size + size_i; end if obj.input_W_diag - fprintf('%d) input: diag(W)\n', i_component); + size_i = obj.nw + fprintf('%d) input: diag(W), size: [nw] = %d\n', i_component, size_i); i_component = i_component + 1; + data_size = data_size + size_i; end if obj.input_W_add_diag - fprintf('%d) input: concatenation of diag(W_gp^k) for i=0,...,N-1\n', i_component); + size_i = dims.N * obj.nw + fprintf('%d) input: concatenation of diag(W_gp^k) for i=0,...,N-1, size: [N * nw] = %d\n', i_component, size_i); i_component = i_component + 1; + data_size = data_size + size_i; end if obj.output_P_matrices - fprintf('%d) output: concatenation of colmaj(P^k) for i=0,...,N\n', i_component); + size_i = dims.nx * dims.nx * (dims.N+1) + fprintf('%d) output: concatenation of colmaj(P^k) for i=0,...,N, size: [nx*nx*(N+1)] = %d\n', i_component, size_i); + i_component = i_component + 1; + data_size = data_size + size_i; end + obj.data_size = data_size; fprintf('\n'); end diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 60ad02be12..afe83fa65a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -51,7 +51,7 @@ check_casadi_version, ACADOS_INFTY) from .penalty_utils import symmetric_huber_penalty, one_sided_huber_penalty -from .zoro_description import ZoroDescription, process_zoro_description +from .zoro_description import ZoroDescription from .casadi_function_generation import ( GenerateContext, AcadosCodegenOptions, generate_c_code_conl_cost, generate_c_code_nls_cost, generate_c_code_external_cost, @@ -1236,7 +1236,7 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None if not isinstance(self.zoro_description, ZoroDescription): raise TypeError('zoro_description should be of type ZoroDescription or None') else: - self.zoro_description = process_zoro_description(self.zoro_description) + self.zoro_description.make_consistent(dims) # nlp_solver_warm_start_first_qp_from_nlp if opts.nlp_solver_warm_start_first_qp_from_nlp and (opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon): diff --git a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c index 1fc3bf3cdc..15d9b27d67 100644 --- a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c +++ b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c @@ -335,7 +335,7 @@ static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_d mem->offset_W_add_diag += nw; // W_diag {% endif %} - mem->offset_P_out += mem->offset_W_add_diag; + mem->offset_P_out = mem->offset_W_add_diag; {%- if zoro_description.input_W_add_diag %} mem->offset_P_out += N * nw; {% endif %} @@ -933,6 +933,13 @@ int custom_update_function({{ model.name }}_solver_capsule* capsule, double* dat int nx = {{ dims.nx }}; int nw = {{ zoro_description.nw }}; +{%- if zoro_description.output_P_matrices or zoro_description.input_W_diag and not zoro_description.input_W_add_diag -%} + if (data_len != {{ zoro_description.data_size }}) + { + printf("custom_update_zoro: data_length does not match expected one. Got %d, expected {{ zoro_description.data_size }}\n", data_len); + exit(1); + } +{%- endif %} {%- if zoro_description.input_P0_diag or zoro_description.input_P0 %} if (data_len > 0) diff --git a/interfaces/acados_template/acados_template/zoro_description.py b/interfaces/acados_template/acados_template/zoro_description.py index 2afdf1dec6..66a78fc79e 100644 --- a/interfaces/acados_template/acados_template/zoro_description.py +++ b/interfaces/acados_template/acados_template/zoro_description.py @@ -28,6 +28,7 @@ from dataclasses import dataclass, field import numpy as np +from .acados_dims import AcadosOcpDims @dataclass @@ -89,47 +90,61 @@ class ZoroDescription: output_P_matrices: bool = False """Determines if the matrices P_k are outputs of the custom update function""" + data_size: int = 0 + """size of data vector when calling custom update, computed automatically""" -def process_zoro_description(zoro_description: ZoroDescription): - zoro_description.nw, _ = zoro_description.W_mat.shape - if zoro_description.unc_jac_G_mat is None: - zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw) - zoro_description.nlbx_t = len(zoro_description.idx_lbx_t) - zoro_description.nubx_t = len(zoro_description.idx_ubx_t) - zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t) - zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t) - zoro_description.nlbu_t = len(zoro_description.idx_lbu_t) - zoro_description.nubu_t = len(zoro_description.idx_ubu_t) - zoro_description.nlg_t = len(zoro_description.idx_lg_t) - zoro_description.nug_t = len(zoro_description.idx_ug_t) - zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t) - zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t) - zoro_description.nlh_t = len(zoro_description.idx_lh_t) - zoro_description.nuh_t = len(zoro_description.idx_uh_t) - zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t) - zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t) - if zoro_description.input_P0_diag and zoro_description.input_P0: - raise ValueError("Only one of input_P0_diag and input_P0 can be True") + def make_consistent(self, dims: AcadosOcpDims) -> None: + self.nw, _ = self.W_mat.shape + if self.unc_jac_G_mat is None: + self.unc_jac_G_mat = np.eye(self.nw) + self.nlbx_t = len(self.idx_lbx_t) + self.nubx_t = len(self.idx_ubx_t) + self.nlbx_e_t = len(self.idx_lbx_e_t) + self.nubx_e_t = len(self.idx_ubx_e_t) + self.nlbu_t = len(self.idx_lbu_t) + self.nubu_t = len(self.idx_ubu_t) + self.nlg_t = len(self.idx_lg_t) + self.nug_t = len(self.idx_ug_t) + self.nlg_e_t = len(self.idx_lg_e_t) + self.nug_e_t = len(self.idx_ug_e_t) + self.nlh_t = len(self.idx_lh_t) + self.nuh_t = len(self.idx_uh_t) + self.nlh_e_t = len(self.idx_lh_e_t) + self.nuh_e_t = len(self.idx_uh_e_t) - # Print input note: - print(f"\nThe data of the generated custom update function consists of the concatenation of:") - i_component = 1 - if zoro_description.input_P0_diag: - print(f"{i_component}) input: diag(P0)") - i_component += 1 - if zoro_description.input_P0: - print(f"{i_component}) input: P0; full matrix in column-major format") - i_component += 1 - if zoro_description.input_W_diag: - print(f"{i_component}) input: diag(W)") - i_component += 1 - if zoro_description.input_W_add_diag: - print(f"{i_component}) input: concatenation of diag(W_gp^k) for i=0,...,N-1") - i_component += 1 - if zoro_description.output_P_matrices: - print(f"{i_component}) output: concatenation of colmaj(P^k) for i=0,...,N") - i_component += 1 - print("\n") + if self.input_P0_diag and self.input_P0: + raise Exception("Only one of input_P0_diag and input_P0 can be True") - return zoro_description + # Print input note: + print(f"\nThe data of the generated custom update function consists of the concatenation of:") + i_component = 1 + data_size = 0 + if self.input_P0_diag: + size_i = dims.nx + print(f"{i_component}) input: diag(P0), size: [nx] = {size_i}") + i_component += 1 + data_size += size_i + if self.input_P0: + size_i = dims.nx ** 2 + print(f"{i_component}) input: P0; full matrix in column-major format, size: [nx*nx] = {size_i}") + i_component += 1 + data_size += size_i + if self.input_W_diag: + size_i = self.nw + print(f"{i_component}) input: diag(W), size: [nw] = {size_i}") + i_component += 1 + data_size += size_i + if self.input_W_add_diag: + size_i = dims.N * self.nw + print(f"{i_component}) input: concatenation of diag(W_gp^k) for i=0,...,N-1, size: [N * nw] = {size_i}") + i_component += 1 + data_size += size_i + if self.output_P_matrices: + size_i = dims.nx * dims.nx * (dims.N+1) + print(f"{i_component}) output: concatenation of colmaj(P^k) for i=0,...,N, size: [nx*nx*(N+1)] = {size_i}") + i_component += 1 + data_size += size_i + + self.data_size = data_size + print("\n") From 67ce902df03cb611f10efe72f4cbbb54da2e858a Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Fri, 12 Sep 2025 15:02:06 +0200 Subject: [PATCH 135/164] `AcadosCasadiOcp`: Add support for GN approximation for Nonlinear cost (#1629) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Enable the Gauss–Newton approximation in `AcadosCasadiOcp` for nonlinear LS, aligned with the GN approx in Acados - Modify test for validation --------- Co-authored-by: Jonathan Frey --- .../ocp/test_casadi_formulation.py | 29 +++++++ .../acados_casadi_ocp_solver.py | 83 ++++++++++++++----- 2 files changed, 91 insertions(+), 21 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py index 58aa448f0f..262a7bdeaa 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py +++ b/examples/acados_python/pendulum_on_cart/ocp/test_casadi_formulation.py @@ -1,3 +1,30 @@ +# 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.; import sys sys.path.insert(0, '../common') @@ -85,5 +112,7 @@ def main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", main(cost_version="LS", constraint_version='bu') main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=True) main(cost_version="LS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=False) + main(cost_version="NLS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=True) + main(cost_version="NLS", constraint_version='h', casadi_solver_name="ipopt", use_acados_hessian=False) main(cost_version="LS", constraint_version='bu', casadi_solver_name="fatrop") main(cost_version="LS", constraint_version='h', casadi_solver_name="fatrop") diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 3ae0c37a19..64f480dca3 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -293,15 +293,21 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): dr_dw = ca.jacobian(r_in_nlp, w) hess_l += dr_dw.T @ outer_hess_r @ dr_dw - ### Cost + ### Cost and residual nlp_cost = 0 + residual_list = [] for i in range(N_horizon+1): - xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, p_for_model, ns, zl, Zl, zu, Zu = \ - self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, p_nlp, sl_nodes, su_nodes, ocp, dims, cost) + xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, residual_expr_i, p_for_model, ns, W_mat, zl, Zl, zu, Zu = \ + self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, p_nlp, sl_nodes, su_nodes, ocp) cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, p_for_model, model.p_global], [cost_expr_i]) cost_i = cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) nlp_cost += solver_options.cost_scaling[i] * cost_i + + if residual_expr_i is not None: + residual_fun_i = ca.Function(f'residual_fun_{i}', [model.x, model.u, p_for_model, model.p_global], [residual_expr_i]) + residual_i = ca.sqrt(solver_options.cost_scaling[i]) * ca.sqrt(W_mat) @ residual_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) + residual_list.append(residual_i) if ns: penalty_expr_i = 0.5 * ca.mtimes(sl_node_i.T, ca.mtimes(np.diag(Zl), sl_node_i)) + \ ca.mtimes(zl.reshape(-1, 1).T, sl_node_i) + \ @@ -311,9 +317,17 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): if with_hessian: lam_f = ca_symbol('lam_f', 1, 1) - if ocp.solver_options.hessian_approx == 'EXACT' or \ - (ocp.cost.cost_type == "LINEAR_LS" and ocp.cost.cost_type_0 == "LINEAR_LS" and ocp.cost.cost_type_e == "LINEAR_LS"): + if (ocp.cost.cost_type == "LINEAR_LS" and ocp.cost.cost_type_0 == "LINEAR_LS" and ocp.cost.cost_type_e == "LINEAR_LS"): hess_l += lam_f * ca.hessian(nlp_cost, w)[0] + elif (ocp.cost.cost_type == "NONLINEAR_LS" and ocp.cost.cost_type_0 == "NONLINEAR_LS" and ocp.cost.cost_type_e == "NONLINEAR_LS"): + if ocp.solver_options.hessian_approx == 'EXACT': + hess_l += lam_f * ca.hessian(nlp_cost, w)[0] + elif ocp.solver_options.hessian_approx == 'GAUSS_NEWTON': + Gauss_Newton = 0 + for i in range(len(residual_list)): + dr_dw = ca.jacobian(residual_list[i], w) + Gauss_Newton += dr_dw.T @ dr_dw + hess_l += lam_f * Gauss_Newton else: raise NotImplementedError("Hessian approximation not implemented for this cost type.") lam_g_vec = ca.vertcat(*lam_g) @@ -506,43 +520,70 @@ def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr self._index_map['lam_su_in_lam_g'][i].append(self.offset_gnl) self.offset_gnl += 1 - def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, p_nlp, sl_node, su_node, ocp: AcadosOcp, dims, cost): + def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, p_nlp, sl_node, su_node, ocp: AcadosOcp): """ Helper function to get the cost node for a given stage. """ + dims = ocp.dims + model = ocp.model + cost = ocp.cost + p_index = self._index_map['p_in_p_nlp'][i] + yref_index = self._index_map['yref_in_p_nlp'][i] + yref = p_nlp[yref_index] if i == 0: - p_index = self._index_map['p_in_p_nlp'][0] - yref_index = self._index_map['yref_in_p_nlp'][0] + if cost.cost_type_0 == "NONLINEAR_LS": + y = ocp.model.cost_y_expr_0 + residual_expr = y - yref + elif cost.cost_type_0 == "LINEAR_LS": + y = cost.Vx_0 @ model.x + cost.Vu_0 @ model.u + residual_expr = y - yref + else: + residual_expr = None return (xtraj_node[0], utraj_node[0], p_nlp[p_index + yref_index], sl_node[0], su_node[0], - ocp.get_initial_cost_expression(p_nlp[yref_index]), - ca.vertcat(ocp.model.p, p_nlp[yref_index]), - dims.ns_0, cost.zl_0, cost.Zl_0, cost.zu_0, cost.Zu_0) + ocp.get_initial_cost_expression(yref), + residual_expr, + ca.vertcat(ocp.model.p, yref), + dims.ns_0, cost.W_0, cost.zl_0, cost.Zl_0, cost.zu_0, cost.Zu_0) elif i < N_horizon: - p_index = self._index_map['p_in_p_nlp'][i] - yref_index = self._index_map['yref_in_p_nlp'][i] + if cost.cost_type_0 == "NONLINEAR_LS": + y = ocp.model.cost_y_expr + residual_expr = y - yref + elif cost.cost_type_0 == "LINEAR_LS": + y = cost.Vx @ model.x + cost.Vu @ model.u + residual_expr = y - yref + else: + residual_expr = None return (xtraj_node[i], utraj_node[i], p_nlp[p_index + yref_index], sl_node[i], su_node[i], - ocp.get_path_cost_expression(p_nlp[yref_index]), - ca.vertcat(ocp.model.p, p_nlp[yref_index]), - dims.ns, cost.zl, cost.Zl, cost.zu, cost.Zu) + ocp.get_path_cost_expression(yref), + residual_expr, + ca.vertcat(ocp.model.p, yref), + dims.ns, cost.W, cost.zl, cost.Zl, cost.zu, cost.Zu) else: - p_index = self._index_map['p_in_p_nlp'][-1] - yref_index = self._index_map['yref_in_p_nlp'][-1] + if cost.cost_type_0 == "NONLINEAR_LS": + y = ocp.model.cost_y_expr_e + residual_expr = y - yref + elif cost.cost_type_0 == "LINEAR_LS": + y = cost.Vx_e @ model.x + residual_expr = y - yref + else: + residual_expr = None return (xtraj_node[-1], [], p_nlp[p_index + yref_index], sl_node[-1], su_node[-1], - ocp.get_terminal_cost_expression(p_nlp[yref_index]), - ca.vertcat(ocp.model.p, p_nlp[yref_index]), - dims.ns_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) + ocp.get_terminal_cost_expression(yref), + residual_expr, + ca.vertcat(ocp.model.p, yref), + dims.ns_e, cost.W_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims): """ From 8c554b69bae3c85c8621f1cb364ae5846094c751 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 12 Sep 2025 16:03:15 +0200 Subject: [PATCH 136/164] `AcadosCasadi` fixes (#1630) --- .../acados_casadi_ocp_solver.py | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 64f480dca3..441deb3e40 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -807,7 +807,7 @@ def solve(self) -> int: # statistics solver_stats = self.casadi_solver.stats() - # timing = solver_stats['t_proc_total'] + # timing = solver_stats['t_proc_total'] self.status = solver_stats['return_status'] if 'return_status' in solver_stats else solver_stats['success'] self.nlp_iter = solver_stats['iter_count'] if 'iter_count' in solver_stats else None self.time_total = solver_stats['t_wall_total'] if 't_wall_total' in solver_stats else None @@ -901,7 +901,7 @@ def get(self, stage: int, field: str): lam = np.concatenate((lbu_lam, lbx_lam, lbg_lam, ubu_lam, ubx_lam, ubg_lam, lam_soft)) return lam.flatten() elif field in ['z']: - return np.empty((0, 1)) # Only empty is supported for now. TODO: extend. + return np.empty((0,)) # Only empty is supported for now. TODO: extend. else: raise NotImplementedError(f"Field '{field}' is not implemented in AcadosCasadiOcpSolver") @@ -966,6 +966,18 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: n_lam_i = 2 * (dims.nbx_e + dims.ng_e + dims.nh_e + dims.nphi_e) self.set(i, 'lam', value_[offset : offset + n_lam_i]) offset += n_lam_i + elif field_ in ['sl', 'su']: + offset = 0 + for i in range(dims.N+1): + if i == 0: + ns_i = dims.ns_0 + elif i < dims.N: + ns_i = dims.ns + elif i == dims.N: + ns_i = dims.ns_e + if ns_i > 0: + self.set(i, field_, value_[offset : offset + ns_i]) + offset += ns_i else: raise NotImplementedError(f"Field '{field_}' is not yet implemented in set_flat().") @@ -1172,9 +1184,9 @@ def get_constraints_value(self, stage: int): ub = ca.vertcat(self.bounds['ubx'][self.index_map['lam_bx_in_lam_w'][stage]], self.bounds['ubg'][self.index_map['lam_gnl_in_lam_g'][stage]]).full().flatten() return constraints_value, lambda_values, lb, ub - + def get_constraints_indices(self, stage: int): - """ + """ Get the indices of the constraints for a given stage. This function distinguishes between inequality and equality constraints returns indices of From 5da8b905b5720c5a9f0e12de478ff3ce1137454d Mon Sep 17 00:00:00 2001 From: Josua Christoph Lindemann <107478604+ArgoJ@users.noreply.github.com> Date: Mon, 15 Sep 2025 16:29:36 +0200 Subject: [PATCH 137/164] Ros2 Node Generation (#1622) A generic Ros2 Humble Node generation for a simple MPC including generic messages. Create your `AcadosOcp` and add the `AcadosOcpRosOptions` class to the ocp variable `ros_opts` as below: ```python from acados_template import AcadosOcpRosOptions, AcadosOcp ocp = AcadosOcp() ... ocp.ros_opts = AcadosOcpRosOptions() ocp.ros_opts.package_name = "ocp_pkg" ``` That's it. Two packages are created next to the `c_generated_code` folder. One which includes the interface for the messages (here `ocp_pkg_interface`) to communicate with the main package, and the main package with MPC functionality (here `ocp_pkg`). All weights and constraints are implemented as node parameters, which can be changed during runtime. The change of `ts` only changes the control loop timer and not yet the `ts` of the OCP solver. The references, OCP parameters as well as state can be updated via the given message which are included in the interface package. Look at the generated `Redme.md` in the main folder to see how to build, start and test the generated packages. This MPC is triggered by a timer, other versions like topic trigger are not implemented yet. Other architecture types like lifecycle nodes are also not implemented yet as well as services or actions (other form of messages). Similarly, a ROS2 node for the `AcadosSimSolver` can be generated. Additionally, some improvements to the CI workflow are in this PR. --------- Co-authored-by: Jonathan Frey --- .../setup-python-environment/action.yml | 17 + .../setup-system-dependencies/action.yml | 11 + .../actions/setup-test-environment/action.yml | 28 + .github/actions/test-ros2-package/action.yml | 97 +++ .github/linux/export_paths.sh | 12 +- .github/linux/install_ros2_humble.sh | 58 ++ .github/workflows/core_build.yml | 105 +++ .github/workflows/full_build.yml | 392 +++++----- README.md | 2 +- .../acados_python/chain_mass/plot_utils.py | 2 +- .../ros2/example_ros_minimal_ocp.py | 152 ++++ .../ros2/example_ros_minimal_sim.py | 54 ++ .../acados_template/__init__.py | 3 + .../acados_template/acados_ocp.py | 81 ++- .../acados_template/acados_sim.py | 133 +++- .../ocp_interface_templates/CMakeLists.in.txt | 29 + .../ControlInput.in.msg | 8 + .../ocp_interface_templates/Parameters.in.msg | 5 + .../ros2/ocp_interface_templates/README.in.md | 17 + .../ocp_interface_templates/References.in.msg | 13 + .../ros2/ocp_interface_templates/State.in.msg | 5 + .../ocp_interface_templates/package.in.xml | 26 + .../ros2/ocp_node_templates/CMakeLists.in.txt | 106 +++ .../ros2/ocp_node_templates/README.in.md | 25 + .../ros2/ocp_node_templates/config.in.hpp | 19 + .../ros2/ocp_node_templates/node.in.cpp | 681 ++++++++++++++++++ .../ros2/ocp_node_templates/node.in.h | 165 +++++ .../ros2/ocp_node_templates/package.in.xml | 23 + .../ros2/ocp_node_templates/test.launch.in.py | 205 ++++++ .../ros2/ocp_node_templates/utils.in.hpp | 97 +++ .../sim_interface_templates/CMakeLists.in.txt | 23 + .../ControlInput.in.msg | 5 + .../ros2/sim_interface_templates/README.in.md | 17 + .../ros2/sim_interface_templates/State.in.msg | 8 + .../sim_interface_templates/package.in.xml | 26 + .../ros2/sim_node_templates/CMakeLists.in.txt | 79 ++ .../ros2/sim_node_templates/README.in.md | 25 + .../ros2/sim_node_templates/config.in.hpp | 19 + .../ros2/sim_node_templates/node.in.cpp | 231 ++++++ .../ros2/sim_node_templates/node.in.h | 92 +++ .../ros2/sim_node_templates/package.in.xml | 23 + .../ros2/sim_node_templates/test.launch.in.py | 137 ++++ .../ros2/sim_node_templates/utils.in.hpp | 97 +++ .../acados_template/ros2/__init__.py | 3 + .../acados_template/ros2/ocp_node.py | 105 +++ .../acados_template/ros2/sim_node.py | 70 ++ .../acados_template/ros2/utils.py | 126 ++++ 47 files changed, 3422 insertions(+), 235 deletions(-) create mode 100644 .github/actions/setup-python-environment/action.yml create mode 100644 .github/actions/setup-system-dependencies/action.yml create mode 100644 .github/actions/setup-test-environment/action.yml create mode 100644 .github/actions/test-ros2-package/action.yml create mode 100755 .github/linux/install_ros2_humble.sh create mode 100644 .github/workflows/core_build.yml create mode 100644 examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py create mode 100644 examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py create mode 100644 interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp create mode 100644 interfaces/acados_template/acados_template/ros2/__init__.py create mode 100644 interfaces/acados_template/acados_template/ros2/ocp_node.py create mode 100644 interfaces/acados_template/acados_template/ros2/sim_node.py create mode 100644 interfaces/acados_template/acados_template/ros2/utils.py diff --git a/.github/actions/setup-python-environment/action.yml b/.github/actions/setup-python-environment/action.yml new file mode 100644 index 0000000000..c426098d10 --- /dev/null +++ b/.github/actions/setup-python-environment/action.yml @@ -0,0 +1,17 @@ +name: 'Setup Python Environment' +description: 'Exports necessary paths and installs the Python interface and Tera.' + +runs: + using: "composite" + steps: + - name: Export Paths + shell: bash + run: ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} + + - name: Install Python interface + shell: bash + run: ${{ github.workspace }}/.github/linux/install_python.sh + + - name: Install Tera + shell: bash + run: ${{ github.workspace }}/.github/linux/install_tera.sh \ No newline at end of file diff --git a/.github/actions/setup-system-dependencies/action.yml b/.github/actions/setup-system-dependencies/action.yml new file mode 100644 index 0000000000..831966ce54 --- /dev/null +++ b/.github/actions/setup-system-dependencies/action.yml @@ -0,0 +1,11 @@ +name: 'Setup System Dependencies' +description: 'Installs system dependencies like BLAS and LAPACK.' + +runs: + using: "composite" + steps: + - name: Install BLAS and LAPACK + run: | + sudo apt-get update + sudo apt-get install -y libblas-dev liblapack-dev + shell: bash \ No newline at end of file diff --git a/.github/actions/setup-test-environment/action.yml b/.github/actions/setup-test-environment/action.yml new file mode 100644 index 0000000000..fe6bfb0135 --- /dev/null +++ b/.github/actions/setup-test-environment/action.yml @@ -0,0 +1,28 @@ +name: 'Setup Test Environment' +description: 'Downloads build artifacts (lib, build, include).' + +inputs: + artifact-name-prefix: + description: 'The unique prefix of the artifacts to download.' + required: true + +runs: + using: "composite" + steps: + - name: Download lib artifact + uses: actions/download-artifact@v4 + with: + name: lib-${{ inputs.artifact-name-prefix }} + path: ${{ github.workspace }}/lib + + - name: Download build artifact + uses: actions/download-artifact@v4 + with: + name: build-${{ inputs.artifact-name-prefix }} + path: ${{ github.workspace }}/build + + - name: Download include artifact + uses: actions/download-artifact@v4 + with: + name: include-${{ inputs.artifact-name-prefix }} + path: ${{ github.workspace }}/include diff --git a/.github/actions/test-ros2-package/action.yml b/.github/actions/test-ros2-package/action.yml new file mode 100644 index 0000000000..9d8974da34 --- /dev/null +++ b/.github/actions/test-ros2-package/action.yml @@ -0,0 +1,97 @@ +name: 'Test ROS2 Package' +description: 'Sources ROS, creates an isolated workspace, then builds and tests a specific ROS2 package.' + +inputs: + python-file-path: + description: 'Path to the Python script to run for code generation.' + required: true + default: '' + source-link-paths: + description: 'A multiline string of all source directories to be symlinked into the workspace.' + required: false + default: '' + source-file-paths: + description: 'A multiline string of all source files to be copied into the workspace.' + required: false + default: '' + ros-distro: + description: 'The ROS2 distribution to use (e.g., humble, iron).' + required: false + default: 'humble' + +runs: + using: 'composite' + steps: + - name: Set up Workspace with Symbolic Links + shell: bash + run: | + set -e + WS_PATH="${{ github.workspace }}/ros2_test_ws" + mkdir -p $WS_PATH/src + + echo "Copying python script to workspace..." + cp "${{ inputs.python-file-path }}" "$WS_PATH/src/" + + - name: Create Symbolic Links + if: ${{ inputs.source-link-paths != '' }} + shell: bash + working-directory: ${{ github.workspace }}/ros2_test_ws/src + run: | + echo "Creating symbolic links for all specified source paths..." + echo "${{ inputs.source-link-paths }}" | while IFS= read -r link_path; do + if [ -n "$link_path" ]; then + echo "- Linking '$link_path'..." + ln -s "$link_path" "." + fi + done + + - name: Create Copies of Source Files + if: ${{ inputs.source-file-paths != '' }} + shell: bash + working-directory: ${{ github.workspace }}/ros2_test_ws/src + run: | + echo "Creating copies for all specified source paths..." + echo "${{ inputs.source-file-paths }}" | while IFS= read -r copy_path; do + if [ -n "$copy_path" ]; then + echo "- Copying '$copy_path'..." + cp "$copy_path" "." + fi + done + + - name: Verify Workspace Setup + shell: bash + working-directory: ${{ github.workspace }}/ros2_test_ws/src + run: | + echo "Workspace source content:" + ls -la + + - name: Generate Code with Python Script + shell: bash + working-directory: ${{ github.workspace }}/ros2_test_ws/src + run: | + SCRIPT_NAME=$(basename ${{ inputs.python-file-path }}) + echo "Running Python code generator: $SCRIPT_NAME" + source ${{ github.workspace }}/acadosenv/bin/activate + python $SCRIPT_NAME + + - name: Build and Test Package + shell: bash + working-directory: ${{ github.workspace }}/ros2_test_ws + run: | + set -e + source /opt/ros/${{ inputs.ros-distro }}/setup.bash + rosdep update || echo "rosdep update failed but continuing." + rosdep install --from-paths src --ignore-src -r -y --rosdistro ${{ inputs.ros-distro }} + + echo "Building and testing ROS2 packages..." + colcon build + colcon test \ + --event-handlers console_direct+ \ + --pytest-args "--reruns 3 --reruns-delay 1" + + - name: Tear down Workspace + if: always() + shell: bash + run: | + echo "Cleaning up workspace..." + rm -rf ${{ github.workspace }}/ros2_test_ws diff --git a/.github/linux/export_paths.sh b/.github/linux/export_paths.sh index cbd927d5b8..b893274694 100755 --- a/.github/linux/export_paths.sh +++ b/.github/linux/export_paths.sh @@ -29,10 +29,10 @@ # POSSIBILITY OF SUCH DAMAGE.; # -echo "ACADOS_SOURCE_DIR=$1/acados" >> $GITHUB_ENV -echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV -echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV -echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV -echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV -echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/pendulum_on_cart_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code:${1}/acados/examples/acados_python/p_global_example/c_generated_code_single_phase:${1}/acados/examples/acados_python/pendulum_on_cart/sim/c_generated_code" >> $GITHUB_ENV +echo "ACADOS_SOURCE_DIR=$1" >> $GITHUB_ENV +echo "ACADOS_INSTALL_DIR=$1" >> $GITHUB_ENV +echo "LD_LIBRARY_PATH=$1/lib:$LD_LIBRARY_PATH" >> $GITHUB_ENV +echo "MATLABPATH=$MATLABPATH:$1/interfaces/acados_matlab_octave:$1//interfaces/acados_matlab_octave/acados_template_mex:${1}/external/casadi-matlab" >> $GITHUB_ENV +echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/interfaces/acados_matlab_octave:${1}/interfaces/acados_matlab_octave/acados_template_mex:${1}/external/casadi-octave" >> $GITHUB_ENV +echo "LD_RUN_PATH=${1}/examples/acados_matlab_octave/test/c_generated_code:${1}/examples/acados_matlab_octave/pendulum_on_cart_model/c_generated_code:${1}/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/examples/acados_matlab_octave/lorentz/c_generated_code:${1}/examples/acados_python/p_global_example/c_generated_code:${1}/examples/acados_python/p_global_example/c_generated_code_single_phase:${1}/examples/acados_python/pendulum_on_cart/sim/c_generated_code" >> $GITHUB_ENV echo "ENV_RUN=true" >> $GITHUB_ENV diff --git a/.github/linux/install_ros2_humble.sh b/.github/linux/install_ros2_humble.sh new file mode 100755 index 0000000000..649dc33b00 --- /dev/null +++ b/.github/linux/install_ros2_humble.sh @@ -0,0 +1,58 @@ +#!/bin/bash +# +# 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. +# + +set -e + +# 1. install dependencies for ROS 2 Humble +echo "Setting up system locales and ROS 2 package sources..." +sudo apt-get update +sudo apt-get install -y locales software-properties-common curl +sudo locale-gen en_US en_US.UTF-8 +sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +export LANG=en_US.UTF-8 +sudo add-apt-repository universe -y + +# 2. add the ROS 2 GPG key and repository +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + +# 3. update package index +sudo apt-get update + +# 4. check if ROS 2 is already installed (cached), if not install it +echo "Installing ros-humble-ros-base..." +sudo apt-get install -y ros-humble-ros-base + +# 5. rosdep init and update +echo "Initializing and updating rosdep..." +sudo apt-get install -y python3-rosdep ros-dev-tools +sudo rosdep init || echo "rosdep already initialized." +rosdep update \ No newline at end of file diff --git a/.github/workflows/core_build.yml b/.github/workflows/core_build.yml new file mode 100644 index 0000000000..d489e64af1 --- /dev/null +++ b/.github/workflows/core_build.yml @@ -0,0 +1,105 @@ +name: Reusable Core Build + +on: + workflow_call: + outputs: + artifact-name-prefix: + description: "The unique prefix for artifact names for this run" + value: ${{ jobs.core_build.outputs.artifact-name-prefix }} + +env: + BUILD_TYPE: Release + ACADOS_PYTHON: ON + ACADOS_OCTAVE: ON + ACADOS_WITH_OSQP: ON + ACADOS_WITH_QPOASES: ON + ACADOS_WITH_DAQP: ON + ACADOS_WITH_QPDUNES: ON + ACADOS_ON_CI: ON + + +jobs: + core_build: + runs-on: ubuntu-22.04 + + outputs: + artifact-name-prefix: ${{ steps.vars.outputs.PREFIX }} + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Set up variables + id: vars + run: echo "PREFIX=build-${{ github.run_id }}-${{ github.run_attempt }}" >> $GITHUB_OUTPUT + + - name: Submodule fingerprint + id: submods + run: | + set -euo pipefail + git submodule status --recursive \ + | awk '{print $1 " " $2}' \ + | sed 's/^[-+ ]*//' \ + | sort -k2,2 \ + | sha256sum | cut -d' ' -f1 > submods.sha + echo "sha=$(cat submods.sha)" >> "$GITHUB_OUTPUT" + + - name: Cache Build Artifacts + id: cache-build + uses: actions/cache@v4 + with: + path: | + ${{ github.workspace }}/build + ${{ github.workspace }}/lib + ${{ github.workspace }}/include + key: ${{ runner.os }}-${{ env.BUILD_TYPE }}-acados-${{ steps.submods.outputs.sha }}-${{ hashFiles( + '.gitmodules', + 'CMakeLists.txt', + 'cmake/**/*.cmake', + 'acados/**', + 'interfaces/acados_c/**' + ) }} + + - name: Create Build Environment & Configure + if: steps.cache-build.outputs.cache-hit != 'true' + run: | + cmake -E make_directory ${{ github.workspace }}/build + cd ${{ github.workspace }}/build + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE \ + -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES \ + -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP \ + -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES \ + -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP \ + -DACADOS_PYTHON=$ACADOS_PYTHON \ + -DACADOS_OCTAVE=OFF \ + -DACADOS_WITH_OPENMP=ON \ + -DACADOS_NUM_THREADS=1 + + - name: Build & Install + if: steps.cache-build.outputs.cache-hit != 'true' + working-directory: ${{ github.workspace }}/build + run: | + cmake --build . --config $BUILD_TYPE + make install -j4 + + - name: Store shared libs (/lib) + uses: actions/upload-artifact@v4 + with: + name: lib-${{ steps.vars.outputs.PREFIX }} + path: ${{ github.workspace }}/lib/ + + - name: Store build scripts (/build) + uses: actions/upload-artifact@v4 + with: + name: build-${{ steps.vars.outputs.PREFIX }} + path: | + ${{ github.workspace }}/build/ + !${{ github.workspace }}/**/*.dir + + - name: Store include directory (/include) + uses: actions/upload-artifact@v4 + with: + name: include-${{ steps.vars.outputs.PREFIX }} + path: ${{ github.workspace }}/include/ diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 7ade1e3075..c24fd6743a 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -9,266 +9,199 @@ on: branches: - '*' -env: - BUILD_TYPE: Release - ACADOS_PYTHON: ON - ACADOS_OCTAVE: ON - ACADOS_WITH_OSQP: ON - ACADOS_WITH_QPOASES: ON - ACADOS_WITH_DAQP: ON - ACADOS_WITH_QPDUNES: ON - ACADOS_ON_CI: ON - jobs: - core_build: - runs-on: ubuntu-22.04 - - steps: - - uses: actions/checkout@v4 - with: - submodules: 'recursive' - - - name: Create Build Environment - working-directory: ${{runner.workspace}} - run: cmake -E make_directory ${{runner.workspace}}/acados/build + call_core_build: + uses: ./.github/workflows/core_build.yml - - name: Configure CMake - shell: bash - working-directory: ${{runner.workspace}}/acados/build - run: | - cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1 - - - name: Build & Install - working-directory: ${{runner.workspace}}/acados/build - shell: bash - run: | - cmake --build . --config $BUILD_TYPE - make install -j4 - - - name: Store shared libs (/lib) - uses: actions/upload-artifact@v4 - with: - name: lib - if-no-files-found: error - path: ${{runner.workspace}}/acados/lib/ - compression-level: 0 - overwrite: true - - - name: Store build scripts (/build) - uses: actions/upload-artifact@v4 - with: - name: build - if-no-files-found: error - path: | - ${{runner.workspace}}/acados/build/ - !${{runner.workspace}}/**/*.dir - compression-level: 0 - overwrite: true - # exclude object files in .dir directories - - - name: Store include directory (/include) - uses: actions/upload-artifact@v4 - with: - name: include - if-no-files-found: error - path: ${{runner.workspace}}/acados/include/ - compression-level: 0 - overwrite: true +# ===== PYTHON TESTS ===== python_interface: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - - name: Export Paths - working-directory: ${{runner.workspace}}/acados - shell: bash - run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} - - - name: Install Python interface - working-directory: ${{runner.workspace}}/acados - shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_python.sh'' - - - name: Install Tera - working-directory: ${{runner.workspace}}/acados - shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment - name: Run CMake python tests (ctest) - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate + source ${{ github.workspace }}/acadosenv/bin/activate ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4; + - name: Rerun failed tests with verbose output + if: failure() + working-directory: ${{ github.workspace }}/build + shell: bash + run: | + echo "--- CTest failed. Re-running all failing tests with verbose output: ---" + source ${{ github.workspace }}/acadosenv/bin/activate + ctest -C Release --rerun-failed -V + exit 1 + + python_interface_new_casadi_and_py2octave: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados - - - name: Export Paths - working-directory: ${{runner.workspace}}/acados - shell: bash - run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - - name: Install Python interface - working-directory: ${{runner.workspace}}/acados - shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_python.sh'' - - - name: Install Tera - working-directory: ${{runner.workspace}}/acados - shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment - name: Install simde - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_simde.sh'' + run: ${{ github.workspace }}/.github/linux/install_simde.sh # - name: Install new CasADi Python - # working-directory: ${{runner.workspace}}/acados + # working-directory: ${{ github.workspace }} # shell: bash - # run: ${{runner.workspace}}/acados/.github/linux/install_new_casadi_python.sh'' + # run: ${{ github.workspace }}/.github/linux/install_new_casadi_python.sh - name: Prepare Octave - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | sudo apt-get update sudo apt-get install liboctave-dev -y --fix-missing octave --version - ${{runner.workspace}}/acados/.github/linux/install_new_casadi_octave.sh + ${{ github.workspace }}/.github/linux/install_new_casadi_octave.sh # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: | cmake --version cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Export Paths for octave - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Run Python tests that need new CasADi & test py2matlab - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/p_global_example + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/p_global_example python example_p_global.py echo "\nPython run done; testing OCP tranfer to Octave\n" octave code_reuse_py2matlab.m - name: Run Python to Octave sim transfer test - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/sim + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/sim python minimal_example_sim_cmake.py echo "\nPython run done; testing SIM tranfer to Octave\n" octave code_reuse_py2matlab_sim.m - name: Run more Python tests - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/tests + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/tests python test_rti_sqp_residuals.py - cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ocp python test_casadi_formulation.py - name: Python sensitivity examples - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/solution_sensitivities + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/solution_sensitivities python value_gradient_example.py python policy_gradient_example.py python test_solution_sens_and_exact_hess.py python forw_vs_adj_param_sens.py python smooth_policy_gradients.py - cd ${{runner.workspace}}/acados/examples/acados_python/solution_sensitivities_convex_example + cd ${{ github.workspace }}/examples/acados_python/solution_sensitivities_convex_example python value_gradient_example_linear.py python batch_adjoint_solution_sensitivity_example.py python non_ocp_example.py - cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ocp python initialization_test.py python ocp_example_cost_formulations.py - cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart + cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart python example_solution_sens_closed_loop.py - cd ${{runner.workspace}}/acados/examples/acados_python/chain_mass/ + cd ${{ github.workspace }}/examples/acados_python/chain_mass/ python solution_sensitivity_example.py - name: Python Furuta pendulum timeout test - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/furuta_pendulum + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/furuta_pendulum python main_closed_loop.py python convergence_experiment.py - name: Python evaluator test - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: | - source ${{runner.workspace}}/acados/acadosenv/bin/activate - cd ${{runner.workspace}}/acados/examples/acados_python/evaluation + source ${{ github.workspace }}/acadosenv/bin/activate + cd ${{ github.workspace }}/examples/acados_python/evaluation python minimal_example_evaluation.py + +# ===== MATLAB TESTS ===== MATLAB_test: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 + steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - name: Install Casadi MATLAB - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/install_new_casadi_matlab.sh + ${{ github.workspace }}/.github/linux/install_new_casadi_matlab.sh - name: Export Paths - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Install MATLAB uses: matlab-actions/setup-matlab@v2 @@ -280,16 +213,16 @@ jobs: # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: | cmake --version cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash - working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + working-directory: ${{ github.workspace }}/examples/acados_matlab_octave/test run: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + cd ${{ github.workspace }}/examples/acados_matlab_octave/test source env.sh - name: Run MATLAB tests @@ -297,36 +230,40 @@ jobs: if: always() with: command: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; run_matlab_tests + cd ${{ github.workspace }}/examples/acados_matlab_octave/test; run_matlab_tests + MATLAB_examples_new_casadi: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 + steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - name: Install Casadi MATLAB - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/install_new_casadi_matlab.sh + ${{ github.workspace }}/.github/linux/install_new_casadi_matlab.sh - name: Install simde - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_simde.sh'' + run: ${{ github.workspace }}/.github/linux/install_simde.sh'' - name: Export Paths - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Install MATLAB uses: matlab-actions/setup-matlab@v2 @@ -339,16 +276,16 @@ jobs: # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: | cmake --version cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash - working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + working-directory: ${{ github.workspace }}/examples/acados_matlab_octave/test run: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + cd ${{ github.workspace }}/examples/acados_matlab_octave/test source env.sh - name: Run MATLAB tests @@ -356,38 +293,42 @@ jobs: if: always() with: command: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; run_matlab_examples_new_casadi + cd ${{ github.workspace }}/examples/acados_matlab_octave/test; run_matlab_examples_new_casadi - name: Run Simulink MOCP test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/mocp_transition_example; main_mocp_simulink + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/mocp_transition_example; main_mocp_simulink + # run selected matlab examples MATLAB_examples: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 + steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - name: Install Casadi MATLAB - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh + ${{ github.workspace }}/.github/linux/install_casadi_matlab.sh - name: Export Paths - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Install MATLAB uses: matlab-actions/setup-matlab@v2 @@ -400,16 +341,16 @@ jobs: # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: | cmake --version cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash - working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + working-directory: ${{ github.workspace }}/examples/acados_matlab_octave/test run: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test + cd ${{ github.workspace }}/examples/acados_matlab_octave/test source env.sh - name: Run MATLAB examples @@ -417,32 +358,36 @@ jobs: if: always() with: command: | - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; test_all_examples; + cd ${{ github.workspace }}/examples/acados_matlab_octave/test; test_all_examples; + +# ===== SIMULINK TESTS ===== simulink_test: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - name: Install Casadi MATLAB - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh + ${{ github.workspace }}/.github/linux/install_casadi_matlab.sh - name: Export Paths - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Install MATLAB uses: matlab-actions/setup-matlab@v2 @@ -455,7 +400,7 @@ jobs: # just needed for blasfeo_target.h in MEX interface - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: | cmake --version cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF @@ -464,79 +409,120 @@ jobs: uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_test - name: Run Simulink initialization test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_init_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_init_test - name: Run Simulink QP test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_qp_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_qp_test - name: Run Simulink slack test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_slack_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_slack_test - name: Run Simulink parameter test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_param_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_param_test - name: Run Simulink sparse parameter test uses: matlab-actions/run-command@v2 if: always() with: - command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_sparse_param_test + command: cd ${{ github.workspace }}/examples/acados_matlab_octave/test; simulink_sparse_param_test octave_test: - needs: core_build + needs: call_core_build runs-on: ubuntu-22.04 steps: - - - uses: actions/checkout@v4 + - name: Checkout repository + uses: actions/checkout@v4 with: submodules: 'recursive' - - uses: actions/download-artifact@v4 + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment with: - path: ${{runner.workspace}}/acados + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} - name: Prepare Octave - working-directory: ${{runner.workspace}}/acados/external + working-directory: ${{ github.workspace }}/external shell: bash run: | sudo apt-get update sudo apt-get install liboctave-dev -y --fix-missing octave --version - ${{runner.workspace}}/acados/.github/linux/install_new_casadi_octave.sh + ${{ github.workspace }}/.github/linux/install_new_casadi_octave.sh - name: Install Tera - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash - run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh'' + run: ${{ github.workspace }}/.github/linux/install_tera.sh'' - name: Export Paths - working-directory: ${{runner.workspace}}/acados + working-directory: ${{ github.workspace }} shell: bash run: | - ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}} + ${{ github.workspace }}/.github/linux/export_paths.sh ${{ github.workspace }} - name: Configure CMake shell: bash - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE - name: Run CMake Octave tests (ctest) - working-directory: ${{runner.workspace}}/acados/build + working-directory: ${{ github.workspace }}/build shell: bash run: ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4; + + +# ===== ROS TESTS ===== + ros2_humble_test: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Install ROS2 Humble + working-directory: ${{ github.workspace }} + run: ${{ github.workspace }}/.github/linux/install_ros2_humble.sh + shell: bash + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: Test ROS2 OCP Package + uses: ./.github/actions/test-ros2-package + with: + python-file-path: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py + source-file-paths: | + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/pendulum_model.py + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/utils.py + + - name: Test ROS2 Simulation Package + uses: ./.github/actions/test-ros2-package + with: + python-file-path: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py + source-file-paths: | + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/pendulum_model.py \ No newline at end of file diff --git a/README.md b/README.md index 383364a6a6..7d5c0de291 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ![](docs/_static/acados_logo.png) [![Appveyor status](https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true)](https://ci.appveyor.com/project/roversch/acados) -![Github actions full build workflow](https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg) +[![Test Full Build Linux](https://github.com/ArgoJ/acados/actions/workflows/full_build.yml/badge.svg)](https://github.com/ArgoJ/acados/actions/workflows/full_build.yml) `acados` provides fast and embedded solvers for nonlinear optimal control, specifically designed for real-time applications and embedded systems. diff --git a/examples/acados_python/chain_mass/plot_utils.py b/examples/acados_python/chain_mass/plot_utils.py index 27726b0849..059c12a841 100644 --- a/examples/acados_python/chain_mass/plot_utils.py +++ b/examples/acados_python/chain_mass/plot_utils.py @@ -221,7 +221,7 @@ def plot_chain_position_3D(X, xPosFirstMass, XNames=None): XNames = [XNames] fig = plt.figure() - ax = fig.gca(projection='3d') + ax = fig.add_subplot(projection='3d') ax.plot(xPosFirstMass[0], xPosFirstMass[1], xPosFirstMass[2], 'rx') for i, x in enumerate(X): diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py new file mode 100644 index 0000000000..31e6acbc0e --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py @@ -0,0 +1,152 @@ +# +# 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.; +# + +import numpy as np +import scipy.linalg + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosOcpRosOptions + +import sys +import os +script_dir = os.path.dirname(os.path.realpath(__file__)) +common_path = os.path.join(script_dir, '..', 'common') +sys.path.insert(0, os.path.abspath(common_path)) +from pendulum_model import export_pendulum_ode_model +from utils import plot_pendulum + +def main(): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + Tf = 1.0 + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + N = 20 + + # set dimensions + ocp.solver_options.N_horizon = N + + # set cost + Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R = 2*np.diag([1e-2]) + + ocp.cost.W_e = Q + ocp.cost.W = scipy.linalg.block_diag(Q, R) + + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vx[:nx,:nx] = np.eye(nx) + + Vu = np.zeros((ny, nu)) + Vu[4,0] = 1.0 + ocp.cost.Vu = Vu + + ocp.cost.Vx_e = np.eye(nx) + + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) + + # set constraints + + # bound on u + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + # initial state + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.print_level = 0 + ocp.solver_options.nlp_solver_type = 'SQP_RTI' + + # set prediction horizon + ocp.solver_options.tf = Tf + + # Ros stuff + ocp.ros_opts = AcadosOcpRosOptions() + ocp.ros_opts.package_name = "pendulum_on_cart_ocp" + + export_code = os.path.join(script_dir, 'generated_ocp') + ocp.code_export_directory = str(os.path.join(export_code, "c_generated_code")) + ocp_solver = AcadosOcpSolver(ocp, json_file = str(os.path.join(export_code, 'acados_ocp.json'))) + + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) + + # call SQP_RTI solver in the loop: + tol = 1e-6 + + SPLIT_RTI = True + for i in range(20): + if SPLIT_RTI: + # preparation + ocp_solver.options_set("rti_phase", 1) + status = ocp_solver.solve() + # feedback + ocp_solver.options_set("rti_phase", 2) + status = ocp_solver.solve() + else: + status = ocp_solver.solve() + ocp_solver.print_statistics() + residuals = ocp_solver.get_residuals() + print("residuals after ", i, "SQP_RTI iterations:\n", residuals) + if max(residuals) < tol: + break + + if status != 0: + raise Exception(f'acados returned status {status}.') + + # get solution + for i in range(N): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N,:] = ocp_solver.get(N, "x") + + ocp_solver.print_statistics() + + # plot + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) + + +if __name__ == "__main__": + main() diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py new file mode 100644 index 0000000000..774a4ca45b --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py @@ -0,0 +1,54 @@ +import numpy as np +from acados_template import AcadosSim, AcadosSimSolver, AcadosSimRosOptions + +import sys +import os +script_dir = os.path.dirname(os.path.realpath(__file__)) +common_path = os.path.join(script_dir, '..', 'common') +sys.path.insert(0, os.path.abspath(common_path)) +from pendulum_model import export_pendulum_ode_model + + +def main(): + sim = AcadosSim() + sim.model = export_pendulum_ode_model() + + Tf = 0.1 + nx = sim.model.x.rows() + N = 200 + + # set simulation time + sim.solver_options.T = Tf + # set options + sim.solver_options.integrator_type = 'IRK' + sim.solver_options.num_stages = 3 + sim.solver_options.num_steps = 3 + sim.solver_options.newton_iter = 3 # for implicit integrator + sim.solver_options.collocation_type = "GAUSS_RADAU_IIA" + + sim.ros_opts = AcadosSimRosOptions() + sim.ros_opts.package_name = "pendulum_on_cart_sim" + + export_code = os.path.join(script_dir, 'generated_sim') + sim.code_export_directory = str( os.path.join(export_code, "c_generated_code")) + acados_integrator = AcadosSimSolver(sim, json_file=str(os.path.join(export_code, 'acados_sim.json'))) + + x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) + u0 = np.array([0.0]) + + simX = np.zeros((N+1, nx)) + simX[0,:] = x0 + + for i in range(N): + # initialize IRK + if sim.solver_options.integrator_type == 'IRK': + acados_integrator.set("xdot", np.zeros((nx,))) + + simX[i+1,:] = acados_integrator.simulate(x=simX[i, :], u=u0) + + S_forw = acados_integrator.get("S_forw") + print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw) + + +if __name__ == "__main__": + main() diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index 19f05d64d4..9290efdb3e 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -42,6 +42,9 @@ from .acados_sim import AcadosSim, AcadosSimOptions from .acados_multiphase_ocp import AcadosMultiphaseOcp +from .ros2.ocp_node import AcadosOcpRosOptions +from .ros2.sim_node import AcadosSimRosOptions + from .acados_ocp_solver import AcadosOcpSolver from .acados_casadi_ocp_solver import AcadosCasadiOcpSolver, AcadosCasadiOcp from .acados_sim_solver import AcadosSimSolver diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index afe83fa65a..f7f06c26a0 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -45,6 +45,7 @@ from .acados_dims import AcadosOcpDims from .acados_ocp_options import AcadosOcpOptions from .acados_ocp_iterate import AcadosOcpIterate +from .ros2.ocp_node import AcadosOcpRosOptions from .utils import (get_acados_path, format_class_dict, make_object_json_dumpable, render_template, get_shared_lib_ext, is_column, is_empty, casadi_length, check_if_square, ns_from_idxs_rev, @@ -114,6 +115,7 @@ def __init__(self, acados_path=''): self.__p_global_values = np.array([]) self.__problem_class = 'OCP' self.__json_file = "acados_ocp.json" + self.__ros_opts: Optional[AcadosOcpRosOptions] = None self.code_export_directory = 'c_generated_code' """Path to where code will be exported. Default: `c_generated_code`.""" @@ -160,10 +162,20 @@ def json_file(self): """Name of the json file where the problem description is stored.""" return self.__json_file + @property + def ros_opts(self) -> Optional[AcadosOcpRosOptions]: + """Options to configure ROS 2 nodes and topics.""" + return self.__ros_opts + @json_file.setter def json_file(self, json_file): self.__json_file = json_file + @ros_opts.setter + def ros_opts(self, ros_opts: AcadosOcpRosOptions): + if not isinstance(ros_opts, AcadosOcpRosOptions): + raise TypeError('Invalid ros_opts value, expected AcadosOcpRos.\n') + self.__ros_opts = ros_opts def _make_consistent_cost_initial(self): dims = self.dims @@ -1282,6 +1294,67 @@ def _get_external_function_header_templates(self, ) -> list: return template_list + def _get_ros_template_list(self) -> list: + template_list = [] + + # --- Interface Package --- + ros_interface_dir = os.path.join('ros2', 'ocp_interface_templates') + interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') + template_file = os.path.join(ros_interface_dir, 'README.in.md') + template_list.append((template_file, 'README.md', interface_dir)) + template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') + template_list.append((template_file, 'CMakeLists.txt', interface_dir)) + template_file = os.path.join(ros_interface_dir, 'package.in.xml') + template_list.append((template_file, 'package.xml', interface_dir)) + + # Messages + msg_dir = os.path.join(interface_dir, 'msg') + template_file = os.path.join(ros_interface_dir, 'State.in.msg') + template_list.append((template_file, 'State.msg', msg_dir)) + template_file = os.path.join(ros_interface_dir, 'References.in.msg') + template_list.append((template_file, 'References.msg', msg_dir)) + template_file = os.path.join(ros_interface_dir, 'Parameters.in.msg') + template_list.append((template_file, 'Parameters.msg', msg_dir)) + template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') + template_list.append((template_file, 'ControlInput.msg', msg_dir)) + + # Services + # TODO: No node implementation yet + + # Actions + # TODO: No Template yet and no node implementation + + # --- Solver Package --- + ros_pkg_dir = os.path.join('ros2', 'ocp_node_templates') + package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) + template_file = os.path.join(ros_pkg_dir, 'README.in.md') + template_list.append((template_file, 'README.md', package_dir)) + template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') + template_list.append((template_file, 'CMakeLists.txt', package_dir)) + template_file = os.path.join(ros_pkg_dir, 'package.in.xml') + template_list.append((template_file, 'package.xml', package_dir)) + + # Header + include_dir = os.path.join(package_dir, 'include', self.ros_opts.package_name) + template_file = os.path.join(ros_pkg_dir, 'config.in.hpp') + template_list.append((template_file, 'config.hpp', include_dir)) + template_file = os.path.join(ros_pkg_dir, 'utils.in.hpp') + template_list.append((template_file, 'utils.hpp', include_dir)) + template_file = os.path.join(ros_pkg_dir, 'node.in.h') + template_list.append((template_file, 'node.h', include_dir)) + + # Source + src_dir = os.path.join(package_dir, 'src') + template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') + template_list.append((template_file, 'node.cpp', src_dir)) + + # Test + test_dir = os.path.join(package_dir, 'test') + template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') + template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir)) + return template_list + + def __get_template_list(self, cmake_builder=None) -> list: """ returns a list of tuples in the form: @@ -1319,6 +1392,10 @@ def __get_template_list(self, cmake_builder=None) -> list: template_list += self._get_matlab_simulink_template_list(name) template_list += self._get_integrator_simulink_template_list(name) + # ROS + if self.ros_opts is not None: + template_list += self._get_ros_template_list() + return template_list @@ -1496,7 +1573,9 @@ def to_dict(self) -> dict: # convert acados classes to dicts for key, v in ocp_dict.items(): if isinstance(v, (AcadosModel, AcadosOcpDims, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpOptions, ZoroDescription)): - ocp_dict[key]=dict(getattr(self, key).__dict__) + ocp_dict[key] = dict(getattr(self, key).__dict__) + if isinstance(v, AcadosOcpRosOptions): + ocp_dict[key] = v.to_dict() ocp_dict = format_class_dict(ocp_dict) return ocp_dict diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index f6ba29fd11..0806a7be0e 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -31,10 +31,12 @@ import os, json import numpy as np +from typing import Optional from copy import deepcopy from .acados_model import AcadosModel from .acados_dims import AcadosSimDims from .builders import CMakeBuilder +from .ros2.sim_node import AcadosSimRosOptions from .utils import (get_acados_path, get_shared_lib_ext, format_class_dict, check_casadi_version, make_object_json_dumpable, render_template) from .casadi_function_generation import ( @@ -71,6 +73,8 @@ def __init__(self): self.__num_threads_in_batch_solve: int = 1 self.__with_batch_functionality: bool = False + + @property def integrator_type(self): """Integrator type. Default: 'ERK'.""" @@ -350,11 +354,18 @@ def __init__(self, acados_path=''): self.__parameter_values = np.array([]) self.__problem_class = 'SIM' + + self.__ros_opts: Optional[AcadosSimRosOptions] = None @property def parameter_values(self): """:math:`p` - initial values for parameter - can be updated""" return self.__parameter_values + + @property + def ros_opts(self) -> Optional[AcadosSimRosOptions]: + """Options to configure ROS 2 nodes and topics.""" + return self.__ros_opts @parameter_values.setter def parameter_values(self, parameter_values): @@ -363,9 +374,16 @@ def parameter_values(self, parameter_values): else: raise ValueError('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') + + @ros_opts.setter + def ros_opts(self, ros_opts: AcadosSimRosOptions): + if not isinstance(ros_opts, AcadosSimRosOptions): + raise TypeError('Invalid ros_opts value, expected AcadosOcpRos.\n') + self.__ros_opts = ros_opts def make_consistent(self): self.model.make_consistent(self.dims) + self.name = self.model.name if self.parameter_values.shape[0] != self.dims.np: raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ @@ -385,6 +403,8 @@ def to_dict(self) -> dict: # skip non dict attributes if isinstance(v, (AcadosSim, AcadosSimDims, AcadosSimOptions, AcadosModel)): sim_dict[key]=dict(getattr(self, key).__dict__) + if isinstance(v, AcadosSimRosOptions): + sim_dict[key] = v.to_dict() return format_class_dict(sim_dict) @@ -394,6 +414,82 @@ def dump_to_json(self, json_file='acados_sim.json') -> None: json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) + def _get_ros_template_list(self) -> list: + template_list = [] + + # --- Interface Package --- + ros_interface_dir = os.path.join('ros2', 'sim_interface_templates') + interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') + template_file = os.path.join(ros_interface_dir, 'README.in.md') + template_list.append((template_file, 'README.md', interface_dir)) + template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') + template_list.append((template_file, 'CMakeLists.txt', interface_dir)) + template_file = os.path.join(ros_interface_dir, 'package.in.xml') + template_list.append((template_file, 'package.xml', interface_dir)) + + # Messages + msg_dir = os.path.join(interface_dir, 'msg') + template_file = os.path.join(ros_interface_dir, 'State.in.msg') + template_list.append((template_file, 'State.msg', msg_dir)) + template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') + template_list.append((template_file, 'ControlInput.msg', msg_dir)) + + # Services + # TODO: No node implementation yet + + # Actions + # TODO: No Template yet and no node implementation + + # --- Simulator Package --- + ros_pkg_dir = os.path.join('ros2', 'sim_node_templates') + package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) + template_file = os.path.join(ros_pkg_dir, 'README.in.md') + template_list.append((template_file, 'README.md', package_dir)) + template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') + template_list.append((template_file, 'CMakeLists.txt', package_dir)) + template_file = os.path.join(ros_pkg_dir, 'package.in.xml') + template_list.append((template_file, 'package.xml', package_dir)) + + # Header + include_dir = os.path.join(package_dir, 'include', self.ros_opts.package_name) + template_file = os.path.join(ros_pkg_dir, 'config.in.hpp') + template_list.append((template_file, 'config.hpp', include_dir)) + template_file = os.path.join(ros_pkg_dir, 'utils.in.hpp') + template_list.append((template_file, 'utils.hpp', include_dir)) + template_file = os.path.join(ros_pkg_dir, 'node.in.h') + template_list.append((template_file, 'node.h', include_dir)) + + # Source + src_dir = os.path.join(package_dir, 'src') + template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') + template_list.append((template_file, 'node.cpp', src_dir)) + + # Test + test_dir = os.path.join(package_dir, 'test') + template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') + template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir)) + return template_list + + + def _get_simulink_template_list(self, name: str) -> list: + template_list = [] + template_file = os.path.join('matlab_templates', 'mex_sim_solver.in.m') + template_list.append((template_file, f'{name}_mex_sim_solver.m')) + template_file = os.path.join('matlab_templates', 'make_mex_sim.in.m') + template_list.append((template_file, f'make_mex_sim_{name}.m')) + template_file = os.path.join('matlab_templates', 'acados_sim_create.in.c') + template_list.append((template_file, f'acados_sim_create_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_free.in.c') + template_list.append((template_file, f'acados_sim_free_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_set.in.c') + template_list.append((template_file, f'acados_sim_set_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') + template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) + template_file = os.path.join('matlab_templates', 'make_sfun_sim.in.m') + template_list.append((template_file, f'make_sfun_sim_{name}.m')) + return template_list + + def render_templates(self, json_file, cmake_options: CMakeBuilder = None): # setting up loader and environment json_path = os.path.join(os.getcwd(), json_file) @@ -408,21 +504,18 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): ('acados_sim_solver.in.pxd', 'acados_sim_solver.pxd'), ('main_sim.in.c', f'main_sim_{name}.c'), ] + + # Model + model_dir = os.path.join(self.code_export_directory, self.model.name + '_model') + template_list.append(('model.in.h', f'{self.model.name}_model.h', model_dir)) + + # Simulink if self.simulink_opts is not None: - template_file = os.path.join('matlab_templates', 'mex_sim_solver.in.m') - template_list.append((template_file, f'{name}_mex_sim_solver.m')) - template_file = os.path.join('matlab_templates', 'make_mex_sim.in.m') - template_list.append((template_file, f'make_mex_sim_{name}.m')) - template_file = os.path.join('matlab_templates', 'acados_sim_create.in.c') - template_list.append((template_file, f'acados_sim_create_{name}.c')) - template_file = os.path.join('matlab_templates', 'acados_sim_free.in.c') - template_list.append((template_file, f'acados_sim_free_{name}.c')) - template_file = os.path.join('matlab_templates', 'acados_sim_set.in.c') - template_list.append((template_file, f'acados_sim_set_{name}.c')) - template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') - template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) - template_file = os.path.join('matlab_templates', 'make_sfun_sim.in.m') - template_list.append((template_file, f'make_sfun_sim_{name}.m')) + template_list += self._get_simulink_template_list(name) + + # ROS2 + if self.ros_opts is not None: + template_list += self._get_ros_template_list() # Builder if cmake_options is not None: @@ -431,15 +524,9 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): template_list.append(('Makefile.in', 'Makefile')) # Render templates - for (in_file, out_file) in template_list: - render_template(in_file, out_file, self.code_export_directory, json_path) - - # folder model - model_dir = os.path.join(self.code_export_directory, self.model.name + '_model') - - in_file = 'model.in.h' - out_file = f'{self.model.name}_model.h' - render_template(in_file, out_file, model_dir, json_path) + for tup in template_list: + output_dir = self.code_export_directory if len(tup) <= 2 else tup[2] + render_template(tup[0], tup[1], output_dir, json_path) def generate_external_functions(self, ): diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt new file mode 100644 index 0000000000..e22e6a7e7e --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.16) +project({{ ros_opts.package_name }}_interface) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +# --- ROS DEPENDENCIES --- +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# --- INTERFACES --- +set(MSG_FILES + "msg/State.msg" + "msg/ControlInput.msg" + {%- if dims.ny > 0 or dims.ny_0 > 0 or dims.ny_e > 0 %} + "msg/References.msg" + {%- endif %} + {%- if dims.np > 0 %} + "msg/Parameters.msg" + {%- endif %} +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_FILES} + DEPENDENCIES std_msgs +) + +ament_package() diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg new file mode 100644 index 0000000000..dc776a312b --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg @@ -0,0 +1,8 @@ +# Generic state message +std_msgs/Header header + +# control vector +float64[{{ dims.nu }}] u + +# Status, 0 = OK +uint8 status diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg new file mode 100644 index 0000000000..c16ca61035 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg @@ -0,0 +1,5 @@ +# Generic parameter message +std_msgs/Header header + +# Parameter vector +float64[{{ dims.np }}] p diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md new file mode 100644 index 0000000000..12a7c758d5 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md @@ -0,0 +1,17 @@ +# # {{ ros_opts.package_name | replace(from="_", to=" ") | title }} Interface + +This package is generated by the [`acados_template` package, the Python interface of acados](https://github.com/acados/acados). + +## Overview +This package contains the ROS interface, neede for communication with the acados OCP Solver. + + +## Installation +To install this package, you can use the following command: +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +```bash +colcon build --packages-select {{ ros_opts.package_name }}_interface && source install/setup.bash +``` diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg new file mode 100644 index 0000000000..b95b09854f --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg @@ -0,0 +1,13 @@ +# Generic reference message +std_msgs/Header header + +# Reference vectors +{%- if dims.ny_0 > 0 %} +float64[{{ dims.ny_0 }}] yref_0 +{%- endif %} +{%- if dims.ny > 0 %} +float64[{{ dims.ny }}] yref +{%- endif %} +{%- if dims.ny_e > 0 %} +float64[{{ dims.ny_e }}] yref_e +{%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg new file mode 100644 index 0000000000..843442b395 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg @@ -0,0 +1,5 @@ +# Generic state message +std_msgs/Header header + +# State vector +float64[{{ dims.nx }}] x diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml new file mode 100644 index 0000000000..33af67459b --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml @@ -0,0 +1,26 @@ + + + + {{ ros_opts.package_name }}_interface + 0.1.0 + A Generic Interface for the acados solver + Josua Lindemann + MIT + + ament_cmake + + std_msgs + + rosidl_default_generators + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt new file mode 100644 index 0000000000..9df9e2d06e --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt @@ -0,0 +1,106 @@ +{%- if solver_options.qp_solver %} + {%- set qp_solver = solver_options.qp_solver %} +{%- else %} + {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} +{%- endif -%} + +cmake_minimum_required(VERSION 3.16) +project({{ ros_opts.package_name }}) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +# --- ROS DEPENDENCIES --- +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package({{ ros_opts.package_name }}_interface REQUIRED) +{%- if solver_options.with_batch_functionality %} +find_package(OpenMP REQUIRED) +{%- endif %} + +# --- ACADOS PATHS --- +set(ACADOS_INCLUDE_PATH {{ acados_include_path }}) +set(ACADOS_LIB_DIR {{ acados_lib_path }}) +set(ACADOS_GENERATED_CODE_DIR {{ code_export_directory }}) + +# --- EXECUTABLE --- +add_executable({{ ros_opts.node_name }} + src/node.cpp +) + +# --- TARGET CONFIGURATION --- +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # Dies bindet die Optionen direkt an den Node und ist die moderne Variante + target_compile_options({{ ros_opts.node_name }} PRIVATE -Wall -Wextra -Wpedantic) +endif() + +target_include_directories({{ ros_opts.node_name }} PUBLIC + $ + $ + ${ACADOS_GENERATED_CODE_DIR} + ${ACADOS_INCLUDE_PATH} + ${ACADOS_INCLUDE_PATH}/acados + ${ACADOS_INCLUDE_PATH}/blasfeo/include + ${ACADOS_INCLUDE_PATH}/hpipm/include + {%- if "QPOASES" in qp_solver %} + ${ACADOS_INCLUDE_PATH}/qpOASES_e + {%- elif "DAQP" in qp_solver %} + ${ACADOS_INCLUDE_PATH}/daqp/include + {%- elif "OSQP" in qp_solver %} + ${ACADOS_INCLUDE_PATH}/osqp + {%- endif %} +) + +target_link_libraries({{ ros_opts.node_name }} + ${ACADOS_GENERATED_CODE_DIR}/libacados_ocp_solver_{{ name }}{{ shared_lib_ext }} + ${ACADOS_LIB_DIR}/libacados.so + ${ACADOS_LIB_DIR}/libblasfeo.so + ${ACADOS_LIB_DIR}/libhpipm.so + m + {%- if "QPOASES" in qp_solver %} + ${ACADOS_LIB_DIR}/libqpOASES_e.so + {%- elif "DAQP" in qp_solver %} + ${ACADOS_LIB_DIR}/libdaqp.so + {%- elif "OSQP" in qp_solver %} + ${ACADOS_LIB_DIR}/libosqp.so + {%- endif %} + {%- if solver_options.with_batch_functionality %} + OpenMP::OpenMP_CXX + {%- endif %} +) + +# --- DEPENDENCIES --- +ament_target_dependencies({{ ros_opts.node_name }} + rclcpp + std_msgs + {{ ros_opts.package_name }}_interface +) + +# --- INSTALLATIONS --- +install(TARGETS + {{ ros_opts.node_name }} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + include/ + DESTINATION include +) + +install(FILES + ${ACADOS_GENERATED_CODE_DIR}/libacados_ocp_solver_{{ name }}{{ shared_lib_ext }} + DESTINATION lib +) + +# --- TESTS --- +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test( + test/test_{{ ros_opts.package_name }}.launch.py + TARGET test_{{ ros_opts.package_name }} + TIMEOUT 180 + ) +endif() + +ament_package() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md new file mode 100644 index 0000000000..7760474c0d --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md @@ -0,0 +1,25 @@ +# {{ ros_opts.package_name | replace(from="_", to=" ") | title }} + +This package is generated by the [acados_template](https://github.com/acados/acados) package. + +## Overview +This package contains the ROS node generation for acados, which is a high-performance solver for optimal control problems. The generated nodes can be used to interface with acados solvers in a ROS environment. + + +## Installation +To install this package, you can use the following command: +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +```bash +colcon build --packages-select {{ ros_opts.package_name }} {{ ros_opts.package_name }}_interface && source install/setup.bash +``` + +## Usage +After building the package, you can run the generated nodes using: +```bash +ros2 run {{ ros_opts.package_name }} {{ ros_opts.node_name }} +``` + + diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp new file mode 100644 index 0000000000..44695127a9 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp @@ -0,0 +1,19 @@ +#ifndef {{ ros_opts.package_name | upper }}_CONFIG_H +#define {{ ros_opts.package_name | upper }}_CONFIG_H + +#include +#include +#include + + +namespace {{ ros_opts.package_name }} +{ +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} + +struct {{ ClassName }}Config { + double ts{ {{ solver_options.Tsim }} }; +}; + +} // namespace {{ ros_opts.package_name }} + +#endif // {{ ros_opts.package_name | upper }}_CONFIG_H diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp new file mode 100644 index 0000000000..93b4d36f54 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp @@ -0,0 +1,681 @@ +#include "{{ ros_opts.package_name }}/node.h" + +namespace {{ ros_opts.package_name }} +{ + +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +{%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} +{%- if ns %} +{%- set control_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} +{%- set references_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.reference_topic %} +{%- set parameters_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.parameters_topic %} +{%- else %} +{%- set control_topic = "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.state_topic %} +{%- set references_topic = "/" ~ ros_opts.reference_topic %} +{%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} +{%- endif %} +{%- set has_slack = dims.ns > 0 or dims.ns_0 > 0 or dims.ns_e > 0 %} +{{ ClassName }}::{{ ClassName }}() + : Node("{{ ros_opts.node_name }}") +{ + RCLCPP_INFO(this->get_logger(), "Initializing {{ ros_opts.node_name | replace(from="_", to=" ") | title }}..."); + + // --- default values --- + config_ = {{ ClassName }}Config(); + {%- if dims.ny_0 > 0 %} + current_yref_0_ = { {{- cost.yref_0 | join(sep=', ') -}} }; + {%- endif %} + {%- if dims.ny > 0 %} + current_yref_ = { {{- cost.yref | join(sep=', ') -}} }; + {%- endif %} + {%- if dims.ny_e > 0 %} + current_yref_e_ = { {{- cost.yref_e | join(sep=', ') -}} }; + {%- endif %} + {%- if dims.np > 0 %} + current_p_ = { {{- parameter_values | join(sep=', ') -}} }; + {%- endif %} + + // --- Parameters --- + this->declare_parameters(); + this->setup_parameter_handlers(); + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&{{ ClassName }}::on_parameter_update, this, std::placeholders::_1)); + this->load_parameters(); + + // --- Subscriber --- + state_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::State>( + "{{ state_topic }}", 10, + std::bind(&{{ ClassName }}::state_callback, this, std::placeholders::_1)); + references_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::References>( + "{{ references_topic }}", 10, + std::bind(&{{ ClassName }}::references_callback, this, std::placeholders::_1)); + {%- if dims.np > 0 %} + parameters_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::Parameters>( + "{{ parameters_topic }}", 10, + std::bind(&{{ ClassName }}::parameters_callback, this, std::placeholders::_1)); + {%- endif %} + + // --- Publisher --- + control_input_pub_ = this->create_publisher<{{ ros_opts.package_name }}_interface::msg::ControlInput>( + "{{ control_topic }}", 10); + + // --- Init solver --- + this->initialize_solver(); + this->apply_all_parameters_to_solver(); + this->start_control_timer(config_.ts); +} + +{{ ClassName }}::~{{ ClassName }}() { + RCLCPP_INFO(this->get_logger(), "Shutting down and freeing Acados solver memory."); + if (ocp_capsule_) { + int status = {{ model.name }}_acados_free(ocp_capsule_); + if (status) { + RCLCPP_ERROR(this->get_logger(), "{{ model.name }}_acados_free() returned status %d.", status); + } + status = {{ model.name }}_acados_free_capsule(ocp_capsule_); + if (status) { + RCLCPP_ERROR(this->get_logger(), "{{ model.name }}_acados_free_capsule() returned status %d.", status); + } + } +} + + +// --- Core Methods --- +void {{ ClassName }}::initialize_solver() { + ocp_capsule_ = {{ model.name }}_acados_create_capsule(); + int status = {{ model.name }}_acados_create(ocp_capsule_); + if (status) { + RCLCPP_FATAL(this->get_logger(), "{{ model.name }}acados_create() failed with status %d.", status); + rclcpp::shutdown(); + } + + ocp_nlp_config_ = {{ model.name }}_acados_get_nlp_config(ocp_capsule_); + ocp_nlp_dims_ = {{ model.name }}_acados_get_nlp_dims(ocp_capsule_); + ocp_nlp_in_ = {{ model.name }}_acados_get_nlp_in(ocp_capsule_); + ocp_nlp_out_ = {{ model.name }}_acados_get_nlp_out(ocp_capsule_); + ocp_nlp_opts_ = {{ model.name }}_acados_get_nlp_opts(ocp_capsule_); + + RCLCPP_INFO(this->get_logger(), "acados solver initialized successfully."); +} + +void {{ ClassName }}::control_loop() { + // TODO: check for received msgs first + std::array x0{}; + {%- if dims.ny_0 > 0 %} + std::array yref0{}; + {%- endif %} + {%- if dims.ny > 0 %} + std::array yref{}; + {%- endif %} + {%- if dims.ny_e > 0 %} + std::array yrefN{}; + {%- endif %} + {%- if dims.np > 0 %} + std::array p{}; + {%- endif %} + + { + std::scoped_lock lock(data_mutex_); + x0 = current_x_; + {%- if dims.ny_0 > 0 %} + yref0 = current_yref_0_; + {%- endif %} + {%- if dims.ny > 0 %} + yref = current_yref_; + {%- endif %} + {%- if dims.ny_e > 0 %} + yrefN = current_yref_e_; + {%- endif %} + {%- if dims.np > 0 %} + p = current_p_; + {%- endif %} + } + + // Update solver + this->set_x0(x0.data()); + + {%- if dims.ny_0 > 0 %} + this->set_yref0(yref0.data()); + {%- endif %} + {%- if dims.ny > 0 %} + this->set_yrefs(yref.data()); + {%- endif %} + {%- if dims.ny_e > 0 %} + this->set_yref_e(yrefN.data()); + {%- endif %} + {%- if dims.np > 0 %} + this->set_ocp_parameters(p.data(), p.size()); + {%- endif %} + + // Solve OCP + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + int status; + if (first_solve_) { + this->warmstart_solver_states(x0.data()); + status = this->ocp_solve(); + } + else { + status = this->feedback_rti_solve(); + } + + {%- else %} + int status = this->ocp_solve(); + {%- endif %} + this->solver_status_behaviour(status); +} + +void {{ ClassName }}::solver_status_behaviour(int status) { + // publish u0 also if the solver failed + this->get_input(u0_.data(), 0); + this->publish_input(u0_, status); + + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + // prepare for next iteration + if (status == ACADOS_SUCCESS) { + first_solve_ = false; + this->prepare_rti_solve(); + } + else { + first_solve_ = true; + } + {%- endif %} + + // reset solver if nan is detected + if (status == ACADOS_NAN_DETECTED) { + {{ model.name }}_acados_reset(ocp_capsule_, 1); + } +} + + +// --- ROS Callbacks --- +void {{ ClassName }}::state_callback(const {{ ros_opts.package_name }}_interface::msg::State::SharedPtr msg) { + std::scoped_lock lock(data_mutex_); + std::copy_n(msg->x.begin(), {{ model.name | upper }}_NX, current_x_.begin()); +} + +void {{ ClassName }}::references_callback(const {{ ros_opts.package_name }}_interface::msg::References::SharedPtr msg) { + std::scoped_lock lock(data_mutex_); + {%- if dims.ny_0 > 0 %} + std::copy_n(msg->yref_0.begin(), {{ model.name | upper }}_NY0, current_yref_0_.begin()); + {%- endif %} + {%- if dims.ny > 0 %} + std::copy_n(msg->yref.begin(), {{ model.name | upper }}_NY, current_yref_.begin()); + {%- endif %} + {%- if dims.ny_e > 0 %} + std::copy_n(msg->yref_e.begin(), {{ model.name | upper }}_NYN, current_yref_e_.begin()); + {%- endif %} +} +{%- if dims.np > 0 %} + +void {{ ClassName }}::parameters_callback(const {{ ros_opts.package_name }}_interface::msg::Parameters::SharedPtr msg) { + std::scoped_lock lock(data_mutex_); + std::copy_n(msg->p.begin(), {{ model.name | upper }}_NP, current_p_.begin()); +} +{%- endif %} + + +// --- ROS Publisher --- +void {{ ClassName }}::publish_input(const std::array& u0, int status) { + auto control_input = std::make_unique<{{ ros_opts.package_name }}_interface::msg::ControlInput>(); + control_input->header.stamp = this->get_clock()->now(); + control_input->header.frame_id = "{{ ros_opts.node_name }}_control_input"; + control_input->status = status; + std::copy_n(u0.begin(), {{ model.name | upper }}_NU, control_input->u.begin()); + control_input_pub_->publish(std::move(control_input)); +} + + +// --- Parameter Handling Methods --- +void {{ ClassName }}::setup_parameter_handlers() { + {%- if dims.nh_0 > 0 or dims.nphi_0 > 0 or dims.nsh_0 > 0 or dims.nsphi_0 > 0 %} + // Initial Constraints + {%- for field, param in constraints %} + {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and (field is ending_with('_0')) and ('bx_0' not in field) %} + {%- set suffix = "" %} + {%- if "h" in field and "s" not in field %} + {%- set suffix = "_NH0" %} + {%- elif "phi" in field and "s" not in field %} + {%- set suffix = "_NPHI0" %} + {%- elif "sh" in field %} + {%- set suffix = "_NSH0" %} + {%- elif "sphi" in field %} + {%- set suffix = "_NSPHI0" %} + {%- endif %} + {%- set constraint_size = model.name ~ suffix | upper %} + parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", std::vector{0}); + }; + {%- endif %} + {%- endfor %} + {%- endif %} + {%- if dims.nbu > 0 or dims.nbx > 0 or dims.ng > 0 or dims.nh > 0 or dims.nphi > 0 or dims.nsbx > 0 or dims.nsg > 0 or dims.nsh > 0 or dims.nsphi > 0 %} + + // Stage Constraints + {%- for field, param in constraints %} + {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and (field is not ending_with('_0')) and (field is not ending_with('_e')) %} + {%- set suffix = "" %} + {%- if "bx" in field and "s" not in field %} + {%- set suffix = "_NBX" %} + {%- elif "bu" in field and "s" not in field %} + {%- set suffix = "_NBU" %} + {%- elif "h" in field and "s" not in field %} + {%- set suffix = "_NH" %} + {%- elif "phi" in field and "s" not in field %} + {%- set suffix = "_NPHI" %} + {%- elif "g" in field and "s" not in field %} + {%- set suffix = "_NG" %} + {%- elif "sbx" in field %} + {%- set suffix = "_NSBX" %} + {%- elif "sbu" in field %} + {%- set suffix = "_NSBU" %} + {%- elif "sh" in field %} + {%- set suffix = "_NSH" %} + {%- elif "sphi" in field %} + {%- set suffix = "_NSPHI" %} + {%- elif "sg" in field %} + {%- set suffix = "_NSG" %} + {%- endif %} + {%- set constraint_size = model.name ~ suffix | upper %} + parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + auto stages = range(1, {{ model.name | upper }}_N); + this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", stages); + }; + {%- endif %} + {%- endfor %} + {%- endif %} + {%- if dims.nbx_e > 0 or dims.ng_e > 0 or dims.nh_e > 0 or dims.nphi_e > 0 or dims.nsbx_e > 0 or dims.nsg_e > 0 or dims.nsh_e > 0 or dims.nsphi_e > 0 %} + + // Terminal Constraints + {%- for field, param in constraints %} + {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and (field is ending_with('_e')) %} + {%- set suffix = "" %} + {%- if "bx" in field and "s" not in field %} + {%- set suffix = "_NBXN" %} + {%- elif "bh" in field and "s" not in field %} + {%- set suffix = "_NHN" %} + {%- elif "phi" in field and "s" not in field %} + {%- set suffix = "_NPHIN" %} + {%- elif "g" in field and "s" not in field %} + {%- set suffix = "_NGN" %} + {%- elif "sbx" in field %} + {%- set suffix = "_NSBXN" %} + {%- elif "sh" in field %} + {%- set suffix = "_NSHN" %} + {%- elif "sphi" in field %} + {%- set suffix = "_NSPHIN" %} + {%- elif "sg" in field %} + {%- set suffix = "_NSGN" %} + {%- endif %} + {%- set constraint_size = model.name ~ suffix | upper %} + parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", std::vector{ {{- model.name | upper -}}_N}); + }; + {%- endif %} + {%- endfor %} + {%- endif %} + + // Weights + {%- if dims.ny_0 > 0 %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.W_0"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_cost<{{ model.name | upper }}_NY0>(p, res, "W", std::vector{0}); + }; + {%- endif %} + {%- if dims.ny > 0 %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.W"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + auto stages = range(1, {{ model.name | upper }}_N); + this->update_cost<{{ model.name | upper }}_NY>(p, res, "W", stages); + }; + {%- endif %} + {%- if dims.ny_e > 0 %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.W_e"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_cost<{{ model.name | upper }}_NYN>(p, res, "W", std::vector{ {{- model.name | upper -}}_N}); + }; + {%- endif %} + {%- if has_slack %} + + {%- if dims.ns_0 > 0 %} + // Initial Slacks + {%- for field, param in cost %} + {%- set field_l = field | lower %} + {%- if param and (field_l is starting_with('z')) and (field is ending_with('_0')) %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_cost<{{ model.name | upper }}_NS0>(p, res, "{{ field }}", std::vector{0}); + }; + {%- endif %} + {%- endfor %} + + {%- endif %} + {%- if dims.ns > 0 %} + // Stage Slacks + {%- for field, param in cost %} + {%- set field_l = field | lower %} + {%- if param and (field_l is starting_with('z')) and (field is not ending_with('_0')) and (field is not ending_with('_e')) %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + auto stages = range(1, {{ model.name | upper }}_N); + this->update_cost<{{ model.name | upper }}_NS>(p, res, "{{ field }}", stages); + }; + {%- endif %} + {%- endfor %} + {%- endif %} + {%- if dims.ns_e > 0 %} + + // Terminal Slacks + {%- for field, param in cost %} + {%- set field_l = field | lower %} + {%- if param and (field_l is starting_with('z')) and (field is ending_with('_e')) %} + parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->update_cost<{{ model.name | upper }}_NSN>(p, res, "{{ field }}", std::vector{ {{- model.name | upper -}}_N}); + }; + {%- endif %} + {%- endfor %} + {%- endif %} + {%- endif %} + + // Solver Options + parameter_handlers_["{{ ros_opts.package_name }}.ts"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->config_.ts = p.as_double(); + try { + this->start_control_timer(this->config_.ts); + } catch (const std::exception& e) { + res.reason = "Failed to start control timer, while setting parameter '" + p.get_name() + "': " + e.what(); + res.successful = false; + } + }; +} + +void {{ ClassName }}::declare_parameters() { + // Constraints + {%- for field, param in constraints %} + {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and ('bx_0' not in field) %} + this->declare_parameter("{{ ros_opts.package_name }}.constraints.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); + {%- endif %} + {%- endfor %} + + // Weights + {%- for field, param in cost %} + {%- if param and (field is starting_with('W')) %} + this->declare_parameter("{{ ros_opts.package_name }}.cost.{{ field }}", std::vector{ + {%- set n_diag = param | length -%} + {%- for i in range(end=n_diag) -%} + {{- param[i][i] -}} + {%- if not loop.last %}, {% endif -%} + {%- endfor -%} + }); + {%- endif %} + {%- endfor %} + {%- if has_slack %} + + // Slacks + {%- for field, param in cost %} + {%- set field_l = field | lower %} + {%- if param and (field_l is starting_with('z')) %} + this->declare_parameter("{{ ros_opts.package_name }}.cost.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); + {%- endif %} + {%- endfor %} + {%- endif %} + + // Solver Options + this->declare_parameter("{{ ros_opts.package_name }}.ts", {{ solver_options.Tsim }}); +} + +void {{ ClassName }}::load_parameters() { + this->get_parameter("{{ ros_opts.package_name }}.ts", config_.ts); +} + +void {{ ClassName }}::apply_all_parameters_to_solver() { + if (!ocp_capsule_) { + RCLCPP_WARN(this->get_logger(), "apply_all_parameters_to_solver() called before solver init."); + return; + } + rcl_interfaces::msg::SetParametersResult res; + res.successful = true; + + for (auto & kv : parameter_handlers_) { + const auto & name = kv.first; + if (!this->has_parameter(name)) continue; + auto param = this->get_parameter(name); + kv.second(param, res); + if (!res.successful) { + RCLCPP_ERROR(this->get_logger(), + "Failed to apply initial parameter '%s': %s", + name.c_str(), res.reason.c_str()); + // reset flag for next parameter + res.successful = true; + res.reason.clear(); + } + } +} + +rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( + const std::vector& params +) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto& param : params) { + auto& param_name = param.get_name(); + + if (parameter_handlers_.count(param_name)) { + parameter_handlers_.at(param_name)(param, result); + if (!result.successful) break; + } else { + result.reason = "Update for unknown parameter '%s' received.", param_name.c_str(); + result.successful = false; + } + } + return result; +} + +template +void {{ ClassName }}::update_param_array( + const rclcpp::Parameter& param, + std::array& destination_array, + rcl_interfaces::msg::SetParametersResult& result +) { + auto values = param.as_double_array(); + + if (values.size() != N) { + result.successful = false; + result.reason = "Parameter '" + param.get_name() + "' has size " + + std::to_string(values.size()) + ", but expected is " + std::to_string(N) + "."; + return; + } + + std::copy_n(values.begin(), N, destination_array.begin()); +} + +template +void {{ ClassName }}::update_constraint( + const rclcpp::Parameter& param, + rcl_interfaces::msg::SetParametersResult& result, + const char* field, + const std::vector& stages +) { + auto values = param.as_double_array(); + + if (values.size() != N) { + result.successful = false; + result.reason = "Constraint '" + std::string(param.get_name()) + "' has size " + + std::to_string(values.size()) + ", but expected is " + std::to_string(N) + "."; + return; + } + + std::array vec{}; + std::copy_n(values.begin(), N, vec.begin()); + + for (int stage : stages) { + int status = ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, stage, field, vec.data()); + + if (status != ACADOS_SUCCESS) { + result.successful = false; + result.reason = "Acados solver failed to set cost field '" + std::string(field) + + "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; + return; + } + } +} + +template +void {{ ClassName }}::update_cost( + const rclcpp::Parameter& param, + rcl_interfaces::msg::SetParametersResult& result, + const char* field, + const std::vector& stages +) { + const auto values = param.as_double_array(); + + if (values.size() != N) { + result.successful = false; + result.reason = "Cost '" + std::string(param.get_name()) + "' has size " + + std::to_string(values.size()) + ", but expected is " + std::to_string(N) + "."; + return; + } + + std::array vec; + std::array mat{}; + std::copy_n(values.begin(), N, vec.begin()); + double* data_ptr = nullptr; + + const bool is_weight = std::strcmp(field, "W") == 0; + if (is_weight) { + mat = diag_from_vec(vec); + data_ptr = mat.data(); + RCLCPP_INFO_STREAM(this->get_logger(), "update cost field '" << field << "' mat(flat) = " << mat); + } else { + data_ptr = vec.data(); + RCLCPP_INFO_STREAM(this->get_logger(), "update cost field '" << field << "' values = " << vec); + } + + for (int stage : stages) { + int status = ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, field, data_ptr); + if (status != ACADOS_SUCCESS) { + result.successful = false; + result.reason = "Acados solver failed to set cost field '" + std::string(field) + + "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; + return; + } + } +} + +// --- Helpers --- +void {{ ClassName }}::start_control_timer(double period_seconds) { + if (period_seconds <= 0.0) period_seconds = 0.02; + auto period = std::chrono::duration_cast( + std::chrono::duration(period_seconds)); + control_timer_ = this->create_wall_timer( + period, + std::bind(&{{ ClassName }}::control_loop, this)); +} + + +// --- Acados Helpers --- +{%- if solver_options.nlp_solver_type == 'SQP_RTI' %} +void {{ ClassName }}::warmstart_solver_states(double *x0) { + for (int i = 1; i <= {{ model.name | upper }}_N; ++i) { + ocp_nlp_out_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, ocp_nlp_in_, i, "x", x0); + } +} + +int {{ ClassName }}::prepare_rti_solve() { + int phase = PREPARATION; + ocp_nlp_sqp_rti_opts_set(ocp_nlp_config_, ocp_nlp_opts_, "rti_phase", &phase); + int status = {{ model.name }}_acados_solve(ocp_capsule_); + if (status != ACADOS_SUCCESS && status != ACADOS_READY) { + first_solve_ = true; + RCLCPP_ERROR(this->get_logger(), "Solver failed at preparation phase: %d", status); + } + return status; +} + +int {{ ClassName }}::feedback_rti_solve() { + int phase = FEEDBACK; + ocp_nlp_sqp_rti_opts_set(ocp_nlp_config_, ocp_nlp_opts_, "rti_phase", &phase); + int status = {{ model.name }}_acados_solve(ocp_capsule_); + if (status != ACADOS_SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Solver failed at feedback phase: %d", status); + } + return status; +} + +{%- endif %} +int {{ ClassName }}::ocp_solve() { + int status = {{ model.name }}_acados_solve(ocp_capsule_); + if (status != ACADOS_SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Solver failed with status: %d", status); + } + return status; +} + +void {{ ClassName }}::get_input(double* u, int stage) { + ocp_nlp_out_get(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, stage, "u", u); +} + +void {{ ClassName }}::get_state(double* x, int stage) { + ocp_nlp_out_get(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, stage, "x", x); +} + +void {{ ClassName }}::set_x0(double* x0) { + ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "lbx", x0); + ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "ubx", x0); +} + +{%- if dims.ny_0 > 0 %} +void {{ ClassName }}::set_yref0(double* yref0) { + ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, 0, "yref", yref0); +} + +{%- endif %} +{%- if dims.ny > 0 %} + +void {{ ClassName }}::set_yref(double* yref, int stage) { + ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, "yref", yref); +} + +void {{ ClassName }}::set_yrefs(double* yref) { + for (int i = 1; i < {{ model.name | upper }}_N; i++) { + this->set_yref(yref, i); + } +} +{%- endif %} +{%- if dims.ny_e > 0 %} + +void {{ ClassName }}::set_yref_e(double* yrefN) { + ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, {{ model.name | upper }}_N, "yref", yrefN); +} +{%- endif %} +{%- if dims.np > 0 %} + +void {{ ClassName }}::set_ocp_parameter(double* p, size_t np, int stage) { + {{ model.name }}_acados_update_params(ocp_capsule_, stage, p, np); +} + +void {{ ClassName }}::set_ocp_parameters(double* p, size_t np) { + for (int i = 0; i <= {{ model.name | upper }}_N; i++) { + this->set_ocp_parameter(p, np, i); + } +} +{%- endif %} + +} // namespace {{ ros_opts.package_name }} + + +// --- Main --- +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared<{{ ros_opts.package_name }}::{{ ClassName }}>(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h new file mode 100644 index 0000000000..2de93eb961 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h @@ -0,0 +1,165 @@ +#ifndef {{ ros_opts.node_name | upper }}_H +#define {{ ros_opts.node_name | upper }}_H + +#include +#include +#include +#include +#include + +// ROS2 message includes +#include "{{ ros_opts.package_name }}_interface/msg/state.hpp" +#include "{{ ros_opts.package_name }}_interface/msg/control_input.hpp" +#include "{{ ros_opts.package_name }}_interface/msg/references.hpp" +{%- if dims.np > 0 %} +#include "{{ ros_opts.package_name }}_interface/msg/parameters.hpp" +{%- endif %} +#include "std_msgs/msg/header.hpp" + +// Acados includes +#include "acados/ocp_nlp/ocp_nlp_sqp_rti.h" +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_{{ model.name }}.h" + +// Package includes +#include "{{ ros_opts.package_name }}/utils.hpp" +#include "{{ ros_opts.package_name }}/config.hpp" + + +namespace {{ ros_opts.package_name }} +{ + +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +class {{ ClassName }} : public rclcpp::Node { +private: + // --- ROS Subscriptions --- + rclcpp::Subscription<{{ ros_opts.package_name }}_interface::msg::State>::SharedPtr state_sub_; + rclcpp::Subscription<{{ ros_opts.package_name }}_interface::msg::References>::SharedPtr references_sub_; + {%- if dims.np > 0 %} + rclcpp::Subscription<{{ ros_opts.package_name }}_interface::msg::Parameters>::SharedPtr parameters_sub_; + {%- endif %} + + // --- ROS Publishers --- + rclcpp::Publisher<{{ ros_opts.package_name }}_interface::msg::ControlInput>::SharedPtr control_input_pub_; + + // --- ROS Params and Timer + rclcpp::TimerBase::SharedPtr control_timer_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + using ParamHandler = std::function; + std::unordered_map parameter_handlers_; + + // --- Acados Solver --- + {{ model.name }}_solver_capsule *ocp_capsule_; + ocp_nlp_config* ocp_nlp_config_; + ocp_nlp_dims* ocp_nlp_dims_; + ocp_nlp_in* ocp_nlp_in_; + ocp_nlp_out* ocp_nlp_out_; + void* ocp_nlp_opts_; + + // --- Data and States --- + std::mutex data_mutex_; + {{ ClassName }}Config config_; + + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + bool first_solve_{true}; + {%- endif %} + std::array u0_; + std::array current_x_; + {%- if dims.ny_0 > 0 %} + std::array current_yref_0_; + {%- endif %} + {%- if dims.ny > 0 %} + std::array current_yref_; + {%- endif %} + {%- if dims.ny_e > 0 %} + std::array current_yref_e_; + {%- endif %} + {%- if dims.np > 0 %} + std::array current_p_; + {%- endif %} + +public: + {{ ClassName }}(); + ~{{ ClassName }}(); + +private: + // --- Core Methods --- + void initialize_solver(); + void control_loop(); + void solver_status_behaviour(int status); + + // --- ROS Callbacks --- + void state_callback(const {{ ros_opts.package_name }}_interface::msg::State::SharedPtr msg); + void references_callback(const {{ ros_opts.package_name }}_interface::msg::References::SharedPtr msg); + {%- if dims.np > 0 %} + void parameters_callback(const {{ ros_opts.package_name }}_interface::msg::Parameters::SharedPtr msg); + {%- endif %} + + // --- ROS Publisher --- + void publish_input(const std::array& u0, int status); + + // --- Parameter Handling Methods --- + void setup_parameter_handlers(); + void declare_parameters(); + void load_parameters(); + void apply_all_parameters_to_solver(); + rcl_interfaces::msg::SetParametersResult on_parameter_update(const std::vector& params); + + template + void get_and_check_array_param( + const std::string& param_name, + std::array& destination); + template + void update_param_array( + const rclcpp::Parameter& param, + std::array& destination_array, + rcl_interfaces::msg::SetParametersResult& result); + template + void update_constraint( + const rclcpp::Parameter& param, + rcl_interfaces::msg::SetParametersResult& result, + const char* field, + const std::vector& stages); + template + void update_cost( + const rclcpp::Parameter& param, + rcl_interfaces::msg::SetParametersResult& result, + const char* field, + const std::vector& stages); + + // --- Helpers --- + void start_control_timer(double period_seconds = 0.02); + + // --- Acados Helpers --- + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + void warmstart_solver_states(double *x0); + int prepare_rti_solve(); + int feedback_rti_solve(); + {%- endif %} + int ocp_solve(); + + void get_input(double* u, int stage); + void get_state(double* x, int stage); + + void set_x0(double* x0); + {%- if dims.ny_0 > 0 %} + void set_yref0(double* yref0); + {%- endif %} + {%- if dims.ny > 0 %} + void set_yref(double* yref, int stage); + void set_yrefs(double* yref); + {%- endif %} + {%- if dims.ny_e > 0 %} + void set_yref_e(double* yref_e); + {%- endif %} + {%- if dims.np > 0 %} + void set_ocp_parameter(double* p, size_t np, int stage); + void set_ocp_parameters(double* p, size_t np); + {%- endif %} +}; + +} // namespace {{ ros_opts.package_name }} + +#endif // {{ ros_opts.node_name | upper }}_H diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml new file mode 100644 index 0000000000..7340c8970d --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml @@ -0,0 +1,23 @@ + + + + {{ ros_opts.package_name }} + 0.1.0 + A Generic ROS2 node for the acados solver + Josua Lindemann + MIT + + ament_cmake + + rclcpp + std_msgs + {{ ros_opts.package_name }}_interface + + launch_testing + launch_testing_ament_cmake + python3-pytest + + + ament_cmake + + diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py new file mode 100644 index 0000000000..2e8d7cd33e --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py @@ -0,0 +1,205 @@ +import re +from typing import Union +from unittest import result +import rclpy +import unittest +import launch +import time +import launch_testing +import pytest +import subprocess +from launch_ros.actions import Node + +from {{ ros_opts.package_name }}_interface.msg import State, ControlInput, References +{%- if dims.np > 0 -%} +, Parameters +{%- endif %} +{%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} +{%- if ns %} +{%- set control_input_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} +{%- set references_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.reference_topic %} +{%- set parameters_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.parameters_topic %} +{%- else %} +{%- set control_input_topic = "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.state_topic %} +{%- set references_topic = "/" ~ ros_opts.reference_topic %} +{%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} +{%- endif %} + +@pytest.mark.launch_test +def generate_test_description(): + """Generate launch description for node testing.""" + start_{{ ros_opts.node_name }} = Node( + package='{{ ros_opts.package_name }}', + executable='{{ ros_opts.node_name }}', + name='{{ ros_opts.node_name }}' + ) + + return launch.LaunchDescription([ + start_{{ ros_opts.node_name }}, + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()]), + ]) + + +class GeneratedNodeTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('generated_node_test') + + def tearDown(self): + self.node.destroy_node() + + def test_set_constraints(self, proc_info): + """ + Test if constraints compile-time declared default parameters. + """ + {%- for field, param in constraints %} + {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and ('bx_0' not in field) %} + param_name = "{{ ros_opts.package_name }}.constraints.{{ field }}" + expected_value = [{{- param | join(sep=', ') -}}] + self.__check_parameter_set(param_name, expected_value) + {%- endif %} + {%- endfor %} + + def test_set_cost(self, proc_info): + """ + Test if cost compile-time declared default parameters. + """ + # --- Weights --- + {%- for field, param in cost %} + {%- if param and (field is starting_with('W')) %} + param_name = "{{ ros_opts.package_name }}.cost.{{ field }}" + expected_value = [ + {%- set n_diag = param | length -%} + {%- for i in range(end=n_diag) -%} + {{- param[i][i] -}} + {%- if not loop.last %}, {% endif -%} + {%- endfor -%} + ] + self.__check_parameter_set(param_name, expected_value) + {%- endif %} + {%- endfor %} + {%- if has_slack %} + + # --- Slacks --- + {%- for field, param in cost %} + {%- set field_l = field | lower %} + {%- if param and (field_l is starting_with('z')) %} + param_name = "{{ ros_opts.package_name }}.cost.{{ field }}" + expected_value = [{{- param | join(sep=', ') -}}] + self.__check_parameter_set(param_name, expected_value) + {%- endif %} + {%- endfor %} + {%- endif %} + + def test_set_solver_options(self, proc_info): + """ + Test if solver options compile-time declared default parameters. + """ + # --- Solver Options --- + param_name = "{{ ros_opts.package_name }}.ts" + expected_value = {{ solver_options.Tsim }} + self.__check_parameter_set(param_name, expected_value) + + def test_subscribing(self, proc_info): + """Test if the node subscribes to all expected topics.""" + try: + self.wait_for_subscription('{{ state_topic }}') + except TimeoutError: + self.fail("Node has NOT subscribed to '{{ state_topic }}'.") + + try: + self.wait_for_subscription('{{ references_topic }}') + except TimeoutError: + self.fail("Node has NOT subscribed to '{{ references_topic }}'.") + + {%- if dims.np > 0 %} + try: + self.wait_for_subscription('{{ parameters_topic }}') + except TimeoutError: + self.fail("Node has NOT subscribed to '{{ parameters_topic }}'.") + {%- endif %} + + def test_publishing(self, proc_info): + """Test if the node publishes to all expected topics.""" + try: + self.wait_for_publisher('{{ control_input_topic }}') + except TimeoutError: + self.fail("Node has NOT published to '{{ control_input_topic }}'.") + + def wait_for_subscription(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + subs = self.node.get_subscriptions_info_by_topic(topic) + if subs: + return True + time.sleep(0.05) + raise TimeoutError(f"No subscriber found on {topic} within {timeout}s") + + def wait_for_publisher(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + pubs = self.node.get_publishers_info_by_topic(topic) + if pubs: + return True + time.sleep(0.05) + raise TimeoutError(f"No publisher found on {topic} within {timeout}s") + + def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + output = get_parameter(param_name) + numbers = [float(x) for x in re.findall(r"[-+]?\d*\.\d+|\d+", output)] + if isinstance(expected_value, list): + self.assertListEqual(numbers, expected_value, f"Parameter {param_name} has the wrong value! Got {numbers}") + else: + self.assertEqual(numbers[0], expected_value, f"Parameter {param_name} has the wrong value! Got {numbers[0]}") + + def __check_parameter_set(self, param_name: str, new_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + try: + set_parameter(param_name, new_value) + self.__check_parameter_get(param_name, new_value) + except subprocess.CalledProcessError as e: + self.fail(f"Failed to set parameter {param_name}.\n" + f"Exit-Code: {e.returncode}\n" + f"Stderr: {e.stderr}\n" + f"Stdout: {e.stdout}") + + +def get_parameter(param_name: str): + """Run a subprocess command and return its output.""" + cmd = ['ros2', 'param', 'get', '{{ ros_opts.node_name }}', param_name] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stdout + +def set_parameter(param_name: str, value: Union[list[float], float]): + """Run a subprocess command to set a parameter.""" + if isinstance(value, list): + value_str = "[" + ",".join(map(str, value)) + "]" + else: + value_str = str(value) + + cmd = ['ros2', 'param', 'set', '{{ ros_opts.node_name }}', param_name, value_str] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stderr \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp new file mode 100644 index 0000000000..0acd126ae3 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp @@ -0,0 +1,97 @@ +#ifndef {{ ros_opts.package_name | upper }}_UTILS_HPP +#define {{ ros_opts.package_name | upper }}_UTILS_HPP + +#include +#include +#include +#include +#include + +template +std::ostream& operator<<(std::ostream& os, const double (&arr)[N]) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i + 1 < N) os << ", "; + } + os << "]"; + return os; +} + +template +std::ostream& operator<<(std::ostream& os, const std::array& arr) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i < N - 1) os << ", "; + } + os << "]"; + return os; +} + +/** + * @brief Extract the diagonal of a square matrix stored as a flat row-major std::array. + * + * @tparam T element type + * @tparam N matrix dimension (NxN) + * @param flat_mat flat row-major storage of size N*N + * @return std::array diagonal elements + */ +template +inline std::array diagonal(const std::array& flat_mat) noexcept +{ + std::array d{}; + for (size_t i = 0; i < N; ++i) { + d[i] = flat_mat[i * N + i]; + } + return d; +} + +/** + * @brief Create a diagonal matrix (flat row-major) from a vector. + * + * @tparam T element type + * @tparam N vector dimension (N) + * @param v flat vector storage of size N + * @return std::array with only diagonal elements + */ +template +inline std::array diag_from_vec(const std::array& v) noexcept +{ + std::array mat{}; + // mat is zero-initialized; set only diagonal entries + for (size_t i = 0; i < N; ++i) { + mat[i * N + i] = v[i]; + } + return mat; +} + +/** + * @brief Create an alternating array, which takes a pattern of two values and expands it to a larger size. + * + * @tparam T element type + * @tparam N vector dimension (N) + * @param pattern_values flat pattern vector storage of size 2 + * @return std::array + */ +template +inline std::array create_repeating_array(const std::array& pattern_values) { + std::array alternated{}; + for (size_t i = 0; i < N; ++i) { + alternated[i] = pattern_values[i % K]; + } + return alternated; +} + + +inline std::vector range(int start, int end) { + std::vector result; + if (end <= start) return result; + result.reserve(end - start); + for (int i = start; i < end; ++i) { + result.push_back(i); + } + return result; +} + +#endif // {{ ros_opts.package_name | upper }}_UTILS_HPP diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt new file mode 100644 index 0000000000..5982d09817 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.16) +project({{ ros_opts.package_name }}_interface) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +# --- ROS DEPENDENCIES --- +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# --- INTERFACES --- +set(MSG_FILES + "msg/State.msg" + "msg/ControlInput.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_FILES} + DEPENDENCIES std_msgs +) + +ament_package() diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg new file mode 100644 index 0000000000..fd4efacb5c --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg @@ -0,0 +1,5 @@ +# Generic state message +std_msgs/Header header + +# control vector +float64[{{ dims.nu }}] u \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md new file mode 100644 index 0000000000..12a7c758d5 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md @@ -0,0 +1,17 @@ +# # {{ ros_opts.package_name | replace(from="_", to=" ") | title }} Interface + +This package is generated by the [`acados_template` package, the Python interface of acados](https://github.com/acados/acados). + +## Overview +This package contains the ROS interface, neede for communication with the acados OCP Solver. + + +## Installation +To install this package, you can use the following command: +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +```bash +colcon build --packages-select {{ ros_opts.package_name }}_interface && source install/setup.bash +``` diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg new file mode 100644 index 0000000000..e1ec27e10c --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg @@ -0,0 +1,8 @@ +# Generic state message +std_msgs/Header header + +# state vector +float64[{{ dims.nx }}] x + +# status, 0 = OK +uint8 status \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml new file mode 100644 index 0000000000..33af67459b --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml @@ -0,0 +1,26 @@ + + + + {{ ros_opts.package_name }}_interface + 0.1.0 + A Generic Interface for the acados solver + Josua Lindemann + MIT + + ament_cmake + + std_msgs + + rosidl_default_generators + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt new file mode 100644 index 0000000000..80bbcc426b --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.16) +project({{ ros_opts.package_name }}) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +# --- ROS DEPENDENCIES --- +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package({{ ros_opts.package_name }}_interface REQUIRED) + +# --- ACADOS PATHS --- +set(ACADOS_INCLUDE_PATH {{ acados_include_path }}) +set(ACADOS_LIB_DIR {{ acados_lib_path }}) +set(ACADOS_GENERATED_CODE_DIR {{ code_export_directory }}) + +# --- EXECUTABLE --- +add_executable({{ ros_opts.node_name }} + src/node.cpp +) + +# --- TARGET CONFIGURATION --- +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options({{ ros_opts.node_name }} PRIVATE -Wall -Wextra -Wpedantic) +endif() + +target_include_directories({{ ros_opts.node_name }} PUBLIC + $ + $ + ${ACADOS_GENERATED_CODE_DIR} + ${ACADOS_INCLUDE_PATH} + ${ACADOS_INCLUDE_PATH}/acados + ${ACADOS_INCLUDE_PATH}/blasfeo/include + ${ACADOS_INCLUDE_PATH}/hpipm/include +) + +target_link_libraries({{ ros_opts.node_name }} + ${ACADOS_GENERATED_CODE_DIR}/libacados_sim_solver_{{ name }}{{ shared_lib_ext }} + ${ACADOS_LIB_DIR}/libacados.so + ${ACADOS_LIB_DIR}/libblasfeo.so + ${ACADOS_LIB_DIR}/libhpipm.so + m +) + +# --- DEPENDENCIES --- +ament_target_dependencies({{ ros_opts.node_name }} + rclcpp + std_msgs + {{ ros_opts.package_name }}_interface +) + +# --- INSTALLATIONS --- +install(TARGETS + {{ ros_opts.node_name }} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + include/ + DESTINATION include +) + +install(FILES + ${ACADOS_GENERATED_CODE_DIR}/libacados_sim_solver_{{ name }}{{ shared_lib_ext }} + DESTINATION lib +) + +# --- TESTS --- +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test( + test/test_{{ ros_opts.package_name }}.launch.py + TARGET test_{{ ros_opts.package_name }} + TIMEOUT 180 + ) +endif() + +ament_package() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md new file mode 100644 index 0000000000..7760474c0d --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md @@ -0,0 +1,25 @@ +# {{ ros_opts.package_name | replace(from="_", to=" ") | title }} + +This package is generated by the [acados_template](https://github.com/acados/acados) package. + +## Overview +This package contains the ROS node generation for acados, which is a high-performance solver for optimal control problems. The generated nodes can be used to interface with acados solvers in a ROS environment. + + +## Installation +To install this package, you can use the following command: +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +```bash +colcon build --packages-select {{ ros_opts.package_name }} {{ ros_opts.package_name }}_interface && source install/setup.bash +``` + +## Usage +After building the package, you can run the generated nodes using: +```bash +ros2 run {{ ros_opts.package_name }} {{ ros_opts.node_name }} +``` + + diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp new file mode 100644 index 0000000000..7c4ab23105 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp @@ -0,0 +1,19 @@ +#ifndef {{ ros_opts.package_name | upper }}_CONFIG_H +#define {{ ros_opts.package_name | upper }}_CONFIG_H + +#include +#include +#include + + +namespace {{ ros_opts.package_name }} +{ +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} + +struct {{ ClassName }}Config { + double ts{ {{ solver_options.Tsim }} }; +}; + +} // namespace {{ ros_opts.package_name }} + +#endif // {{ ros_opts.package_name | upper }}_CONFIG_H \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp new file mode 100644 index 0000000000..631b9cf127 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp @@ -0,0 +1,231 @@ +#include "{{ ros_opts.package_name }}/node.h" + +namespace {{ ros_opts.package_name }} +{ + +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +{%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} +{%- if ns %} +{%- set control_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} +{%- else %} +{%- set control_topic = "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.state_topic %} +{%- endif %} +{{ ClassName }}::{{ ClassName }}() + : Node("{{ ros_opts.node_name }}") +{ + RCLCPP_INFO(this->get_logger(), "Initializing {{ ros_opts.node_name | replace(from="_", to=" ") | title }}..."); + + // --- default values --- + config_ = {{ ClassName }}Config(); + + // --- Parameters --- + this->declare_parameters(); + this->setup_parameter_handlers(); + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&{{ ClassName }}::on_parameter_update, this, std::placeholders::_1)); + this->load_parameters(); + + // --- Subscriber --- + control_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::ControlInput>( + "{{ control_topic }}", 10, + std::bind(&{{ ClassName }}::control_callback, this, std::placeholders::_1)); + + // --- Publisher --- + state_pub_ = this->create_publisher<{{ ros_opts.package_name }}_interface::msg::State>( + "{{ state_topic }}", 10); + + // --- Init simulator --- + this->initialize_simulator(); + this->start_integration_timer(config_.ts); +} + +{{ ClassName }}::~{{ ClassName }}() { + RCLCPP_INFO(this->get_logger(), "Shutting down and freeing Acados simulator memory."); + if (sim_capsule_) { + int status = {{ model.name }}_acados_sim_free(sim_capsule_); + if (status) { + RCLCPP_ERROR(this->get_logger(), "{{ model.name }}_acados_sim_free() returned status %d.", status); + } + status = {{ model.name }}_acados_sim_solver_free_capsule(sim_capsule_); + if (status) { + RCLCPP_ERROR(this->get_logger(), "{{ model.name }}_acados_sim_solver_free_capsule() returned status %d.", status); + } + } +} + + +// --- Core Methods --- +void {{ ClassName }}::initialize_simulator() { + sim_capsule_ = {{ model.name }}_acados_sim_solver_create_capsule(); + int status = {{ model.name }}_acados_sim_create(sim_capsule_); + if (status) { + RCLCPP_FATAL(this->get_logger(), "{{ model.name }}acados_create() failed with status %d.", status); + rclcpp::shutdown(); + } + + sim_config_ = {{ model.name }}_acados_get_sim_config(sim_capsule_); + sim_dims_ = {{ model.name }}_acados_get_sim_dims(sim_capsule_); + sim_in_ = {{ model.name }}_acados_get_sim_in(sim_capsule_); + sim_out_ = {{ model.name }}_acados_get_sim_out(sim_capsule_); + sim_opts_ = {{ model.name }}_acados_get_sim_opts(sim_capsule_); + + RCLCPP_INFO(this->get_logger(), "acados solver initialized successfully."); +} + +void {{ ClassName }}::integration_step() { + std::array u{}; + + { + std::scoped_lock lock(data_mutex_); + u = current_u_; + } + + // Update solver + this->set_u(u.data()); + + // Integrate + int status = this->sim_solve(); + this->sim_status_behaviour(status); +} + +void {{ ClassName }}::sim_status_behaviour(int status) { + this->get_next_state(xn_.data()); + this->publish_state(xn_, status); +} + + +// --- ROS Callbacks --- +void {{ ClassName }}::control_callback(const {{ ros_opts.package_name }}_interface::msg::ControlInput::SharedPtr msg) { + std::scoped_lock lock(data_mutex_); + std::copy_n(msg->u.begin(), {{ model.name | upper }}_NU, current_u_.begin()); +} + + +// --- ROS Publisher --- +void {{ ClassName }}::publish_state(const std::array& xn, int status) { + auto state_msg = std::make_unique<{{ ros_opts.package_name }}_interface::msg::State>(); + state_msg->header.stamp = this->get_clock()->now(); + state_msg->header.frame_id = ""; + state_msg->status = status; + std::copy_n(xn.begin(), {{ model.name | upper }}_NX, state_msg->x.begin()); + state_pub_->publish(std::move(state_msg)); +} + + +// --- Parameter Handling Methods --- +void {{ ClassName }}::setup_parameter_handlers() { + parameter_handlers_["{{ ros_opts.package_name }}.ts"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { + this->config_.ts = p.as_double(); + try { + this->start_integration_timer(this->config_.ts); + } catch (const std::exception& e) { + res.reason = "Failed to start integration timer, while setting parameter '" + p.get_name() + "': " + e.what(); + res.successful = false; + } + }; +} + +void {{ ClassName }}::declare_parameters() { + this->declare_parameter("{{ ros_opts.package_name }}.ts", {{ solver_options.Tsim }}); +} + +void {{ ClassName }}::load_parameters() { + this->get_parameter("{{ ros_opts.package_name }}.ts", config_.ts); +} + +rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( + const std::vector& params +) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto& param : params) { + auto& param_name = param.get_name(); + + if (parameter_handlers_.count(param_name)) { + parameter_handlers_.at(param_name)(param, result); + if (!result.successful) break; + } else { + result.reason = "Update for unknown parameter '%s' received.", param_name.c_str(); + result.successful = false; + } + } + return result; +} + +// template +// void {{ ClassName }}::get_and_check_array_param( +// const std::string& param_name, +// std::array& destination +// ) { +// auto param_value = this->get_parameter(param_name).as_double_array(); + +// if (param_value.size() != N) { +// RCLCPP_ERROR(this->get_logger(), "Parameter '%s' has the wrong size. Expected: %ld, got: %ld", +// param_name.c_str(), N, param_value.size()); +// return; +// } +// std::copy_n(param_value.begin(), N, destination.begin()); +// } + +// template +// void {{ ClassName }}::update_param_array( +// const rclcpp::Parameter& param, +// std::array& destination_array, +// rcl_interfaces::msg::SetParametersResult& result +// ) { +// auto values = param.as_double_array(); + +// if (values.size() != N) { +// result.successful = false; +// result.reason = "Parameter '" + param.get_name() + "' has size " + +// std::to_string(values.size()) + ", but expected is " + std::to_string(N) + "."; +// return; +// } + +// std::copy_n(values.begin(), N, destination_array.begin()); +// } + + +// --- Helpers --- +void {{ ClassName }}::start_integration_timer(double period_seconds) { + if (period_seconds <= 0.0) period_seconds = 0.02; + auto period = std::chrono::duration_cast( + std::chrono::duration(period_seconds)); + integration_timer_ = this->create_wall_timer( + period, + std::bind(&{{ ClassName }}::integration_step, this)); +} + + +// --- Acados Helpers --- +int {{ ClassName }}::sim_solve() { + int status = {{ model.name }}_acados_sim_solve(sim_capsule_); + if (status != ACADOS_SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Simulation Solver failed with status: %d", status); + } + return status; +} + +void {{ ClassName }}::get_next_state(double* xn) { + sim_out_get(sim_config_, sim_dims_, sim_out_, "xn", xn); +} + +void {{ ClassName }}::set_u(double* u) { + sim_in_set(sim_config_, sim_dims_, sim_in_, "u", u); +} + +} // namespace {{ ros_opts.package_name }} + + +// --- Main --- +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared<{{ ros_opts.package_name }}::{{ ClassName }}>(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h new file mode 100644 index 0000000000..56ee7bb254 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h @@ -0,0 +1,92 @@ +#ifndef {{ ros_opts.node_name | upper }}_H +#define {{ ros_opts.node_name | upper }}_H + +#include +#include +#include +#include +#include + +// ROS2 message includes +#include "{{ ros_opts.package_name }}_interface/msg/state.hpp" +#include "{{ ros_opts.package_name }}_interface/msg/control_input.hpp" +#include "std_msgs/msg/header.hpp" + +// Acados includes +#include "acados/sim/sim_common.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_sim_solver_{{ model.name }}.h" + +// Package includes +#include "{{ ros_opts.package_name }}/utils.hpp" +#include "{{ ros_opts.package_name }}/config.hpp" + + +namespace {{ ros_opts.package_name }} +{ + +{%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +class {{ ClassName }} : public rclcpp::Node { +private: + // --- ROS Subscriptions --- + rclcpp::Subscription<{{ ros_opts.package_name }}_interface::msg::ControlInput>::SharedPtr control_sub_; + + // --- ROS Publishers --- + rclcpp::Publisher<{{ ros_opts.package_name }}_interface::msg::State>::SharedPtr state_pub_; + + // --- ROS Params and Timer + rclcpp::TimerBase::SharedPtr integration_timer_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + using ParamHandler = std::function; + std::unordered_map parameter_handlers_; + + // --- Acados Solver --- + {{ model.name }}_sim_solver_capsule *sim_capsule_; + sim_config* sim_config_; + void* sim_dims_; + sim_in* sim_in_; + sim_out* sim_out_; + sim_opts* sim_opts_; + + // --- Data and States --- + std::mutex data_mutex_; + {{ ClassName }}Config config_; + + std::array xn_; + std::array current_u_; + +public: + {{ ClassName }}(); + ~{{ ClassName }}(); + +private: + // --- Core Methods --- + void initialize_simulator(); + void integration_step(); + void sim_status_behaviour(int status); + + // --- ROS Callbacks --- + void control_callback(const {{ ros_opts.package_name }}_interface::msg::ControlInput::SharedPtr msg); + + // --- ROS Publisher --- + void publish_state(const std::array& xn, int status); + + // --- Parameter Handling Methods --- + void setup_parameter_handlers(); + void declare_parameters(); + void load_parameters(); + rcl_interfaces::msg::SetParametersResult on_parameter_update(const std::vector& params); + + // --- Helpers --- + void start_integration_timer(double period_seconds = 0.02); + + // --- Acados Helpers --- + int sim_solve(); + void get_next_state(double* xn); + void set_u(double* u); +}; + +} // namespace {{ ros_opts.package_name }} + +#endif // {{ ros_opts.node_name | upper }}_H \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml new file mode 100644 index 0000000000..12fc9ed426 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml @@ -0,0 +1,23 @@ + + + + {{ ros_opts.package_name }} + 0.1.0 + A Generic ROS2 node for the acados solver + Josua Lindemann + MIT + + ament_cmake + + rclcpp + std_msgs + {{ ros_opts.package_name }}_interface + + launch_testing + launch_testing_ament_cmake + python3-pytest + + + ament_cmake + + \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py new file mode 100644 index 0000000000..a76061cf19 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py @@ -0,0 +1,137 @@ +import re +from typing import Union +from unittest import result +import rclpy +import unittest +import launch +import time +import launch_testing +import pytest +import subprocess +from launch_ros.actions import Node + +from {{ ros_opts.package_name }}_interface.msg import State, ControlInput +{%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} +{%- if ns %} +{%- set control_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} +{%- else %} +{%- set control_topic = "/" ~ ros_opts.control_topic %} +{%- set state_topic = "/" ~ ros_opts.state_topic %} +{%- endif %} + +@pytest.mark.launch_test +def generate_test_description(): + """Generate launch description for node testing.""" + start_{{ ros_opts.node_name }} = Node( + package='{{ ros_opts.package_name }}', + executable='{{ ros_opts.node_name }}', + name='{{ ros_opts.node_name }}' + ) + + return launch.LaunchDescription([ + start_{{ ros_opts.node_name }}, + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()]), + ]) + + +class GeneratedNodeTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('generated_node_test') + + def tearDown(self): + self.node.destroy_node() + + def test_parameters_set(self, proc_info): + """ + Test if all compile-time declared default parameters. + """ + # --- Solver Options --- + param_name = "{{ ros_opts.package_name }}.ts" + expected_value = {{ solver_options.Tsim }} + self.__check_parameter_set(param_name, expected_value) + + def test_subscribing(self, proc_info): + """Test if the node subscribes to all expected topics.""" + self.wait_for_subscription('{{ control_topic }}') + + def test_publishing(self, proc_info): + """Test if the node publishes to all expected topics.""" + self.wait_for_publisher('{{ state_topic }}') + + def wait_for_subscription(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + subs = self.node.get_subscriptions_info_by_topic(topic) + if subs: + return True + time.sleep(0.05) + self.fail(f"Node has NOT subscribed to '{topic}'.") + + def wait_for_publisher(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + pubs = self.node.get_publishers_info_by_topic(topic) + if pubs: + return True + time.sleep(0.05) + self.fail(f"Node has NOT published to '{topic}'.") + + def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + output = get_parameter(param_name) + numbers = [float(x) for x in re.findall(r"[-+]?\d*\.\d+|\d+", output)] + if isinstance(expected_value, list): + self.assertListEqual(numbers, expected_value, f"Parameter {param_name} has the wrong value! Got {numbers}") + else: + self.assertEqual(numbers[0], expected_value, f"Parameter {param_name} has the wrong value! Got {numbers[0]}") + + def __check_parameter_set(self, param_name: str, new_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + try: + set_parameter(param_name, new_value) + self.__check_parameter_get(param_name, new_value) + except subprocess.CalledProcessError as e: + self.fail(f"Failed to set parameter {param_name}.\n" + f"Exit-Code: {e.returncode}\n" + f"Stderr: {e.stderr}\n" + f"Stdout: {e.stdout}") + + +def get_parameter(param_name: str): + """Run a subprocess command and return its output.""" + cmd = ['ros2', 'param', 'get', '{{ ros_opts.node_name }}', param_name] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stdout + +def set_parameter(param_name: str, value: Union[list[float], float]): + """Run a subprocess command to set a parameter.""" + if isinstance(value, list): + value_str = "[" + ",".join(map(str, value)) + "]" + else: + value_str = str(value) + + cmd = ['ros2', 'param', 'set', '{{ ros_opts.node_name }}', param_name, value_str] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stderr \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp new file mode 100644 index 0000000000..0acd126ae3 --- /dev/null +++ b/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp @@ -0,0 +1,97 @@ +#ifndef {{ ros_opts.package_name | upper }}_UTILS_HPP +#define {{ ros_opts.package_name | upper }}_UTILS_HPP + +#include +#include +#include +#include +#include + +template +std::ostream& operator<<(std::ostream& os, const double (&arr)[N]) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i + 1 < N) os << ", "; + } + os << "]"; + return os; +} + +template +std::ostream& operator<<(std::ostream& os, const std::array& arr) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i < N - 1) os << ", "; + } + os << "]"; + return os; +} + +/** + * @brief Extract the diagonal of a square matrix stored as a flat row-major std::array. + * + * @tparam T element type + * @tparam N matrix dimension (NxN) + * @param flat_mat flat row-major storage of size N*N + * @return std::array diagonal elements + */ +template +inline std::array diagonal(const std::array& flat_mat) noexcept +{ + std::array d{}; + for (size_t i = 0; i < N; ++i) { + d[i] = flat_mat[i * N + i]; + } + return d; +} + +/** + * @brief Create a diagonal matrix (flat row-major) from a vector. + * + * @tparam T element type + * @tparam N vector dimension (N) + * @param v flat vector storage of size N + * @return std::array with only diagonal elements + */ +template +inline std::array diag_from_vec(const std::array& v) noexcept +{ + std::array mat{}; + // mat is zero-initialized; set only diagonal entries + for (size_t i = 0; i < N; ++i) { + mat[i * N + i] = v[i]; + } + return mat; +} + +/** + * @brief Create an alternating array, which takes a pattern of two values and expands it to a larger size. + * + * @tparam T element type + * @tparam N vector dimension (N) + * @param pattern_values flat pattern vector storage of size 2 + * @return std::array + */ +template +inline std::array create_repeating_array(const std::array& pattern_values) { + std::array alternated{}; + for (size_t i = 0; i < N; ++i) { + alternated[i] = pattern_values[i % K]; + } + return alternated; +} + + +inline std::vector range(int start, int end) { + std::vector result; + if (end <= start) return result; + result.reserve(end - start); + for (int i = start; i < end; ++i) { + result.push_back(i); + } + return result; +} + +#endif // {{ ros_opts.package_name | upper }}_UTILS_HPP diff --git a/interfaces/acados_template/acados_template/ros2/__init__.py b/interfaces/acados_template/acados_template/ros2/__init__.py new file mode 100644 index 0000000000..ebd7957f2d --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/__init__.py @@ -0,0 +1,3 @@ +from .ocp_node import AcadosOcpRosOptions +from .sim_node import AcadosSimRosOptions +from .utils import ArchType, ControlLoopExec \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/ocp_node.py b/interfaces/acados_template/acados_template/ros2/ocp_node.py new file mode 100644 index 0000000000..863567a678 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/ocp_node.py @@ -0,0 +1,105 @@ +# 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.; +# + +from .utils import ControlLoopExec, ArchType, AcadosRosBaseOptions + +# --- Ros Options --- +class AcadosOcpRosOptions(AcadosRosBaseOptions): + def __init__(self): + super().__init__() + self.package_name: str = "acados_ocp" + self.node_name: str = "" + self.namespace: str = "" + self.archtype: str = ArchType.NODE.value + self.control_loop_executor: str = ControlLoopExec.TIMER.value + + self.__control_topic = "ocp_control" + self.__state_topic = "ocp_state" + self.__parameters_topic = "ocp_params" + self.__reference_topic = "ocp_reference" + + @property + def control_topic(self) -> str: + return self.__control_topic + + @property + def state_topic(self) -> str: + return self.__state_topic + + @property + def parameters_topic(self) -> str: + return self.__parameters_topic + + @property + def reference_topic(self) -> str: + return self.__reference_topic + + @control_topic.setter + def control_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid control_topic value, expected str.\n') + self.__control_topic = value + + @state_topic.setter + def state_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid state_topic value, expected str.\n') + self.__state_topic = value + + @parameters_topic.setter + def parameters_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid parameters_topic value, expected str.\n') + self.__parameters_topic = value + + @reference_topic.setter + def reference_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid reference_topic value, expected str.\n') + self.__reference_topic = value + + def to_dict(self) -> dict: + return super().to_dict() | { + "control_topic": self.control_topic, + "state_topic": self.state_topic, + "parameters_topic": self.parameters_topic, + "reference_topic": self.reference_topic, + } + + +if __name__ == "__main__": + ros_opt = AcadosOcpRosOptions() + + # ros_opt.node_name = "my_node" + ros_opt.package_name = "that_package" + ros_opt.namespace = "/my_namespace" + # ros_opt.control_loop_executor = ControlLoopExec.TOPIC + # ros_opt.archtype = ArchType.LIFECYCLE_NODE + + print(ros_opt.to_dict()) diff --git a/interfaces/acados_template/acados_template/ros2/sim_node.py b/interfaces/acados_template/acados_template/ros2/sim_node.py new file mode 100644 index 0000000000..0ac6cc4e0f --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/sim_node.py @@ -0,0 +1,70 @@ +# 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.; +# + +from .utils import ControlLoopExec, ArchType, AcadosRosBaseOptions + + +# --- Ros Options --- +class AcadosSimRosOptions(AcadosRosBaseOptions): + def __init__(self): + super().__init__() + self.package_name: str = "acados_sim" + self.node_name: str = "" + self.namespace: str = "" + self.archtype: str = ArchType.NODE.value + self.control_loop_executor: str = ControlLoopExec.TIMER.value + + self.__control_topic = "sim_control" + self.__state_topic = "sim_state" + + @property + def control_topic(self) -> str: + return self.__control_topic + + @property + def state_topic(self) -> str: + return self.__state_topic + + @control_topic.setter + def control_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid control_topic value, expected str.\n') + self.__control_topic = value + + @state_topic.setter + def state_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid state_topic value, expected str.\n') + self.__state_topic = value + + def to_dict(self) -> dict: + return super().to_dict() | { + "control_topic": self.control_topic, + "state_topic": self.state_topic, + } \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/utils.py b/interfaces/acados_template/acados_template/ros2/utils.py new file mode 100644 index 0000000000..ef2c42fdc5 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/utils.py @@ -0,0 +1,126 @@ +import re + +from enum import Enum +from typing import Union + + +class ControlLoopExec(str, Enum): + TOPIC = "topic" + TIMER = "timer" + SRV = "srv" + ACTION = "action" + +class ArchType(str, Enum): + NODE = "node" + LIFECYCLE_NODE = "lifecycle_node" + ROS2_CONTROLLER = "ros2_controller" + NAV2_CONTROLLER = "nav2_controller" + + +class AcadosRosBaseOptions: + def __init__(self): + self.__package_name: str = "acados_base" + self.__node_name: str = "acados_base_node" + self.__namespace: str = "" + self.__archtype: str = ArchType.NODE.value + self.__control_loop_executor: str = ControlLoopExec.TIMER.value + + @property + def package_name(self) -> str: + return self.__package_name + + @property + def node_name(self) -> str: + return self.__node_name + + @property + def namespace(self) -> str: + return self.__namespace + + @property + def archtype(self) -> str: + return self.__archtype + + @property + def control_loop_executor(self) -> str: + return self.__control_loop_executor + + @package_name.setter + def package_name(self, package_name: str): + if not isinstance(package_name, str): + raise TypeError('Invalid package_name value, expected str.\n') + self.__package_name = package_name + + @node_name.setter + def node_name(self, node_name: str): + if not isinstance(node_name, str): + raise TypeError('Invalid node_name value, expected str.\n') + self.__node_name = node_name + + @namespace.setter + def namespace(self, namespace: str): + if not isinstance(namespace, str): + raise TypeError('Invalid namespace value, expected str.\n') + self.__namespace = namespace + + @archtype.setter + def archtype(self, node_archtype: Union[ArchType, str]): + try: + if isinstance(node_archtype, ArchType): + archtype_enum = node_archtype + elif isinstance(node_archtype, str): + archtype_enum = ArchType(node_archtype) + else: + raise TypeError() + except (ValueError, TypeError): + valid_types = [e.value for e in ArchType] + raise TypeError(f"Invalid node_archtype. Expected one of {valid_types} or a ArchType enum member.") + + not_implemented = [ + ArchType.ROS2_CONTROLLER, + ArchType.NAV2_CONTROLLER, + ArchType.LIFECYCLE_NODE + ] + if archtype_enum in not_implemented: + raise NotImplementedError(f"Archtype '{archtype_enum.value}' is not implemented yet.") + self.__archtype = archtype_enum.value + + @control_loop_executor.setter + def control_loop_executor(self, control_loop_executor: Union[ControlLoopExec, str]) -> None: + try: + if isinstance(control_loop_executor, ControlLoopExec): + control_loop_executor_enum = control_loop_executor + elif isinstance(control_loop_executor, str): + control_loop_executor_enum = ControlLoopExec(control_loop_executor) + else: + raise TypeError() + except (ValueError, TypeError): + valid_types = [e.value for e in ControlLoopExec] + raise TypeError(f"Invalid control_loop_executor. Expected one of {valid_types} or a ControlLoopExec enum member.") + + not_implemented = [ + ControlLoopExec.SRV, + ControlLoopExec.ACTION + ] + if control_loop_executor_enum in not_implemented: + raise NotImplementedError(f"Control loop executor '{control_loop_executor_enum.value}' is not implemented yet.") + self.__control_loop_executor = control_loop_executor_enum.value + + def to_dict(self) -> dict: + if self.node_name == "": + self.node_name = self.package_name + "_node" + package_name_snake = self.camel_to_snake(self.package_name) + node_name_snake = self.camel_to_snake(self.node_name) + namespace_snake = self.camel_to_snake(self.namespace) + return { + "package_name": package_name_snake, + "node_name": node_name_snake, + "namespace": namespace_snake, + "archtype": self.archtype, + "control_loop_executor": self.control_loop_executor + } + + @staticmethod + def camel_to_snake(name: str) -> str: + s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', name) + return re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1).lower() From 4522181842f50612f289f3d8214cee35c835b1bf Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 17 Sep 2025 16:18:01 +0200 Subject: [PATCH 138/164] Backward compatibility of tera renderer (#1635) Two issues: 1) older Linux versions with default tera binaries have issues where libg6 is not found. To fix this. Run the following in a Python file: ``` acados_template import get_tera tera_version = '0.0.34', force_download=True) ``` 2) ROS templates are not compatibile with old tera version. Only relevant if you try generating a ROS node. This PR - moved ROS templates to separate folder, to avoid loading them when not needed. There is some logic in there that is not compatible with the old tera renderer version (0.0.34). - adapt `get_tera` to be able to download older version. - print those known issues in case rendering fails. --- .github/workflows/full_build.yml | 4 +- .../acados_template/acados_ocp.py | 39 +++++++------- .../acados_template/acados_sim.py | 53 ++++++++++--------- .../ocp_interface_templates/CMakeLists.in.txt | 0 .../ControlInput.in.msg | 0 .../ocp_interface_templates/Parameters.in.msg | 0 .../ocp_interface_templates/README.in.md | 0 .../ocp_interface_templates/References.in.msg | 0 .../ocp_interface_templates/State.in.msg | 0 .../ocp_interface_templates/package.in.xml | 0 .../ocp_node_templates/CMakeLists.in.txt | 0 .../ocp_node_templates/README.in.md | 0 .../ocp_node_templates/config.in.hpp | 0 .../ocp_node_templates/node.in.cpp | 0 .../ocp_node_templates/node.in.h | 0 .../ocp_node_templates/package.in.xml | 0 .../ocp_node_templates/test.launch.in.py | 0 .../ocp_node_templates/utils.in.hpp | 0 .../sim_interface_templates/CMakeLists.in.txt | 0 .../ControlInput.in.msg | 0 .../sim_interface_templates/README.in.md | 0 .../sim_interface_templates/State.in.msg | 0 .../sim_interface_templates/package.in.xml | 0 .../sim_node_templates/CMakeLists.in.txt | 0 .../sim_node_templates/README.in.md | 0 .../sim_node_templates/config.in.hpp | 0 .../sim_node_templates/node.in.cpp | 0 .../sim_node_templates/node.in.h | 0 .../sim_node_templates/package.in.xml | 0 .../sim_node_templates/test.launch.in.py | 0 .../sim_node_templates/utils.in.hpp | 0 .../acados_template/acados_template/utils.py | 42 ++++++++++----- 32 files changed, 79 insertions(+), 59 deletions(-) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/CMakeLists.in.txt (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/ControlInput.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/Parameters.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/README.in.md (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/References.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/State.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_interface_templates/package.in.xml (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/CMakeLists.in.txt (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/README.in.md (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/config.in.hpp (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/node.in.cpp (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/node.in.h (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/package.in.xml (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/test.launch.in.py (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/ocp_node_templates/utils.in.hpp (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_interface_templates/CMakeLists.in.txt (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_interface_templates/ControlInput.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_interface_templates/README.in.md (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_interface_templates/State.in.msg (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_interface_templates/package.in.xml (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/CMakeLists.in.txt (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/README.in.md (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/config.in.hpp (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/node.in.cpp (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/node.in.h (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/package.in.xml (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/test.launch.in.py (100%) rename interfaces/acados_template/acados_template/{c_templates_tera/ros2 => ros2_templates}/sim_node_templates/utils.in.hpp (100%) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index c24fd6743a..2bbb8012b5 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -50,7 +50,7 @@ jobs: ctest -C Release --rerun-failed -V exit 1 - + python_interface_new_casadi_and_py2octave: needs: call_core_build runs-on: ubuntu-22.04 @@ -511,7 +511,7 @@ jobs: - name: Setup Python Environment uses: ./.github/actions/setup-python-environment - + - name: Test ROS2 OCP Package uses: ./.github/actions/test-ros2-package with: diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index f7f06c26a0..ceae2040ea 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1296,27 +1296,29 @@ def _get_external_function_header_templates(self, ) -> list: def _get_ros_template_list(self) -> list: template_list = [] + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + ros_template_glob = os.path.join(acados_template_path, 'ros2_templates', '**', '*') # --- Interface Package --- - ros_interface_dir = os.path.join('ros2', 'ocp_interface_templates') + ros_interface_dir = os.path.join('ocp_interface_templates') interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') template_file = os.path.join(ros_interface_dir, 'README.in.md') - template_list.append((template_file, 'README.md', interface_dir)) + template_list.append((template_file, 'README.md', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') - template_list.append((template_file, 'CMakeLists.txt', interface_dir)) + template_list.append((template_file, 'CMakeLists.txt', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'package.in.xml') - template_list.append((template_file, 'package.xml', interface_dir)) + template_list.append((template_file, 'package.xml', interface_dir, ros_template_glob)) # Messages msg_dir = os.path.join(interface_dir, 'msg') template_file = os.path.join(ros_interface_dir, 'State.in.msg') - template_list.append((template_file, 'State.msg', msg_dir)) + template_list.append((template_file, 'State.msg', msg_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'References.in.msg') - template_list.append((template_file, 'References.msg', msg_dir)) + template_list.append((template_file, 'References.msg', msg_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'Parameters.in.msg') - template_list.append((template_file, 'Parameters.msg', msg_dir)) + template_list.append((template_file, 'Parameters.msg', msg_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') - template_list.append((template_file, 'ControlInput.msg', msg_dir)) + template_list.append((template_file, 'ControlInput.msg', msg_dir, ros_template_glob)) # Services # TODO: No node implementation yet @@ -1325,33 +1327,33 @@ def _get_ros_template_list(self) -> list: # TODO: No Template yet and no node implementation # --- Solver Package --- - ros_pkg_dir = os.path.join('ros2', 'ocp_node_templates') + ros_pkg_dir = os.path.join('ocp_node_templates') package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'README.in.md') - template_list.append((template_file, 'README.md', package_dir)) + template_list.append((template_file, 'README.md', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') - template_list.append((template_file, 'CMakeLists.txt', package_dir)) + template_list.append((template_file, 'CMakeLists.txt', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'package.in.xml') - template_list.append((template_file, 'package.xml', package_dir)) + template_list.append((template_file, 'package.xml', package_dir, ros_template_glob)) # Header include_dir = os.path.join(package_dir, 'include', self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'config.in.hpp') - template_list.append((template_file, 'config.hpp', include_dir)) + template_list.append((template_file, 'config.hpp', include_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'utils.in.hpp') - template_list.append((template_file, 'utils.hpp', include_dir)) + template_list.append((template_file, 'utils.hpp', include_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'node.in.h') - template_list.append((template_file, 'node.h', include_dir)) + template_list.append((template_file, 'node.h', include_dir, ros_template_glob)) # Source src_dir = os.path.join(package_dir, 'src') template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') - template_list.append((template_file, 'node.cpp', src_dir)) + template_list.append((template_file, 'node.cpp', src_dir, ros_template_glob)) # Test test_dir = os.path.join(package_dir, 'test') template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') - template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir)) + template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir, ros_template_glob)) return template_list @@ -1443,7 +1445,8 @@ def render_templates(self, cmake_builder=None): # Render templates for tup in template_list: output_dir = self.code_export_directory if len(tup) <= 2 else tup[2] - render_template(tup[0], tup[1], output_dir, json_path) + template_glob = None if len(tup) <= 3 else tup[3] + render_template(tup[0], tup[1], output_dir, json_path, template_glob=template_glob) # Custom templates acados_template_path = os.path.dirname(os.path.abspath(__file__)) diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 0806a7be0e..16519bc690 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -354,14 +354,14 @@ def __init__(self, acados_path=''): self.__parameter_values = np.array([]) self.__problem_class = 'SIM' - + self.__ros_opts: Optional[AcadosSimRosOptions] = None @property def parameter_values(self): """:math:`p` - initial values for parameter - can be updated""" return self.__parameter_values - + @property def ros_opts(self) -> Optional[AcadosSimRosOptions]: """Options to configure ROS 2 nodes and topics.""" @@ -374,7 +374,7 @@ def parameter_values(self, parameter_values): else: raise ValueError('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') - + @ros_opts.setter def ros_opts(self, ros_opts: AcadosSimRosOptions): if not isinstance(ros_opts, AcadosSimRosOptions): @@ -416,23 +416,25 @@ def dump_to_json(self, json_file='acados_sim.json') -> None: def _get_ros_template_list(self) -> list: template_list = [] + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + ros_template_glob = os.path.join(acados_template_path, 'ros2_templates', '**', '*') - # --- Interface Package --- - ros_interface_dir = os.path.join('ros2', 'sim_interface_templates') + # --- Interface Package --- + ros_interface_dir = os.path.join('sim_interface_templates') interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') template_file = os.path.join(ros_interface_dir, 'README.in.md') - template_list.append((template_file, 'README.md', interface_dir)) + template_list.append((template_file, 'README.md', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') - template_list.append((template_file, 'CMakeLists.txt', interface_dir)) + template_list.append((template_file, 'CMakeLists.txt', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'package.in.xml') - template_list.append((template_file, 'package.xml', interface_dir)) + template_list.append((template_file, 'package.xml', interface_dir, ros_template_glob)) # Messages msg_dir = os.path.join(interface_dir, 'msg') template_file = os.path.join(ros_interface_dir, 'State.in.msg') - template_list.append((template_file, 'State.msg', msg_dir)) + template_list.append((template_file, 'State.msg', msg_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') - template_list.append((template_file, 'ControlInput.msg', msg_dir)) + template_list.append((template_file, 'ControlInput.msg', msg_dir, ros_template_glob)) # Services # TODO: No node implementation yet @@ -440,37 +442,37 @@ def _get_ros_template_list(self) -> list: # Actions # TODO: No Template yet and no node implementation - # --- Simulator Package --- - ros_pkg_dir = os.path.join('ros2', 'sim_node_templates') + # --- Simulator Package --- + ros_pkg_dir = os.path.join('sim_node_templates') package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'README.in.md') - template_list.append((template_file, 'README.md', package_dir)) + template_list.append((template_file, 'README.md', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') - template_list.append((template_file, 'CMakeLists.txt', package_dir)) + template_list.append((template_file, 'CMakeLists.txt', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'package.in.xml') - template_list.append((template_file, 'package.xml', package_dir)) + template_list.append((template_file, 'package.xml', package_dir, ros_template_glob)) # Header include_dir = os.path.join(package_dir, 'include', self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'config.in.hpp') - template_list.append((template_file, 'config.hpp', include_dir)) + template_list.append((template_file, 'config.hpp', include_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'utils.in.hpp') - template_list.append((template_file, 'utils.hpp', include_dir)) + template_list.append((template_file, 'utils.hpp', include_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'node.in.h') - template_list.append((template_file, 'node.h', include_dir)) + template_list.append((template_file, 'node.h', include_dir, ros_template_glob)) # Source src_dir = os.path.join(package_dir, 'src') template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') - template_list.append((template_file, 'node.cpp', src_dir)) - + template_list.append((template_file, 'node.cpp', src_dir, ros_template_glob)) + # Test test_dir = os.path.join(package_dir, 'test') template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') - template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir)) + template_list.append((template_file, f'test_{self.ros_opts.package_name}.launch.py', test_dir, ros_template_glob)) return template_list - - + + def _get_simulink_template_list(self, name: str) -> list: template_list = [] template_file = os.path.join('matlab_templates', 'mex_sim_solver.in.m') @@ -504,7 +506,7 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): ('acados_sim_solver.in.pxd', 'acados_sim_solver.pxd'), ('main_sim.in.c', f'main_sim_{name}.c'), ] - + # Model model_dir = os.path.join(self.code_export_directory, self.model.name + '_model') template_list.append(('model.in.h', f'{self.model.name}_model.h', model_dir)) @@ -526,7 +528,8 @@ def render_templates(self, json_file, cmake_options: CMakeBuilder = None): # Render templates for tup in template_list: output_dir = self.code_export_directory if len(tup) <= 2 else tup[2] - render_template(tup[0], tup[1], output_dir, json_path) + template_glob = None if len(tup) <= 3 else tup[3] + render_template(tup[0], tup[1], output_dir, json_path, template_glob=template_glob) def generate_external_functions(self, ): diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/CMakeLists.in.txt rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlInput.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/ControlInput.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlInput.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/Parameters.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/Parameters.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/Parameters.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/README.in.md similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/README.in.md rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/README.in.md diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/References.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/References.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/References.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/State.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/State.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/State.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/package.in.xml similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_interface_templates/package.in.xml rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/package.in.xml diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/CMakeLists.in.txt rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/README.in.md similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/README.in.md rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/README.in.md diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/config.in.hpp rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.cpp rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/node.in.h rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/package.in.xml similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/package.in.xml rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/package.in.xml diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/test.launch.in.py rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/utils.in.hpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/ocp_node_templates/utils.in.hpp rename to interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/utils.in.hpp diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/CMakeLists.in.txt rename to interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/ControlInput.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/README.in.md similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/README.in.md rename to interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/README.in.md diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/State.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/package.in.xml similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_interface_templates/package.in.xml rename to interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/package.in.xml diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/CMakeLists.in.txt rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/README.in.md rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/config.in.hpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/config.in.hpp rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/config.in.hpp diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.cpp rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/node.in.h rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/package.in.xml rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/test.launch.in.py rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py diff --git a/interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/utils.in.hpp similarity index 100% rename from interfaces/acados_template/acados_template/c_templates_tera/ros2/sim_node_templates/utils.in.hpp rename to interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/utils.in.hpp diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index f5129d0c3e..9a0e94029c 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -29,7 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from typing import Union +from typing import Union, Optional import json import os import shutil @@ -48,7 +48,7 @@ from contextlib import contextmanager -TERA_VERSION = "0.2.0" +TERA_DEFAULT_VERSION = "0.2.0" PLATFORM2TERA = { "linux": "linux", @@ -253,13 +253,16 @@ def get_architecture_amd64_arm64(): else: raise RuntimeError(f"Your detected architecture {current_arch} may not be compatible with amd64 or arm64.") -def get_tera() -> str: +def get_tera(tera_version: Optional[str] = None, force_download = False) -> str: + if tera_version is None: + tera_version = TERA_DEFAULT_VERSION tera_path = get_tera_exec_path() acados_path = get_acados_path() # check if tera exists and is executable - if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): - return tera_path + if not force_download: + if os.path.exists(tera_path) and os.access(tera_path, os.X_OK): + return tera_path try: arch = get_architecture_amd64_arm64() @@ -271,13 +274,17 @@ def get_tera() -> str: binary_ext = get_binary_ext() repo_url = "https://github.com/acados/tera_renderer/releases" url = "{}/download/v{}/t_renderer-v{}-{}-{}{}".format( - repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform], arch, binary_ext) + repo_url, tera_version, tera_version, PLATFORM2TERA[sys.platform], arch, binary_ext) + + if tera_version == "0.0.34": + url = "{}/download/v{}/t_renderer-v{}-{}".format( + repo_url, tera_version, tera_version, PLATFORM2TERA[sys.platform]) manual_install = 'For manual installation follow these instructions:\n' manual_install += '1 Download binaries from {}\n'.format(url) manual_install += '2 Copy them in {}/bin\n'.format(acados_path) manual_install += '3 Strip the version and platform and architecture from the binaries: ' - manual_install += f'as t_renderer-v{TERA_VERSION}-P-A{binary_ext} -> t_renderer{binary_ext})\n' + manual_install += f'as t_renderer-v{tera_version}-P-A{binary_ext} -> t_renderer{binary_ext})\n' manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n' manual_install += '"chmod +x {}"\n\n'.format(tera_path) @@ -290,13 +297,13 @@ def get_tera() -> str: msg += 'Do you wish to set up Tera renderer automatically?\n' msg += 'y/N? (press y to download tera or any key for manual installation)\n' - if input(msg) != 'y': - msg_cancel = "\nYou cancelled automatic download.\n\n" - msg_cancel += manual_install - msg_cancel += "Once installed re-run your script.\n\n" - print(msg_cancel) - - sys.exit(1) + if not force_download: + if input(msg) != 'y': + msg_cancel = "\nYou cancelled automatic download.\n\n" + msg_cancel += manual_install + msg_cancel += "Once installed re-run your script.\n\n" + print(msg_cancel) + sys.exit(1) # check if parent directory exists otherwise create it tera_dir = os.path.split(tera_path)[0] @@ -336,6 +343,13 @@ def render_template(in_file, out_file, output_dir, json_path, template_glob=None status = os.system(os_cmd) if status != 0: + print(f"\nRendering file {in_file} failed.\n\n", + "Known issues:\n", + "1) older Linux versions with default tera binaries have issues where a compatible libc.so is not found.\n", + "To fix this. Run the following in a Python file:\n" \ + "from acados_template import get_tera\n", + "get_tera(tera_version = '0.0.34', force_download=True)\n\n", + "2) ROS templates are not compatibile with old tera version. Only relevant if you try generating a ROS node.") raise RuntimeError(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n') From 1b56ea7121c0f2c9d4b770e37ecadc7a311e3b24 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 18 Sep 2025 15:05:09 +0200 Subject: [PATCH 139/164] Update qpoases with cmake fix (#1637) --- external/qpoases | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/qpoases b/external/qpoases index 53191bfd40..125e94fa63 160000 --- a/external/qpoases +++ b/external/qpoases @@ -1 +1 @@ -Subproject commit 53191bfd408fe49797362e20fad17d6e50688449 +Subproject commit 125e94fa638f00350608871fc165789b1d1762f1 From 51b379b620b073bb7fce6c77eafcedcd15b04bcc Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 19 Sep 2025 09:35:48 +0200 Subject: [PATCH 140/164] Fix memory alignment issues (#1614) Tested on DS1202. Solved https://discourse.acados.org/t/memory-alignment-error-building-model-on-a-microlabbox-ds1202/2912 --- acados/dense_qp/dense_qp_common.c | 2 +- acados/ocp_nlp/ocp_nlp_common.c | 6 +++++- acados/ocp_nlp/ocp_nlp_dynamics_disc.c | 1 + acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c | 1 + acados/ocp_nlp/ocp_nlp_globalization_funnel.c | 1 + .../ocp_nlp_globalization_merit_backtracking.c | 1 + acados/ocp_nlp/ocp_nlp_qpscaling.c | 6 ++++++ acados/ocp_qp/ocp_qp_common.c | 6 +++--- acados/ocp_qp/ocp_qp_full_condensing.c | 6 +++--- acados/ocp_qp/ocp_qp_osqp.c | 2 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 2 +- acados/ocp_qp/ocp_qp_xcond_solver.c | 14 ++++++++------ acados/sim/sim_gnsf.c | 2 +- interfaces/acados_c/condensing_interface.c | 2 +- interfaces/acados_c/dense_qp_interface.c | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 2 +- interfaces/acados_c/ocp_qp_interface.c | 2 +- interfaces/acados_c/sim_interface.c | 2 +- .../acados_template/acados_sim_solver.py | 1 - 19 files changed, 38 insertions(+), 23 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index f58c2f1079..e792acdc6e 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -109,7 +109,7 @@ dense_qp_dims *dense_qp_dims_assign(void *raw_memory) d_dense_qp_dim_create(dims, c_ptr); c_ptr += d_dense_qp_dim_memsize(); - assert((char *) raw_memory + dense_qp_dims_calculate_size() == c_ptr); + assert((char *) raw_memory + dense_qp_dims_calculate_size() >= c_ptr); return dims; } diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 4fc0bc614b..8145e9daab 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -827,7 +827,7 @@ acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *di dims->constraints[i]); } - size += 4*8 + 64; // aligns + size += 5*8 + 64; // aligns make_int_multiple_of(8, &size); @@ -869,6 +869,7 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * // dmask assign_and_advance_blasfeo_dvec_structs(N + 1, &in->dmask, &c_ptr); + align_char_to(8, &c_ptr); // dynamics for (int i = 0; i < N; i++) @@ -877,6 +878,7 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * config->dynamics[i]->model_assign(config->dynamics[i], dims->dynamics[i], c_ptr); c_ptr += config->dynamics[i]->model_calculate_size(config->dynamics[i], dims->dynamics[i]); + assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); } // cost @@ -884,6 +886,7 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * { in->cost[i] = config->cost[i]->model_assign(config->cost[i], dims->cost[i], c_ptr); c_ptr += config->cost[i]->model_calculate_size(config->cost[i], dims->cost[i]); + assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); } // constraints @@ -893,6 +896,7 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * dims->constraints[i], c_ptr); c_ptr += config->constraints[i]->model_calculate_size(config->constraints[i], dims->constraints[i]); + assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); } // ** doubles ** diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c index 9fa539d1e8..6dfa0ba2b7 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c @@ -532,6 +532,7 @@ acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config_, void *di acados_size_t size = 0; size += sizeof(ocp_nlp_dynamics_disc_model); + make_int_multiple_of(8, &size); return size; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c index e0f0ee5998..92e4564e9b 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c @@ -130,6 +130,7 @@ acados_size_t ocp_nlp_globalization_fixed_step_memory_calculate_size(void *confi acados_size_t size = 0; size += sizeof(ocp_nlp_globalization_fixed_step_memory); + size += 2*8; // aligns return size; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c index 0dd2041861..f537c44233 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_funnel.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_funnel.c @@ -200,6 +200,7 @@ acados_size_t ocp_nlp_globalization_funnel_memory_calculate_size(void *config_, acados_size_t size = 0; size += sizeof(ocp_nlp_globalization_funnel_memory); + size += 2*8; return size; } diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 350cba03fd..7a7e428f56 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -119,6 +119,7 @@ acados_size_t ocp_nlp_globalization_merit_backtracking_memory_calculate_size(voi acados_size_t size = 0; size += sizeof(ocp_nlp_globalization_merit_backtracking_memory); + size += 2*8; return size; } diff --git a/acados/ocp_nlp/ocp_nlp_qpscaling.c b/acados/ocp_nlp/ocp_nlp_qpscaling.c index 05cc9e3caa..9fc12e8886 100644 --- a/acados/ocp_nlp/ocp_nlp_qpscaling.c +++ b/acados/ocp_nlp/ocp_nlp_qpscaling.c @@ -170,6 +170,7 @@ acados_size_t ocp_nlp_qpscaling_memory_calculate_size(ocp_nlp_qpscaling_dims *di acados_size_t size = 0; size += sizeof(ocp_nlp_qpscaling_memory); + size += 8; if (opts->scale_qp_objective != NO_OBJECTIVE_SCALING || opts->scale_qp_constraints != NO_CONSTRAINT_SCALING) @@ -181,6 +182,7 @@ acados_size_t ocp_nlp_qpscaling_memory_calculate_size(ocp_nlp_qpscaling_dims *di // constraints_scaling_vec if (opts->scale_qp_constraints) { + size += 32; size += (N + 1) * sizeof(struct blasfeo_dvec); // constraints_scaling_vec for (i = 0; i <= N; i++) { @@ -205,6 +207,8 @@ void *ocp_nlp_qpscaling_memory_assign(ocp_nlp_qpscaling_dims *dims, void *opts_, ocp_nlp_qpscaling_memory *mem = (ocp_nlp_qpscaling_memory *) c_ptr; c_ptr += sizeof(ocp_nlp_qpscaling_memory); + align_char_to(8, &c_ptr); + mem->status = ACADOS_SUCCESS; if (opts->scale_qp_objective != NO_OBJECTIVE_SCALING || @@ -219,6 +223,8 @@ void *ocp_nlp_qpscaling_memory_assign(ocp_nlp_qpscaling_dims *dims, void *opts_, if (opts->scale_qp_constraints) { assign_and_advance_blasfeo_dvec_structs(N + 1, &mem->constraints_scaling_vec, &c_ptr); + align_char_to(32, &c_ptr); + for (int i = 0; i <= N; ++i) { assign_and_advance_blasfeo_dvec_mem(orig_qp_dim->ng[i], mem->constraints_scaling_vec + i, &c_ptr); diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 7e5e3644c3..4e14dd4127 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -302,7 +302,7 @@ ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory) d_ocp_qp_seed_create(dims, qp_seed, c_ptr); c_ptr += d_ocp_qp_seed_memsize(dims); - assert((char *) raw_memory + ocp_qp_seed_calculate_size(dims) == c_ptr); + assert((char *) raw_memory + ocp_qp_seed_calculate_size(dims) >= c_ptr); return qp_seed; } @@ -524,7 +524,7 @@ ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory) d_ocp_qp_res_create(dims, qp_res, c_ptr); c_ptr += d_ocp_qp_res_memsize(dims); - assert((char *) raw_memory + ocp_qp_res_calculate_size(dims) == c_ptr); + assert((char *) raw_memory + ocp_qp_res_calculate_size(dims) >= c_ptr); return qp_res; } @@ -550,7 +550,7 @@ ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) d_ocp_qp_res_ws_create(dims, qp_res_ws, c_ptr); c_ptr += d_ocp_qp_res_ws_memsize(dims); - assert((char *) raw_memory + ocp_qp_res_workspace_calculate_size(dims) == c_ptr); + assert((char *) raw_memory + ocp_qp_res_workspace_calculate_size(dims) >= c_ptr); return qp_res_ws; } diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index 904145319a..fa3560f49f 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -323,7 +323,7 @@ acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims_, void *op size += sizeof(struct d_ocp_qp_reduce_eq_dof_ws); size += d_ocp_qp_reduce_eq_dof_ws_memsize(dims->orig_dims); - size += 2*8; + size += 3*8; return size; } @@ -352,8 +352,8 @@ void *ocp_qp_full_condensing_memory_assign(void *dims_, void *opts_, void *raw_m mem->hpipm_red_work = (struct d_ocp_qp_reduce_eq_dof_ws *) c_ptr; c_ptr += sizeof(struct d_ocp_qp_reduce_eq_dof_ws); -// align_char_to(8, &c_ptr); -// assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); + // align before calling assign functions + align_char_to(8, &c_ptr); // hpipm_cond_work d_cond_qp_ws_create(dims->red_dims, opts->hpipm_cond_opts, mem->hpipm_cond_work, c_ptr); diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index 54e9ca5ae0..b8d9d669dc 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -979,7 +979,7 @@ void *ocp_qp_osqp_opts_assign(void *config_, void *dims_, void *raw_memory) opts->osqp_opts = (OSQPSettings *) c_ptr; c_ptr += sizeof(OSQPSettings); - assert((char *) raw_memory + ocp_qp_osqp_opts_calculate_size(config_, dims_) == c_ptr); + assert((char *) raw_memory + ocp_qp_osqp_opts_calculate_size(config_, dims_) >= c_ptr); return (void *) opts; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index d28fc64b38..f468e5435e 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -396,7 +396,7 @@ acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims_, void size += sizeof(struct d_ocp_qp_reduce_eq_dof_ws); size += d_ocp_qp_reduce_eq_dof_ws_memsize(dims->orig_dims); - size += 2*8; + size += 3*8; // aligns make_int_multiple_of(8, &size); return size; diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.c b/acados/ocp_qp/ocp_qp_xcond_solver.c index 9150a0eb0c..4ef98ea01b 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -119,7 +119,7 @@ ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config_, int N, dims->xcond_dims = config->xcond->dims_assign(config->xcond, N, c_ptr); c_ptr += config->xcond->dims_calculate_size(config->xcond, N); - assert((char *) raw_memory + ocp_qp_xcond_solver_dims_calculate_size(config_, N) == c_ptr); + assert((char *) raw_memory + ocp_qp_xcond_solver_dims_calculate_size(config_, N) >= c_ptr); return dims; } @@ -191,6 +191,8 @@ acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config_, ocp_qp_xcon size += xcond->opts_calculate_size(dims->xcond_dims); size += qp_solver->opts_calculate_size(qp_solver, xcond_qp_dims); + size += 8; + make_int_multiple_of(8, &size); return size; } @@ -210,8 +212,7 @@ void *ocp_qp_xcond_solver_opts_assign(void *config_, ocp_qp_xcond_solver_dims *d ocp_qp_xcond_solver_opts *opts = (ocp_qp_xcond_solver_opts *) c_ptr; c_ptr += sizeof(ocp_qp_xcond_solver_opts); - - assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); + align_char_to(8, &c_ptr); opts->xcond_opts = xcond->opts_assign(dims->xcond_dims, c_ptr); c_ptr += xcond->opts_calculate_size(dims->xcond_dims); @@ -221,7 +222,7 @@ void *ocp_qp_xcond_solver_opts_assign(void *config_, ocp_qp_xcond_solver_dims *d opts->qp_solver_opts = qp_solver->opts_assign(qp_solver, xcond_qp_dims, c_ptr); c_ptr += qp_solver->opts_calculate_size(qp_solver, xcond_qp_dims); - assert((char *) raw_memory + ocp_qp_xcond_solver_opts_calculate_size(config_, dims) == c_ptr); + assert((char *) raw_memory + ocp_qp_xcond_solver_opts_calculate_size(config_, dims) >= c_ptr); return (void *) opts; } @@ -336,6 +337,8 @@ acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config_, ocp_qp_xc size += qp_solver->memory_calculate_size(qp_solver, xcond_qp_dims, opts->qp_solver_opts); + size += 8; + return size; } @@ -358,8 +361,7 @@ void *ocp_qp_xcond_solver_memory_assign(void *config_, ocp_qp_xcond_solver_dims ocp_qp_xcond_solver_memory *mem = (ocp_qp_xcond_solver_memory *) c_ptr; c_ptr += sizeof(ocp_qp_xcond_solver_memory); - - assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); + align_char_to(8, &c_ptr); mem->xcond_memory = xcond->memory_assign(dims->xcond_dims, opts->xcond_opts, c_ptr); c_ptr += xcond->memory_calculate_size(dims->xcond_dims, opts->xcond_opts); diff --git a/acados/sim/sim_gnsf.c b/acados/sim/sim_gnsf.c index 9c7401167f..81835b2480 100644 --- a/acados/sim/sim_gnsf.c +++ b/acados/sim/sim_gnsf.c @@ -79,7 +79,7 @@ void *sim_gnsf_dims_assign(void *config_, void *raw_memory) dims->ny = 0; dims->nuhat = 0; - assert((char *) raw_memory + sim_gnsf_dims_calculate_size() == c_ptr); + assert((char *) raw_memory + sim_gnsf_dims_calculate_size() >= c_ptr); return dims; } diff --git a/interfaces/acados_c/condensing_interface.c b/interfaces/acados_c/condensing_interface.c index 581dfe45ac..96ab3a8d02 100644 --- a/interfaces/acados_c/condensing_interface.c +++ b/interfaces/acados_c/condensing_interface.c @@ -100,7 +100,7 @@ condensing_module *ocp_qp_condensing_assign(ocp_qp_xcond_config *config, void *d module->work = (void *) c_ptr; c_ptr += config->workspace_calculate_size(dims_, opts_); - assert((char *) raw_memory + ocp_qp_condensing_calculate_size(config, dims_, opts_) == c_ptr); + assert((char *) raw_memory + ocp_qp_condensing_calculate_size(config, dims_, opts_) >= c_ptr); return module; } diff --git a/interfaces/acados_c/dense_qp_interface.c b/interfaces/acados_c/dense_qp_interface.c index a69d91e0c4..3161f7abd8 100644 --- a/interfaces/acados_c/dense_qp_interface.c +++ b/interfaces/acados_c/dense_qp_interface.c @@ -173,7 +173,7 @@ dense_qp_solver *dense_qp_assign(qp_solver_config *config, dense_qp_dims *dims, solver->work = (void *) c_ptr; c_ptr += config->workspace_calculate_size(config, dims, opts_); - assert((char *) raw_memory + dense_qp_calculate_size(config, dims, opts_) == c_ptr); + assert((char *) raw_memory + dense_qp_calculate_size(config, dims, opts_) >= c_ptr); return solver; } diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index cc2432e49d..2c00e56fd8 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1337,7 +1337,7 @@ static ocp_nlp_solver *ocp_nlp_assign(ocp_nlp_config *config, ocp_nlp_dims *dims solver->work = (void *) c_ptr; c_ptr += config->workspace_calculate_size(config, dims, opts_, nlp_in); - assert((char *) raw_memory + ocp_nlp_calculate_size(config, dims, opts_, nlp_in) == c_ptr); + assert((char *) raw_memory + ocp_nlp_calculate_size(config, dims, opts_, nlp_in) >= c_ptr); return solver; } diff --git a/interfaces/acados_c/ocp_qp_interface.c b/interfaces/acados_c/ocp_qp_interface.c index 3d9648b958..128f31bc69 100644 --- a/interfaces/acados_c/ocp_qp_interface.c +++ b/interfaces/acados_c/ocp_qp_interface.c @@ -388,7 +388,7 @@ ocp_qp_solver *ocp_qp_assign(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_so solver->work = (void *) c_ptr; c_ptr += config->workspace_calculate_size(config, dims, opts_); - assert((char *) raw_memory + ocp_qp_calculate_size(config, dims, opts_) == c_ptr); + assert((char *) raw_memory + ocp_qp_calculate_size(config, dims, opts_) >= c_ptr); return solver; } diff --git a/interfaces/acados_c/sim_interface.c b/interfaces/acados_c/sim_interface.c index fc8b25a294..ffe382ff74 100644 --- a/interfaces/acados_c/sim_interface.c +++ b/interfaces/acados_c/sim_interface.c @@ -343,7 +343,7 @@ sim_solver *sim_assign(sim_config *config, void *dims, void *opts_, sim_in *in, config->set_external_fun_workspaces(config, dims, opts_, in->model, c_ptr); c_ptr += config->get_external_fun_workspace_requirement(config, dims, opts_, in->model); - assert((char *) raw_memory + sim_calculate_size(config, dims, opts_, in) == c_ptr); + assert((char *) raw_memory + sim_calculate_size(config, dims, opts_, in) >= c_ptr); return solver; } diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index ccaa695baa..2467e02dd8 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -84,7 +84,6 @@ def T(self,): """`T` - Simulation time.""" return self.__T - # TODO move this to AcadosSim @classmethod def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None): """ From ddfb2ac12407804f7aee3eaa2a75935f71afdd47 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Mon, 22 Sep 2025 14:47:25 +0200 Subject: [PATCH 141/164] Python: Cast in setters (#1638) Try to cast to 1d or 2d numpy array in setters. --- .../acados_template/acados_ocp_constraints.py | 333 ++++++------------ .../acados_template/acados_ocp_cost.py | 112 +++--- .../acados_template/acados_template/utils.py | 51 ++- 3 files changed, 201 insertions(+), 295 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_constraints.py b/interfaces/acados_template/acados_template/acados_ocp_constraints.py index 01cfc2e16e..9133010f34 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_constraints.py +++ b/interfaces/acados_template/acados_template/acados_ocp_constraints.py @@ -30,7 +30,7 @@ # import numpy as np -from .utils import J_to_idx, print_J_to_idx_note, J_to_idx_slack, check_if_nparray_and_flatten, check_if_2d_nparray, is_empty +from .utils import J_to_idx, print_J_to_idx_note, J_to_idx_slack, cast_to_1d_nparray, cast_to_2d_nparray, is_empty class AcadosOcpConstraints: """ @@ -44,6 +44,8 @@ class AcadosOcpConstraints: Option 2) is the new way of formulating soft constraints, which is more flexible, as one slack variable can be used for multiple constraints. """ def __init__(self): + self.__constr_types = ('BGH', 'BGP') + self.__constr_type_0 = 'BGH' self.__constr_type = 'BGH' self.__constr_type_e = 'BGH' @@ -861,58 +863,49 @@ def has_x0(self): # SETTERS @constr_type.setter def constr_type(self, constr_type): - constr_types = ('BGH', 'BGP') - if constr_type in constr_types: + if constr_type in self.__constr_types: self.__constr_type = constr_type else: raise ValueError('Invalid constr_type value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n') + + ',\n'.join(self.__constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n') @constr_type_0.setter def constr_type_0(self, constr_type_0): - constr_types = ('BGH', 'BGP') - if constr_type_0 in constr_types: + if constr_type_0 in self.__constr_types: self.__constr_type_0 = constr_type_0 else: raise ValueError('Invalid constr_type_0 value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_0 + '.\n\n') + + ',\n'.join(self.__constr_types) + '.\n\nYou have: ' + constr_type_0 + '.\n\n') @constr_type_e.setter def constr_type_e(self, constr_type_e): - constr_types = ('BGH', 'BGP') - if constr_type_e in constr_types: + if constr_type_e in self.__constr_types: self.__constr_type_e = constr_type_e else: raise ValueError('Invalid constr_type_e value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n') + + ',\n'.join(self.__constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n') # initial x @lbx_0.setter def lbx_0(self, lbx_0): - lbx_0 = check_if_nparray_and_flatten(lbx_0, "lbx_0") - self.__lbx_0 = lbx_0 + self.__lbx_0 = cast_to_1d_nparray(lbx_0, "lbx_0") @ubx_0.setter def ubx_0(self, ubx_0): - ubx_0 = check_if_nparray_and_flatten(ubx_0, "ubx_0") - self.__ubx_0 = ubx_0 + self.__ubx_0 = cast_to_1d_nparray(ubx_0, "ubx_0") @idxbx_0.setter def idxbx_0(self, idxbx_0): - idxbx_0 = check_if_nparray_and_flatten(idxbx_0, "idxbx_0") - self.__idxbx_0 = idxbx_0 + self.__idxbx_0 = cast_to_1d_nparray(idxbx_0, "idxbx_0") @Jbx_0.setter def Jbx_0(self, Jbx_0): - if isinstance(Jbx_0, np.ndarray): - self.__idxbx_0 = J_to_idx(Jbx_0) - else: - raise ValueError('Invalid Jbx_0 value.') + Jbx_0 = cast_to_2d_nparray(Jbx_0, "Jbx_0") + self.__idxbx_0 = J_to_idx(Jbx_0) @idxbxe_0.setter def idxbxe_0(self, idxbxe_0): - idxbxe_0 = check_if_nparray_and_flatten(idxbxe_0, "idxbxe_0") - self.__idxbxe_0 = idxbxe_0 + self.__idxbxe_0 = cast_to_1d_nparray(idxbxe_0, "idxbxe_0") @x0.setter def x0(self, x0): @@ -923,7 +916,7 @@ def x0(self, x0): self.__idxbx_0 = np.array([]) self.__idxbxe_0 = np.array([]) else: - x0 = check_if_nparray_and_flatten(x0, "x0") + x0 = cast_to_1d_nparray(x0, "x0") self.__lbx_0 = x0 self.__ubx_0 = x0 self.__idxbx_0 = np.arange(x0.size) @@ -933,471 +926,373 @@ def x0(self, x0): # bounds on x @lbx.setter def lbx(self, lbx): - lbx = check_if_nparray_and_flatten(lbx, "lbx") - self.__lbx = lbx + self.__lbx = cast_to_1d_nparray(lbx, "lbx") @ubx.setter def ubx(self, ubx): - ubx = check_if_nparray_and_flatten(ubx, "ubx") - self.__ubx = ubx + self.__ubx = cast_to_1d_nparray(ubx, "ubx") @idxbx.setter def idxbx(self, idxbx): - idxbx = check_if_nparray_and_flatten(idxbx, "idxbx") - self.__idxbx = idxbx + self.__idxbx = cast_to_1d_nparray(idxbx, "idxbx") @Jbx.setter def Jbx(self, Jbx): - if isinstance(Jbx, np.ndarray): - self.__idxbx = J_to_idx(Jbx) - else: - raise ValueError('Invalid Jbx value.') + Jbx = cast_to_2d_nparray(Jbx, "Jbx") + self.__idxbx = J_to_idx(Jbx) # bounds on u @lbu.setter def lbu(self, lbu): - lbu = check_if_nparray_and_flatten(lbu, "lbu") - self.__lbu = lbu + self.__lbu = cast_to_1d_nparray(lbu, "lbu") @ubu.setter def ubu(self, ubu): - ubu = check_if_nparray_and_flatten(ubu, "ubu") - self.__ubu = ubu + self.__ubu = cast_to_1d_nparray(ubu, "ubu") @idxbu.setter def idxbu(self, idxbu): - idxbu = check_if_nparray_and_flatten(idxbu, "idxbu") - self.__idxbu = idxbu + self.__idxbu = cast_to_1d_nparray(idxbu, "idxbu") @Jbu.setter def Jbu(self, Jbu): - if isinstance(Jbu, np.ndarray): - self.__idxbu = J_to_idx(Jbu) - else: - raise ValueError('Invalid Jbu value.') + Jbu = cast_to_2d_nparray(Jbu, "Jbu") + self.__idxbu = J_to_idx(Jbu) # bounds on x at shooting node N @lbx_e.setter def lbx_e(self, lbx_e): - lbx_e = check_if_nparray_and_flatten(lbx_e, "lbx_e") - self.__lbx_e = lbx_e + self.__lbx_e = cast_to_1d_nparray(lbx_e, "lbx_e") @ubx_e.setter def ubx_e(self, ubx_e): - ubx_e = check_if_nparray_and_flatten(ubx_e, "ubx_e") - self.__ubx_e = ubx_e + self.__ubx_e = cast_to_1d_nparray(ubx_e, "ubx_e") @idxbx_e.setter def idxbx_e(self, idxbx_e): - idxbx_e = check_if_nparray_and_flatten(idxbx_e, "idxbx_e") - self.__idxbx_e = idxbx_e + self.__idxbx_e = cast_to_1d_nparray(idxbx_e, "idxbx_e") @Jbx_e.setter def Jbx_e(self, Jbx_e): - if isinstance(Jbx_e, np.ndarray): - self.__idxbx_e = J_to_idx(Jbx_e) - else: - raise ValueError('Invalid Jbx_e value.') + Jbx_e = cast_to_2d_nparray(Jbx_e, "Jbx_e") + self.__idxbx_e = J_to_idx(Jbx_e) # polytopic constraints @D.setter def D(self, D): - check_if_2d_nparray(D, "D") - self.__D = D + self.__D = cast_to_2d_nparray(D, "D") @C.setter def C(self, C): - check_if_2d_nparray(C, "C") - self.__C = C + self.__C = cast_to_2d_nparray(C, "C") # polytopic constraints at shooting node N @C_e.setter def C_e(self, C_e): - check_if_2d_nparray(C_e, "C_e") - self.__C_e = C_e + self.__C_e = cast_to_2d_nparray(C_e, "C_e") @lg.setter def lg(self, value): - value = check_if_nparray_and_flatten(value, 'lg') - self.__lg = value + self.__lg = cast_to_1d_nparray(value, 'lg') @ug.setter def ug(self, value): - value = check_if_nparray_and_flatten(value, 'ug') - self.__ug = value + self.__ug = cast_to_1d_nparray(value, 'ug') @lg_e.setter def lg_e(self, value): - value = check_if_nparray_and_flatten(value, 'lg_e') - self.__lg_e = value + self.__lg_e = cast_to_1d_nparray(value, 'lg_e') @ug_e.setter def ug_e(self, value): - value = check_if_nparray_and_flatten(value, 'ug_e') - self.__ug_e = value + self.__ug_e = cast_to_1d_nparray(value, 'ug_e') # nonlinear constraints @lh.setter def lh(self, value): - value = check_if_nparray_and_flatten(value, 'lh') - self.__lh = value + self.__lh = cast_to_1d_nparray(value, 'lh') @uh.setter def uh(self, value): - value = check_if_nparray_and_flatten(value, 'uh') - self.__uh = value + self.__uh = cast_to_1d_nparray(value, 'uh') @lh_e.setter def lh_e(self, value): - value = check_if_nparray_and_flatten(value, 'lh_e') - self.__lh_e = value + self.__lh_e = cast_to_1d_nparray(value, 'lh_e') @uh_e.setter def uh_e(self, value): - value = check_if_nparray_and_flatten(value, 'uh_e') - self.__uh_e = value + self.__uh_e = cast_to_1d_nparray(value, 'uh_e') @lh_0.setter def lh_0(self, value): - value = check_if_nparray_and_flatten(value, 'lh_0') - self.__lh_0 = value + self.__lh_0 = cast_to_1d_nparray(value, 'lh_0') @uh_0.setter def uh_0(self, value): - value = check_if_nparray_and_flatten(value, 'uh_0') - self.__uh_0 = value + self.__uh_0 = cast_to_1d_nparray(value, 'uh_0') # convex-over-nonlinear constraints @lphi.setter def lphi(self, value): - value = check_if_nparray_and_flatten(value, 'lphi') - self.__lphi = value + self.__lphi = cast_to_1d_nparray(value, 'lphi') @uphi.setter def uphi(self, value): - value = check_if_nparray_and_flatten(value, 'uphi') - self.__uphi = value + self.__uphi = cast_to_1d_nparray(value, 'uphi') @lphi_e.setter def lphi_e(self, value): - value = check_if_nparray_and_flatten(value, 'lphi_e') - self.__lphi_e = value + self.__lphi_e = cast_to_1d_nparray(value, 'lphi_e') @uphi_e.setter def uphi_e(self, value): - value = check_if_nparray_and_flatten(value, 'uphi_e') - self.__uphi_e = value + self.__uphi_e = cast_to_1d_nparray(value, 'uphi_e') @lphi_0.setter def lphi_0(self, value): - value = check_if_nparray_and_flatten(value, 'lphi_0') - self.__lphi_0 = value + self.__lphi_0 = cast_to_1d_nparray(value, 'lphi_0') @uphi_0.setter def uphi_0(self, value): - value = check_if_nparray_and_flatten(value, 'uphi_0') - self.__uphi_0 = value + self.__uphi_0 = cast_to_1d_nparray(value, 'uphi_0') # idxs_rev slack formulation @idxs_rev_0.setter def idxs_rev_0(self, idxs_rev_0): - idxs_rev_0 = check_if_nparray_and_flatten(idxs_rev_0, "idxs_rev_0") - self.__idxs_rev_0 = idxs_rev_0 + self.__idxs_rev_0 = cast_to_1d_nparray(idxs_rev_0, "idxs_rev_0") @idxs_rev.setter def idxs_rev(self, idxs_rev): - idxs_rev = check_if_nparray_and_flatten(idxs_rev, "idxs_rev") - self.__idxs_rev = idxs_rev + self.__idxs_rev = cast_to_1d_nparray(idxs_rev, "idxs_rev") @idxs_rev_e.setter def idxs_rev_e(self, idxs_rev_e): - idxs_rev_e = check_if_nparray_and_flatten(idxs_rev_e, "idxs_rev_e") - self.__idxs_rev_e = idxs_rev_e + self.__idxs_rev_e = cast_to_1d_nparray(idxs_rev_e, "idxs_rev_e") @ls_0.setter def ls_0(self, ls_0): - ls_0 = check_if_nparray_and_flatten(ls_0, "ls_0") - self.__ls_0 = ls_0 + self.__ls_0 = cast_to_1d_nparray(ls_0, "ls_0") @ls.setter def ls(self, ls): - ls = check_if_nparray_and_flatten(ls, "ls") - self.__ls = ls + self.__ls = cast_to_1d_nparray(ls, "ls") @ls_e.setter def ls_e(self, ls_e): - ls_e = check_if_nparray_and_flatten(ls_e, "ls_e") - self.__ls_e = ls_e + self.__ls_e = cast_to_1d_nparray(ls_e, "ls_e") @us_0.setter def us_0(self, us_0): - us_0 = check_if_nparray_and_flatten(us_0, "us_0") - self.__us_0 = us_0 + self.__us_0 = cast_to_1d_nparray(us_0, "us_0") @us.setter def us(self, us): - us = check_if_nparray_and_flatten(us, "us") - self.__us = us + self.__us = cast_to_1d_nparray(us, "us") @us_e.setter def us_e(self, us_e): - us_e = check_if_nparray_and_flatten(us_e, "us_e") - self.__us_e = us_e + self.__us_e = cast_to_1d_nparray(us_e, "us_e") # SLACK bounds # soft bounds on x @lsbx.setter def lsbx(self, value): - value = check_if_nparray_and_flatten(value, 'lsbx') - self.__lsbx = value + self.__lsbx = cast_to_1d_nparray(value, 'lsbx') @usbx.setter def usbx(self, value): - value = check_if_nparray_and_flatten(value, 'usbx') - self.__usbx = value + self.__usbx = cast_to_1d_nparray(value, 'usbx') @idxsbx.setter def idxsbx(self, idxsbx): - idxsbx = check_if_nparray_and_flatten(idxsbx, "idxsbx") - self.__idxsbx = idxsbx + self.__idxsbx = cast_to_1d_nparray(idxsbx, "idxsbx") @Jsbx.setter def Jsbx(self, Jsbx): - if isinstance(Jsbx, np.ndarray): - self.__idxsbx = J_to_idx_slack(Jsbx) - else: - raise TypeError('Invalid Jsbx value, expected numpy array.') + Jsbx = cast_to_2d_nparray(Jsbx, "Jsbx") + self.__idxsbx = J_to_idx_slack(Jsbx) # soft bounds on u @lsbu.setter def lsbu(self, value): - value = check_if_nparray_and_flatten(value, 'lsbu') - self.__lsbu = value + self.__lsbu = cast_to_1d_nparray(value, 'lsbu') @usbu.setter def usbu(self, value): - value = check_if_nparray_and_flatten(value, 'usbu') - self.__usbu = value + self.__usbu = cast_to_1d_nparray(value, 'usbu') @idxsbu.setter def idxsbu(self, idxsbu): - idxsbu = check_if_nparray_and_flatten(idxsbu, "idxsbu") - self.__idxsbu = idxsbu + self.__idxsbu = cast_to_1d_nparray(idxsbu, "idxsbu") @Jsbu.setter def Jsbu(self, Jsbu): - if isinstance(Jsbu, np.ndarray): - self.__idxsbu = J_to_idx_slack(Jsbu) - else: - raise ValueError('Invalid Jsbu value.') + Jsbu = cast_to_2d_nparray(Jsbu, "Jsbu") + self.__idxsbu = J_to_idx_slack(Jsbu) # soft bounds on x at shooting node N @lsbx_e.setter def lsbx_e(self, value): - value = check_if_nparray_and_flatten(value, 'lsbx_e') - self.__lsbx_e = value + self.__lsbx_e = cast_to_1d_nparray(value, 'lsbx_e') @usbx_e.setter def usbx_e(self, value): - value = check_if_nparray_and_flatten(value, 'usbx_e') - self.__usbx_e = value + self.__usbx_e = cast_to_1d_nparray(value, 'usbx_e') @idxsbx_e.setter def idxsbx_e(self, idxsbx_e): - idxsbx_e = check_if_nparray_and_flatten(idxsbx_e, "idxsbx_e") - self.__idxsbx_e = idxsbx_e + self.__idxsbx_e = cast_to_1d_nparray(idxsbx_e, "idxsbx_e") @Jsbx_e.setter def Jsbx_e(self, Jsbx_e): - if isinstance(Jsbx_e, np.ndarray): - self.__idxsbx_e = J_to_idx_slack(Jsbx_e) - else: - raise ValueError('Invalid Jsbx_e value.') + Jsbx_e = cast_to_2d_nparray(Jsbx_e, "Jsbx_e") + self.__idxsbx_e = J_to_idx_slack(Jsbx_e) # soft bounds on general linear constraints @lsg.setter def lsg(self, value): - value = check_if_nparray_and_flatten(value, 'lsg') - self.__lsg = value + self.__lsg = cast_to_1d_nparray(value, 'lsg') @usg.setter def usg(self, value): - value = check_if_nparray_and_flatten(value, 'usg') - self.__usg = value + self.__usg = cast_to_1d_nparray(value, 'usg') @idxsg.setter def idxsg(self, value): - value = check_if_nparray_and_flatten(value, 'idxsg') - self.__idxsg = value + self.__idxsg = cast_to_1d_nparray(value, 'idxsg') @Jsg.setter def Jsg(self, Jsg): - if isinstance(Jsg, np.ndarray): - self.__idxsg = J_to_idx_slack(Jsg) - else: - raise TypeError('Invalid Jsg value, expected numpy array.') + Jsg = cast_to_2d_nparray(Jsg, "Jsg") + self.__idxsg = J_to_idx_slack(Jsg) # soft bounds on nonlinear constraints @lsh.setter def lsh(self, value): - value = check_if_nparray_and_flatten(value, 'lsh') - self.__lsh = value + self.__lsh = cast_to_1d_nparray(value, 'lsh') @ush.setter def ush(self, value): - value = check_if_nparray_and_flatten(value, 'ush') - self.__ush = value + self.__ush = cast_to_1d_nparray(value, 'ush') @idxsh.setter def idxsh(self, value): - value = check_if_nparray_and_flatten(value, 'idxsh') - self.__idxsh = value + self.__idxsh = cast_to_1d_nparray(value, 'idxsh') @Jsh.setter def Jsh(self, Jsh): - if isinstance(Jsh, np.ndarray): - self.__idxsh = J_to_idx_slack(Jsh) - else: - raise TypeError('Invalid Jsh value, expected numpy array.') + Jsh = cast_to_2d_nparray(Jsh, "Jsh") + self.__idxsh = J_to_idx_slack(Jsh) # soft bounds on convex-over-nonlinear constraints @lsphi.setter def lsphi(self, value): - value = check_if_nparray_and_flatten(value, 'lsphi') - self.__lsphi = value + self.__lsphi = cast_to_1d_nparray(value, 'lsphi') @usphi.setter def usphi(self, value): - value = check_if_nparray_and_flatten(value, 'usphi') - self.__usphi = value + self.__usphi = cast_to_1d_nparray(value, 'usphi') @idxsphi.setter def idxsphi(self, value): - value = check_if_nparray_and_flatten(value, 'idxsphi') - self.__idxsphi = value + self.__idxsphi = cast_to_1d_nparray(value, 'idxsphi') @Jsphi.setter def Jsphi(self, Jsphi): - if isinstance(Jsphi, np.ndarray): - self.__idxsphi = J_to_idx_slack(Jsphi) - else: - raise TypeError('Invalid Jsphi value, expected numpy array.') + Jsphi = cast_to_2d_nparray(Jsphi, "Jsphi") + self.__idxsphi = J_to_idx_slack(Jsphi) # soft bounds on general linear constraints at shooting node N @lsg_e.setter def lsg_e(self, value): - value = check_if_nparray_and_flatten(value, 'lsg_e') - self.__lsg_e = value + self.__lsg_e = cast_to_1d_nparray(value, 'lsg_e') @usg_e.setter def usg_e(self, value): - value = check_if_nparray_and_flatten(value, 'usg_e') - self.__usg_e = value + self.__usg_e = cast_to_1d_nparray(value, 'usg_e') @idxsg_e.setter def idxsg_e(self, value): - value = check_if_nparray_and_flatten(value, 'idxsg_e') - self.__idxsg_e = value + self.__idxsg_e = cast_to_1d_nparray(value, 'idxsg_e') @Jsg_e.setter def Jsg_e(self, Jsg_e): - if isinstance(Jsg_e, np.ndarray): - self.__idxsg_e = J_to_idx_slack(Jsg_e) - else: - raise TypeError('Invalid Jsg_e value, expected numpy array.') + Jsg_e = cast_to_2d_nparray(Jsg_e, "Jsg_e") + self.__idxsg_e = J_to_idx_slack(Jsg_e) # soft bounds on nonlinear constraints at shooting node N @lsh_e.setter def lsh_e(self, value): - value = check_if_nparray_and_flatten(value, 'lsh_e') - self.__lsh_e = value + self.__lsh_e = cast_to_1d_nparray(value, 'lsh_e') @ush_e.setter def ush_e(self, value): - value = check_if_nparray_and_flatten(value, 'ush_e') - self.__ush_e = value + self.__ush_e = cast_to_1d_nparray(value, 'ush_e') @idxsh_e.setter def idxsh_e(self, value): - value = check_if_nparray_and_flatten(value, 'idxsh_e') - self.__idxsh_e = value + self.__idxsh_e = cast_to_1d_nparray(value, 'idxsh_e') @Jsh_e.setter def Jsh_e(self, Jsh_e): - if isinstance(Jsh_e, np.ndarray): - self.__idxsh_e = J_to_idx_slack(Jsh_e) - else: - raise TypeError('Invalid Jsh_e value, expected numpy array.') + Jsh_e = cast_to_2d_nparray(Jsh_e, "Jsh_e") + self.__idxsh_e = J_to_idx_slack(Jsh_e) # soft bounds on convex-over-nonlinear constraints at shooting node N @lsphi_e.setter def lsphi_e(self, value): - value = check_if_nparray_and_flatten(value, 'lsphi_e') - self.__lsphi_e = value + self.__lsphi_e = cast_to_1d_nparray(value, 'lsphi_e') @usphi_e.setter def usphi_e(self, value): - value = check_if_nparray_and_flatten(value, 'usphi_e') - self.__usphi_e = value + self.__usphi_e = cast_to_1d_nparray(value, 'usphi_e') @idxsphi_e.setter def idxsphi_e(self, value): - value = check_if_nparray_and_flatten(value, 'idxsphi_e') - self.__idxsphi_e = value + self.__idxsphi_e = cast_to_1d_nparray(value, 'idxsphi_e') @Jsphi_e.setter def Jsphi_e(self, Jsphi_e): - if isinstance(Jsphi_e, np.ndarray): - self.__idxsphi_e = J_to_idx_slack(Jsphi_e) - else: - raise ValueError('Invalid Jsphi_e value.') + Jsphi_e = cast_to_2d_nparray(Jsphi_e, "Jsphi_e") + self.__idxsphi_e = J_to_idx_slack(Jsphi_e) # soft constraints at shooting node 0 @lsh_0.setter def lsh_0(self, value): - value = check_if_nparray_and_flatten(value, 'lsh_0') - self.__lsh_0 = value + self.__lsh_0 = cast_to_1d_nparray(value, 'lsh_0') @ush_0.setter def ush_0(self, value): - value = check_if_nparray_and_flatten(value, 'ush_0') - self.__ush_0 = value + self.__ush_0 = cast_to_1d_nparray(value, 'ush_0') @idxsh_0.setter def idxsh_0(self, value): - value = check_if_nparray_and_flatten(value, 'idxsh_0') - self.__idxsh_0 = value + self.__idxsh_0 = cast_to_1d_nparray(value, 'idxsh_0') @Jsh_0.setter def Jsh_0(self, Jsh_0): - if isinstance(Jsh_0, np.ndarray): - self.__idxsh_0 = J_to_idx_slack(Jsh_0) - else: - raise TypeError('Invalid Jsh_0 value, expected numpy array.') + Jsh_0 = cast_to_2d_nparray(Jsh_0, "Jsh_0") + self.__idxsh_0 = J_to_idx_slack(Jsh_0) @lsphi_0.setter def lsphi_0(self, value): - value = check_if_nparray_and_flatten(value, 'lsphi_0') - self.__lsphi_0 = value + self.__lsphi_0 = cast_to_1d_nparray(value, 'lsphi_0') @usphi_0.setter def usphi_0(self, value): - value = check_if_nparray_and_flatten(value, 'usphi_0') - self.__usphi_0 = value + self.__usphi_0 = cast_to_1d_nparray(value, 'usphi_0') @idxsphi_0.setter def idxsphi_0(self, value): - value = check_if_nparray_and_flatten(value, 'idxsphi_0') - self.__idxsphi_0 = value + self.__idxsphi_0 = cast_to_1d_nparray(value, 'idxsphi_0') @Jsphi_0.setter def Jsphi_0(self, Jsphi_0): - if isinstance(Jsphi_0, np.ndarray): - self.__idxsphi_0 = J_to_idx_slack(Jsphi_0) - else: - raise ValueError('Invalid Jsphi_0 value.') + Jsphi_0 = cast_to_2d_nparray(Jsphi_0, "Jsphi_0") + self.__idxsphi_0 = J_to_idx_slack(Jsphi_0) def set(self, attr, value): setattr(self, attr, value) diff --git a/interfaces/acados_template/acados_template/acados_ocp_cost.py b/interfaces/acados_template/acados_template/acados_ocp_cost.py index e4eef65075..0d07c66867 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_cost.py +++ b/interfaces/acados_template/acados_template/acados_ocp_cost.py @@ -30,7 +30,7 @@ # import numpy as np -from .utils import check_if_nparray_and_flatten, check_if_2d_nparray, check_if_2d_nparray_or_casadi_symbolic, check_if_nparray_or_casadi_symbolic_and_flatten +from .utils import cast_to_2d_nparray, cast_to_2d_nparray_or_casadi_symbolic, cast_to_1d_nparray_or_casadi_symbolic, cast_to_1d_nparray class AcadosOcpCost: r""" @@ -63,6 +63,9 @@ class AcadosOcpCost: :math:`m(x, p) = \psi^e (y^e(x,p) - y_\text{ref}^e, p)` """ def __init__(self): + self.__cost_ext_fun_types = ('casadi', 'generic') + self.__cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR', 'AUTO') + # initial stage self.__cost_type_0 = None self.__W_0 = None @@ -169,35 +172,31 @@ def cost_ext_fun_type_0(self): @yref_0.setter def yref_0(self, yref_0): - yref_0 = check_if_nparray_or_casadi_symbolic_and_flatten(yref_0, "yref_0") - self.__yref_0 = yref_0 + self.__yref_0 = cast_to_1d_nparray_or_casadi_symbolic(yref_0, "yref_0") @W_0.setter def W_0(self, W_0): - check_if_2d_nparray_or_casadi_symbolic(W_0, "W_0") - self.__W_0 = W_0 + self.__W_0 = cast_to_2d_nparray_or_casadi_symbolic(W_0, "W_0") @Vx_0.setter def Vx_0(self, Vx_0): - check_if_2d_nparray(Vx_0, "Vx_0") - self.__Vx_0 = Vx_0 + self.__Vx_0 = cast_to_2d_nparray(Vx_0, "Vx_0") @Vu_0.setter def Vu_0(self, Vu_0): - check_if_2d_nparray(Vu_0, "Vu_0") - self.__Vu_0 = Vu_0 + self.__Vu_0 = cast_to_2d_nparray(Vu_0, "Vu_0") @Vz_0.setter def Vz_0(self, Vz_0): - check_if_2d_nparray(Vz_0, "Vz_0") - self.__Vz_0 = Vz_0 + self.__Vz_0 = cast_to_2d_nparray(Vz_0, "Vz_0") @cost_ext_fun_type_0.setter def cost_ext_fun_type_0(self, cost_ext_fun_type_0): - if cost_ext_fun_type_0 in ['casadi', 'generic']: + if cost_ext_fun_type_0 in self.__cost_ext_fun_types: self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 else: - raise TypeError('Invalid cost_ext_fun_type_0 value, expected numpy array.') + raise ValueError('Invalid cost_ext_fun_type_0 value. Possible values are:\n\n' \ + + ',\n'.join(self.__cost_ext_fun_types) + '.\n\nYou have: ' + cost_ext_fun_type_0 + '.\n\n') # Lagrange term @property @@ -282,80 +281,62 @@ def cost_ext_fun_type(self): @cost_type.setter def cost_type(self, cost_type): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR', 'AUTO') - if cost_type in cost_types: + if cost_type in self.__cost_types: self.__cost_type = cost_type else: raise ValueError('Invalid cost_type value.') @cost_type_0.setter def cost_type_0(self, cost_type_0): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR', 'AUTO') - if cost_type_0 in cost_types: + if cost_type_0 in self.__cost_types: self.__cost_type_0 = cost_type_0 else: raise ValueError('Invalid cost_type_0 value.') @W.setter def W(self, W): - check_if_2d_nparray_or_casadi_symbolic(W, "W") - self.__W = W + self.__W = cast_to_2d_nparray_or_casadi_symbolic(W, "W") @Vx.setter def Vx(self, Vx): - check_if_2d_nparray(Vx, "Vx") - self.__Vx = Vx + self.__Vx = cast_to_2d_nparray(Vx, "Vx") @Vu.setter def Vu(self, Vu): - check_if_2d_nparray(Vu, "Vu") - self.__Vu = Vu + self.__Vu = cast_to_2d_nparray(Vu, "Vu") @Vz.setter def Vz(self, Vz): - check_if_2d_nparray(Vz, "Vz") - self.__Vz = Vz + self.__Vz = cast_to_2d_nparray(Vz, "Vz") @yref.setter def yref(self, yref): - yref = check_if_nparray_or_casadi_symbolic_and_flatten(yref, "yref") - self.__yref = yref + self.__yref = cast_to_1d_nparray_or_casadi_symbolic(yref, "yref") @Zl.setter def Zl(self, Zl): - if isinstance(Zl, np.ndarray): - self.__Zl = Zl - else: - raise TypeError('Invalid Zl value, expected numpy array.') + self.__Zl = cast_to_1d_nparray(Zl, "Zl") @Zu.setter def Zu(self, Zu): - if isinstance(Zu, np.ndarray): - self.__Zu = Zu - else: - raise TypeError('Invalid Zu value, expected numpy array.') + self.__Zu = cast_to_1d_nparray(Zu, "Zu") @zl.setter def zl(self, zl): - if isinstance(zl, np.ndarray): - self.__zl = zl - else: - raise TypeError('Invalid zl value, expected numpy array.') + self.__zl = cast_to_1d_nparray(zl, "zl") @zu.setter def zu(self, zu): - if isinstance(zu, np.ndarray): - self.__zu = zu - else: - raise TypeError('Invalid zu value, expected numpy array.') + self.__zu = cast_to_1d_nparray(zu, "zu") @cost_ext_fun_type.setter def cost_ext_fun_type(self, cost_ext_fun_type): - if cost_ext_fun_type in ['casadi', 'generic']: + if cost_ext_fun_type in self.__cost_ext_fun_types: self.__cost_ext_fun_type = cost_ext_fun_type else: - raise ValueError("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].") + raise ValueError('Invalid cost_ext_fun_type value. Possible values are:\n\n' \ + + ',\n'.join(self.__cost_ext_fun_types) + '.\n\nYou have: ' + cost_ext_fun_type + '.\n\n') # Mayer term @property @@ -455,73 +436,62 @@ def cost_ext_fun_type_e(self): @cost_type_e.setter def cost_type_e(self, cost_type_e): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR', 'AUTO') - if cost_type_e in cost_types: + if cost_type_e in self.__cost_types: self.__cost_type_e = cost_type_e else: raise ValueError('Invalid cost_type_e value.') @W_e.setter def W_e(self, W_e): - check_if_2d_nparray_or_casadi_symbolic(W_e, "W_e") - self.__W_e = W_e + self.__W_e = cast_to_2d_nparray_or_casadi_symbolic(W_e, "W_e") @Vx_e.setter def Vx_e(self, Vx_e): - check_if_2d_nparray(Vx_e, "Vx_e") - self.__Vx_e = Vx_e + self.__Vx_e = cast_to_2d_nparray(Vx_e, "Vx_e") @yref_e.setter def yref_e(self, yref_e): - yref_e = check_if_nparray_or_casadi_symbolic_and_flatten(yref_e, "yref_e") - self.__yref_e = yref_e + self.__yref_e = cast_to_1d_nparray_or_casadi_symbolic(yref_e, "yref_e") @Zl_e.setter def Zl_e(self, Zl_e): - Zl_e = check_if_nparray_and_flatten(Zl_e, "Zl_e") - self.__Zl_e = Zl_e + self.__Zl_e = cast_to_1d_nparray(Zl_e, "Zl_e") @Zu_e.setter def Zu_e(self, Zu_e): - Zu_e = check_if_nparray_and_flatten(Zu_e, "Zu_e") - self.__Zu_e = Zu_e + self.__Zu_e = cast_to_1d_nparray(Zu_e, "Zu_e") @zl_e.setter def zl_e(self, zl_e): - zl_e = check_if_nparray_and_flatten(zl_e, "zl_e") - self.__zl_e = zl_e + self.__zl_e = cast_to_1d_nparray(zl_e, "zl_e") @zu_e.setter def zu_e(self, zu_e): - zu_e = check_if_nparray_and_flatten(zu_e, "zu_e") - self.__zu_e = zu_e + self.__zu_e = cast_to_1d_nparray(zu_e, "zu_e") @Zl_0.setter def Zl_0(self, Zl_0): - Zl_0 = check_if_nparray_and_flatten(Zl_0, "Zl_0") - self.__Zl_0 = Zl_0 + self.__Zl_0 = cast_to_1d_nparray(Zl_0, "Zl_0") @Zu_0.setter def Zu_0(self, Zu_0): - Zu_0 = check_if_nparray_and_flatten(Zu_0, "Zu_0") - self.__Zu_0 = Zu_0 + self.__Zu_0 = cast_to_1d_nparray(Zu_0, "Zu_0") @zl_0.setter def zl_0(self, zl_0): - zl_0 = check_if_nparray_and_flatten(zl_0, "zl_0") - self.__zl_0 = zl_0 + self.__zl_0 = cast_to_1d_nparray(zl_0, "zl_0") @zu_0.setter def zu_0(self, zu_0): - zu_0 = check_if_nparray_and_flatten(zu_0, "zu_0") - self.__zu_0 = zu_0 + self.__zu_0 = cast_to_1d_nparray(zu_0, "zu_0") @cost_ext_fun_type_e.setter def cost_ext_fun_type_e(self, cost_ext_fun_type_e): - if cost_ext_fun_type_e in ['casadi', 'generic']: + if cost_ext_fun_type_e in self.__cost_ext_fun_type: self.__cost_ext_fun_type_e = cost_ext_fun_type_e else: - raise ValueError("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].") + raise ValueError('Invalid cost_ext_fun_type value. Possible values are:\n\n' \ + + ',\n'.join(self.__cost_ext_fun_types) + '.\n\nYou have: ' + cost_ext_fun_type_e + '.\n\n') def set(self, attr, value): setattr(self, attr, value) diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index 9a0e94029c..ffe77a392b 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -400,10 +400,7 @@ def get_default_simulink_opts() -> dict: def J_to_idx(J): - if not isinstance(J, np.ndarray): - raise TypeError('J_to_idx: J must be a numpy array.') - if J.ndim != 2: - raise ValueError('J_to_idx: J must be a 2D numpy array.') + J = cast_to_2d_nparray(J, 'J') nrows = J.shape[0] idx = np.zeros((nrows, )) for i in range(nrows): @@ -418,6 +415,7 @@ def J_to_idx(J): def J_to_idx_slack(J): + J = cast_to_2d_nparray(J, 'J') nrows = J.shape[0] ncol = J.shape[1] idx = np.zeros((ncol, )) @@ -434,7 +432,7 @@ def J_to_idx_slack(J): raise ValueError('J_to_idx_slack: J matrices can only contain 1s, ' \ 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) ) if not i_idx == ncol: - raise ValueError('J_to_idx_slack: J must contain a 1 in every column!') + raise ValueError('J_to_idx_slack: J must contain a 1 in every column!') return idx @@ -475,6 +473,49 @@ def check_if_2d_nparray_or_casadi_symbolic(val, name) -> None: raise Exception(f"{name} must be a 2D array of type np.ndarray, casadi.SX, or casadi.MX, got shape {val.shape}") +def cast_to_1d_nparray(val, name) -> np.ndarray: + try: + val = np.asarray(val) + except: + raise TypeError(f"Failed to cast {name} to np.array, expected array-like type got {type(val)}.") + + val = np.atleast_1d(np.squeeze(val)) + + if val.ndim > 1: + raise ValueError(f"Expected vector-like array, got {val.shape}.") + + return val + + +def cast_to_1d_nparray_or_casadi_symbolic(val, name) -> np.ndarray: + if isinstance(val, (SX, MX, DM)): + if val.shape[0] == 1 or val.shape[1] == 1: + return val + else: + raise ValueError("Expected vector, got {val.shape}.") + else: + return cast_to_1d_nparray(val, name) + + +def cast_to_2d_nparray(val, name) -> np.ndarray: + try: + val = np.asarray(val) + except: + raise TypeError(f"Failed to cast {name} to np.array, expected array-like type got {type(val)}.") + + if val.ndim != 2: + raise ValueError(f"Expected two dimensional array, got {val.shape}.") + + return val + + +def cast_to_2d_nparray_or_casadi_symbolic(val, name) -> np.ndarray: + if isinstance(val, (SX, MX, DM)): + return val + else: + return cast_to_2d_nparray(val, name) + + def print_J_to_idx_note(): print("NOTE: J* matrix is converted to zero based vector idx* vector, which is returned here.") From b4df1fe24a2daeb40adde20fe6c6903e2cc54f35 Mon Sep 17 00:00:00 2001 From: Josua Christoph Lindemann <107478604+ArgoJ@users.noreply.github.com> Date: Tue, 23 Sep 2025 11:40:24 +0200 Subject: [PATCH 142/164] Acados Ros mapper for OCP to SIM messages (#1632) This is an addition to the previous ros pull request. It includes some changes regarding multithreading, a generated dir to specify the package directory, while I also added a `json_file` field to `AcadosSim`. This PR also includes some formating of the ros things. However, the main part is the **ros message mapper* for all sorts of messages. This map can automatically be created for the ocp and sim, by just passing the instances to it like so: ```python from acados_template import AcadosOcp, AcadosSim, AcadosOcpSolver, AcadosSimSolver from acados_template.ros2 import RosTopicMapper, AcadosOcpRosOptions, AcadosSimRosOptions ocp = AcadosOcp() ... ocp.ros_opts = AcadosOcpRosOptions() ocp.ros_opts.package_name = "pendulum_on_cart_ocp" ocp_solver = AcadosOcpSolver(ocp) sim = AcadosSim() ... sim.ros_opts = AcadosSimRosOptions() sim.ros_opts.package_name = "pendulum_on_cart_sim" sim_solver = AcadosSimSolver(sim) # The ros_opts for ocp and sim have to be set for the mapper ros_mapper = RosTopicMapper.from_instances(ocp_solver, sim_solver) ros_mapper.package_name = "sim_ocp_mapper" ros_mapper.generate() ``` This generates all the relevant packages for ocp, sim, interfaces and mapper. Note: For this functionality, the casadi symetrics that correspond to each other need to be named the same. However, one can also use this mapper to build a direct state message to a standard format like Twist, Pose or whatever, like so: ```python from acados_template import AcadosOcp, AcadosSim, AcadosOcpSolver, AcadosSimSolver from acados_template.ros2 import RosTopicMapper, AcadosOcpRosOptions, AcadosSimRosOptions from acados_template.ros2.default_msgs import GEOMETRY_MSGS_TWIST ocp = AcadosOcp() ... ocp.ros_opts = AcadosOcpRosOptions() ocp.ros_opts.package_name = "ocp" AcadosOcpSolver(ocp) # The ros_opts for ocp and sim have to be set for the mapper # --- SETUP MESSAGES --- # We create all messages here that the mapper should subscribe and publish. # e.g. /pose.position.x -> /ocp_state.x[0] # /ocp_control.u[0] -> /cmd_vel.linear.x in_ocp_ctrl = build_default_control(ocp, direction_out=False) # setup state output messages out_ocp_state = build_default_state(ocp, direction_out=True) out_ocp_state.exec_topic = "/pose" out_ocp_state.mapping = [ (f"pose.position.x", "x[0]"), (f"pose.position.y", "x[1]"), (f"pose.orientation.z", "x[2]") ] # setup twist msg twist = RosTopicMsgOutput.from_msg(GEOMETRY_MSGS_TWIST) twist.topic_name = "/cmd_vel" twist.exec_topic = ocp.ros_opts.control_topic twist.mapping = [ (f"{in_ocp_ctrl.topic_name}.u[0]", "linear.x") ] # setup pose msg pose = GEOMETRY_MSGS_POSE pose.topic_name = "/pose" # --- GENERATE MAPPER --- ros_mapper = RosTopicMapper() ros_mapper.package_name = "ocp_mapper" ros_mapper.generated_code_dir = export_dir ros_mapper.in_msgs = [ in_ocp_ctrl, pose ] ros_mapper.out_msgs = [ out_ocp_state, twist ] ros_mapper.generate() ``` ### Breaking: removed keyword argument `json_file` from `AcadosSim.dump_to_json()` and instead use the property `json_file` of `AcadosSim`. --------- Co-authored-by: Jonathan Frey --- .github/workflows/full_build.yml | 13 +- .../ros2/example_ros_mapper.py | 104 +++ .../ros2/example_ros_mapper_ocp_sim.py | 71 ++ .../ros2/example_ros_minimal_ocp.py | 31 +- .../ros2/example_ros_minimal_sim.py | 26 +- .../acados_template/__init__.py | 3 - .../acados_template/acados_ocp.py | 22 +- .../acados_template/acados_sim.py | 50 +- .../acados_template/acados_sim_solver.py | 5 +- .../acados_template/ros2/__init__.py | 3 +- .../acados_template/ros2/default_msgs.py | 97 +++ .../acados_template/ros2/mapping_node.py | 690 ++++++++++++++++++ .../acados_template/ros2/ocp_node.py | 37 +- .../acados_template/ros2/sim_node.py | 5 +- .../acados_template/ros2/utils.py | 87 +-- .../ocp_node_templates/CMakeLists.in.txt | 1 - .../ocp_node_templates/config.in.hpp | 2 + .../ocp_node_templates/node.in.cpp | 342 ++++++--- .../ocp_node_templates/node.in.h | 39 +- .../ocp_node_templates/test.launch.in.py | 42 +- .../ros_mapper_templates/CMakeLists.in.txt | 58 ++ .../ros_mapper_templates/README.in.md | 25 + .../ros_mapper_templates/node.in.cpp | 92 +++ .../ros_mapper_templates/node.in.h | 67 ++ .../ros_mapper_templates/package.in.xml | 24 + .../ros_mapper_templates/test.launch.in.py | 128 ++++ .../ros_mapper_templates/utils.in.hpp | 30 + .../sim_interface_templates/CMakeLists.in.txt | 8 +- .../ControlInput.in.msg | 2 +- .../sim_interface_templates/State.in.msg | 2 +- .../sim_node_templates/README.in.md | 4 +- .../sim_node_templates/node.in.cpp | 4 +- .../sim_node_templates/package.in.xml | 2 +- .../sim_node_templates/test.launch.in.py | 14 +- 34 files changed, 1853 insertions(+), 277 deletions(-) create mode 100644 examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper.py create mode 100644 examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper_ocp_sim.py create mode 100644 interfaces/acados_template/acados_template/ros2/default_msgs.py create mode 100644 interfaces/acados_template/acados_template/ros2/mapping_node.py create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/CMakeLists.in.txt create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/README.in.md create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.cpp create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.h create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/package.in.xml create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/test.launch.in.py create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/utils.in.hpp diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 2bbb8012b5..0dc268683b 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -525,4 +525,15 @@ jobs: with: python-file-path: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py source-file-paths: | - ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/pendulum_model.py \ No newline at end of file + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/pendulum_model.py + + - name: Test ROS2 Mapper Package + uses: ./.github/actions/test-ros2-package + with: + python-file-path: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py + source-file-paths: | + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/pendulum_model.py + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/common/utils.py + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py + ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py + \ No newline at end of file diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper.py new file mode 100644 index 0000000000..7ceadb1671 --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper.py @@ -0,0 +1,104 @@ +# +# 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.; +# + +import sys +import os +script_dir = os.path.dirname(os.path.realpath(__file__)) +common_path = os.path.join(script_dir, '..', 'common') +sys.path.insert(0, os.path.abspath(common_path)) + +from acados_template import AcadosOcpSolver +from acados_template.ros2 import RosTopicMapper, RosTopicMsgOutput, build_default_control, build_default_state +from acados_template.ros2.default_msgs import GEOMETRY_MSGS_TWIST, GEOMETRY_MSGS_POSE + +from example_ros_minimal_ocp import create_minimal_ocp + + +def main(): + Fmax = 80 + Tf_ocp = 1.0 + Tf_sim = 0.05 + N = 20 + export_dir = os.path.join(script_dir, 'generated') + c_generated_code_base = os.path.join(export_dir, "c_generated_code") + + ocp = create_minimal_ocp(export_dir, N, Tf_ocp, Fmax) + ocp.code_export_directory = c_generated_code_base + "_ocp" + + + # --- SETUP MESSAGES --- + # We create all messages here that the mapper should subscribe and publish. + # e.g. /pose.position.x -> /ocp_state.x[0] + # /ocp_control.u[0] -> /cmd_vel.linear.x + + # setup control input messages + in_ocp_ctrl = build_default_control(ocp, direction_out=False) + + # setup state output messages + out_ocp_state = build_default_state(ocp, direction_out=True) + out_ocp_state.exec_topic = "/pose" + out_ocp_state.mapping = [ + (f"pose.position.x", "x[0]"), + (f"pose.position.y", "x[1]"), + (f"pose.orientation.z", "x[2]") + ] + + # setup twist msg + twist = RosTopicMsgOutput.from_msg(GEOMETRY_MSGS_TWIST) + twist.topic_name = "/cmd_vel" + twist.exec_topic = ocp.ros_opts.control_topic + twist.mapping = [ + (f"{in_ocp_ctrl.topic_name}.u[0]", "linear.x") + ] + + # setup pose msg + pose = GEOMETRY_MSGS_POSE + pose.topic_name = "/pose" + + + # --- GENERATE MAPPER --- + ros_mapper = RosTopicMapper() + ros_mapper.package_name = "ocp_mapper" + ros_mapper.generated_code_dir = export_dir + ros_mapper.in_msgs = [ + in_ocp_ctrl, + pose + ] + ros_mapper.out_msgs = [ + out_ocp_state, + twist + ] + + AcadosOcpSolver(ocp, json_file = str(os.path.join(export_dir, 'acados_ocp.json'))) + ros_mapper.generate() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper_ocp_sim.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper_ocp_sim.py new file mode 100644 index 0000000000..6b970091cd --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_mapper_ocp_sim.py @@ -0,0 +1,71 @@ +# +# 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.; +# + +import sys +import os +script_dir = os.path.dirname(os.path.realpath(__file__)) +common_path = os.path.join(script_dir, '..', 'common') +sys.path.insert(0, os.path.abspath(common_path)) + +from pprint import pprint + +from acados_template import AcadosOcpSolver, AcadosSimSolver +from acados_template.ros2 import RosTopicMapper + +from example_ros_minimal_ocp import create_minimal_ocp +from example_ros_minimal_sim import create_minimal_sim + + + +def main(): + Fmax = 80 + Tf_ocp = 1.0 + Tf_sim = 0.05 + N = 20 + export_dir = os.path.join(script_dir, 'generated') + c_generated_code_base = os.path.join(export_dir, "c_generated_code") + + ocp = create_minimal_ocp(export_dir, N, Tf_ocp, Fmax) + ocp.code_export_directory = c_generated_code_base + "_ocp" + ocp_solver = AcadosOcpSolver(ocp, json_file = str(os.path.join(export_dir, 'acados_ocp.json'))) + + sim = create_minimal_sim(export_dir, Tf_sim) + sim.code_export_directory = c_generated_code_base + "_sim" + sim_solver = AcadosSimSolver(sim, json_file = str(os.path.join(export_dir, 'acados_sim.json'))) + + ros_mapper = RosTopicMapper.from_instances(ocp_solver, sim_solver) + ros_mapper.package_name = "sim_ocp_mapper" + ros_mapper.generated_code_dir = export_dir + + ros_mapper.generate() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py index 31e6acbc0e..dfd7c1e20d 100644 --- a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py @@ -31,7 +31,8 @@ import numpy as np import scipy.linalg -from acados_template import AcadosOcp, AcadosOcpSolver, AcadosOcpRosOptions +from acados_template import AcadosOcp, AcadosOcpSolver +from acados_template.ros2 import AcadosOcpRosOptions import sys import os @@ -41,20 +42,18 @@ from pendulum_model import export_pendulum_ode_model from utils import plot_pendulum -def main(): +def create_minimal_ocp(export_dir: str, N: int = 20, Tf: float = 1.0, Fmax: float = 80): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model - - Tf = 1.0 + nx = model.x.rows() nu = model.u.rows() ny = nx + nu ny_e = nx - N = 20 # set dimensions ocp.solver_options.N_horizon = N @@ -84,7 +83,6 @@ def main(): # set constraints # bound on u - Fmax = 80 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) @@ -105,13 +103,24 @@ def main(): # Ros stuff ocp.ros_opts = AcadosOcpRosOptions() ocp.ros_opts.package_name = "pendulum_on_cart_ocp" + ocp.ros_opts.generated_code_dir = export_dir + + ocp.code_export_directory = str(os.path.join(export_dir, "c_generated_code")) + return ocp - export_code = os.path.join(script_dir, 'generated_ocp') - ocp.code_export_directory = str(os.path.join(export_code, "c_generated_code")) - ocp_solver = AcadosOcpSolver(ocp, json_file = str(os.path.join(export_code, 'acados_ocp.json'))) - simX = np.zeros((N+1, nx)) - simU = np.zeros((N, nu)) + +def main(): + Fmax = 80 + Tf = 1.0 + N = 20 + + export_dir = os.path.join(script_dir, 'generated_ocp') + ocp = create_minimal_ocp(export_dir, N, Tf, Fmax) + ocp_solver = AcadosOcpSolver(ocp, json_file = str(os.path.join(export_dir, 'acados_ocp.json'))) + + simX = np.zeros((N+1, ocp.dims.nx)) + simU = np.zeros((N, ocp.dims.nu)) # call SQP_RTI solver in the loop: tol = 1e-6 diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py index 774a4ca45b..abd874f035 100644 --- a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_sim.py @@ -1,5 +1,6 @@ import numpy as np -from acados_template import AcadosSim, AcadosSimSolver, AcadosSimRosOptions +from acados_template import AcadosSim, AcadosSimSolver +from acados_template.ros2 import AcadosSimRosOptions import sys import os @@ -9,14 +10,10 @@ from pendulum_model import export_pendulum_ode_model -def main(): +def create_minimal_sim(export_dir: str, Tf: float = 0.1): sim = AcadosSim() sim.model = export_pendulum_ode_model() - Tf = 0.1 - nx = sim.model.x.rows() - N = 200 - # set simulation time sim.solver_options.T = Tf # set options @@ -28,11 +25,22 @@ def main(): sim.ros_opts = AcadosSimRosOptions() sim.ros_opts.package_name = "pendulum_on_cart_sim" + sim.ros_opts.generated_code_dir = export_dir + + sim.code_export_directory = str( os.path.join(export_dir, "c_generated_code")) + return sim + - export_code = os.path.join(script_dir, 'generated_sim') - sim.code_export_directory = str( os.path.join(export_code, "c_generated_code")) - acados_integrator = AcadosSimSolver(sim, json_file=str(os.path.join(export_code, 'acados_sim.json'))) +def main(): + Tf = 0.1 + N = 200 + + export_dir = os.path.join(script_dir, 'generated_sim') + sim = create_minimal_sim(export_dir, Tf) + acados_integrator = AcadosSimSolver(sim, json_file=str(os.path.join(export_dir, 'acados_sim.json'))) + nx = sim.model.x.rows() + x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) u0 = np.array([0.0]) diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index 9290efdb3e..19f05d64d4 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -42,9 +42,6 @@ from .acados_sim import AcadosSim, AcadosSimOptions from .acados_multiphase_ocp import AcadosMultiphaseOcp -from .ros2.ocp_node import AcadosOcpRosOptions -from .ros2.sim_node import AcadosSimRosOptions - from .acados_ocp_solver import AcadosOcpSolver from .acados_casadi_ocp_solver import AcadosCasadiOcpSolver, AcadosCasadiOcp from .acados_sim_solver import AcadosSimSolver diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index ceae2040ea..4e67807366 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -162,15 +162,15 @@ def json_file(self): """Name of the json file where the problem description is stored.""" return self.__json_file + @json_file.setter + def json_file(self, json_file): + self.__json_file = json_file + @property def ros_opts(self) -> Optional[AcadosOcpRosOptions]: """Options to configure ROS 2 nodes and topics.""" return self.__ros_opts - @json_file.setter - def json_file(self, json_file): - self.__json_file = json_file - @ros_opts.setter def ros_opts(self, ros_opts: AcadosOcpRosOptions): if not isinstance(ros_opts, AcadosOcpRosOptions): @@ -1301,7 +1301,7 @@ def _get_ros_template_list(self) -> list: # --- Interface Package --- ros_interface_dir = os.path.join('ocp_interface_templates') - interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') + interface_dir = os.path.join(self.ros_opts.generated_code_dir, f'{self.ros_opts.package_name}_interface') template_file = os.path.join(ros_interface_dir, 'README.in.md') template_list.append((template_file, 'README.md', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') @@ -1320,15 +1320,9 @@ def _get_ros_template_list(self) -> list: template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') template_list.append((template_file, 'ControlInput.msg', msg_dir, ros_template_glob)) - # Services - # TODO: No node implementation yet - - # Actions - # TODO: No Template yet and no node implementation - # --- Solver Package --- ros_pkg_dir = os.path.join('ocp_node_templates') - package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) + package_dir = os.path.join(self.ros_opts.generated_code_dir, self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'README.in.md') template_list.append((template_file, 'README.md', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') @@ -1457,6 +1451,10 @@ def render_templates(self, cmake_builder=None): def dump_to_json(self) -> None: + dir_name = os.path.dirname(self.json_file) + if dir_name: + os.makedirs(dir_name, exist_ok=True) + with open(self.json_file, 'w') as f: json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) return diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 16519bc690..868f1969ea 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -354,19 +354,15 @@ def __init__(self, acados_path=''): self.__parameter_values = np.array([]) self.__problem_class = 'SIM' - + self.__json_file = "acados_sim.json" + self.__ros_opts: Optional[AcadosSimRosOptions] = None @property def parameter_values(self): """:math:`p` - initial values for parameter - can be updated""" return self.__parameter_values - - @property - def ros_opts(self) -> Optional[AcadosSimRosOptions]: - """Options to configure ROS 2 nodes and topics.""" - return self.__ros_opts - + @parameter_values.setter def parameter_values(self, parameter_values): if isinstance(parameter_values, np.ndarray): @@ -374,6 +370,20 @@ def parameter_values(self, parameter_values): else: raise ValueError('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') + + @property + def json_file(self): + """Name of the json file where the problem description is stored.""" + return self.__json_file + + @json_file.setter + def json_file(self, json_file): + self.__json_file = json_file + + @property + def ros_opts(self) -> Optional[AcadosSimRosOptions]: + """Options to configure ROS 2 nodes and topics.""" + return self.__ros_opts @ros_opts.setter def ros_opts(self, ros_opts: AcadosSimRosOptions): @@ -409,8 +419,12 @@ def to_dict(self) -> dict: return format_class_dict(sim_dict) - def dump_to_json(self, json_file='acados_sim.json') -> None: - with open(json_file, 'w') as f: + def dump_to_json(self) -> None: + dir_name = os.path.dirname(self.json_file) + if dir_name: + os.makedirs(dir_name, exist_ok=True) + + with open(self.json_file, 'w') as f: json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) @@ -419,9 +433,9 @@ def _get_ros_template_list(self) -> list: acados_template_path = os.path.dirname(os.path.abspath(__file__)) ros_template_glob = os.path.join(acados_template_path, 'ros2_templates', '**', '*') - # --- Interface Package --- + # --- Interface Package --- ros_interface_dir = os.path.join('sim_interface_templates') - interface_dir = os.path.join(os.path.dirname(self.code_export_directory), f'{self.ros_opts.package_name}_interface') + interface_dir = os.path.join(self.ros_opts.generated_code_dir, f'{self.ros_opts.package_name}_interface') template_file = os.path.join(ros_interface_dir, 'README.in.md') template_list.append((template_file, 'README.md', interface_dir, ros_template_glob)) template_file = os.path.join(ros_interface_dir, 'CMakeLists.in.txt') @@ -436,15 +450,9 @@ def _get_ros_template_list(self) -> list: template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') template_list.append((template_file, 'ControlInput.msg', msg_dir, ros_template_glob)) - # Services - # TODO: No node implementation yet - - # Actions - # TODO: No Template yet and no node implementation - - # --- Simulator Package --- + # --- Simulator Package --- ros_pkg_dir = os.path.join('sim_node_templates') - package_dir = os.path.join(os.path.dirname(self.code_export_directory), self.ros_opts.package_name) + package_dir = os.path.join(self.ros_opts.generated_code_dir, self.ros_opts.package_name) template_file = os.path.join(ros_pkg_dir, 'README.in.md') template_list.append((template_file, 'README.md', package_dir, ros_template_glob)) template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') @@ -492,9 +500,9 @@ def _get_simulink_template_list(self, name: str) -> list: return template_list - def render_templates(self, json_file, cmake_options: CMakeBuilder = None): + def render_templates(self, cmake_options: CMakeBuilder = None): # setting up loader and environment - json_path = os.path.join(os.getcwd(), json_file) + json_path = os.path.abspath(self.json_file) name = self.model.name if not os.path.exists(json_path): diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index 2467e02dd8..4baa65a231 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -91,6 +91,7 @@ def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_bui """ acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory) + acados_sim.json_file = json_file acados_sim.make_consistent() # module dependent post processing @@ -104,8 +105,8 @@ def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_bui # generate code for external functions acados_sim.generate_external_functions() - acados_sim.dump_to_json(json_file) - acados_sim.render_templates(json_file, cmake_builder) + acados_sim.dump_to_json() + acados_sim.render_templates(cmake_builder) @classmethod diff --git a/interfaces/acados_template/acados_template/ros2/__init__.py b/interfaces/acados_template/acados_template/ros2/__init__.py index ebd7957f2d..f5b8de74a0 100644 --- a/interfaces/acados_template/acados_template/ros2/__init__.py +++ b/interfaces/acados_template/acados_template/ros2/__init__.py @@ -1,3 +1,4 @@ from .ocp_node import AcadosOcpRosOptions from .sim_node import AcadosSimRosOptions -from .utils import ArchType, ControlLoopExec \ No newline at end of file +from .mapping_node import RosTopicMapper, RosTopicMsg, RosTopicMsgOutput, RosField, build_default_state, build_default_control +from .utils import ArchType \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/default_msgs.py b/interfaces/acados_template/acados_template/ros2/default_msgs.py new file mode 100644 index 0000000000..678dff8ad8 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/default_msgs.py @@ -0,0 +1,97 @@ +from .mapping_node import RosField, RosTopicMsg + + + +# --- HEADER MESSAGE --- +STD_MSGS_HEADER_FIELD_TREE = [ + RosField(name="stamp", ftype="builtin_interfaces/Time", children=[ + RosField(name="sec", ftype="int32"), + RosField(name="nanosec", ftype="uint32")]), + RosField(name="frame_id", ftype="string")] + +STD_MSGS_HEADER = RosTopicMsg() +STD_MSGS_HEADER.msg_type = "std_msgs/Header" +STD_MSGS_HEADER.field_tree = STD_MSGS_HEADER_FIELD_TREE + + +# --- Vector3 MESSAGE --- +GEOMETRY_MSGS_VECTOR3_FIELD_TREE = [ + RosField(name="x", ftype="float64"), + RosField(name="y", ftype="float64"), + RosField(name="z", ftype="float64")] + +GEOMETRY_MSGS_VECTOR3 = RosTopicMsg() +GEOMETRY_MSGS_VECTOR3.msg_type = "geometry_msgs/Vector3" +GEOMETRY_MSGS_VECTOR3.field_tree = GEOMETRY_MSGS_VECTOR3_FIELD_TREE + + +# --- POINT MESSAGE --- +GEOMETRY_MSGS_POINT_FIELD_TREE = [ + RosField(name="x", ftype="float64"), + RosField(name="y", ftype="float64"), + RosField(name="z", ftype="float64")] + +GEOMETRY_MSGS_POINT = RosTopicMsg() +GEOMETRY_MSGS_POINT.msg_type = "geometry_msgs/Point" +GEOMETRY_MSGS_POINT.field_tree = GEOMETRY_MSGS_POINT_FIELD_TREE + + +# --- QUATERNION MESSAGE --- +GEOMETRY_MSGS_QUATERNION_FIELD_TREE = [ + RosField(name="x", ftype="float64"), + RosField(name="y", ftype="float64"), + RosField(name="z", ftype="float64"), + RosField(name="w", ftype="float64")] + +GEOMETRY_MSGS_QUATERNION = RosTopicMsg() +GEOMETRY_MSGS_QUATERNION.msg_type = "geometry_msgs/Quaternion" +GEOMETRY_MSGS_QUATERNION.field_tree = GEOMETRY_MSGS_QUATERNION_FIELD_TREE + + +# --- INERTIA MESSAGE --- +GEOMETRY_MSGS_INERTIA_FIELD_TREE = [ + RosField(name="m", ftype="float64"), + RosField(name="com", ftype="geometry_msgs/Vector3", children=GEOMETRY_MSGS_VECTOR3_FIELD_TREE), + RosField(name="ixx", ftype="float64"), + RosField(name="ixy", ftype="float64"), + RosField(name="ixz", ftype="float64"), + RosField(name="iyy", ftype="float64"), + RosField(name="iyz", ftype="float64"), + RosField(name="izz", ftype="float64")] + +GEOMETRY_MSGS_INERTIA = RosTopicMsg() +GEOMETRY_MSGS_INERTIA.msg_type = "geometry_msgs/Inertia" +GEOMETRY_MSGS_INERTIA.field_tree = GEOMETRY_MSGS_INERTIA_FIELD_TREE + + +# --- TWIST MESSAGE --- +GEOMETRY_MSGS_TWIST_FIELD_TREE = [ + RosField(name="linear", ftype="geometry_msgs/Vector3", children=GEOMETRY_MSGS_VECTOR3_FIELD_TREE), + RosField(name="angular", ftype="geometry_msgs/Vector3", children=GEOMETRY_MSGS_VECTOR3_FIELD_TREE)] + + +GEOMETRY_MSGS_TWIST = RosTopicMsg() +GEOMETRY_MSGS_TWIST.msg_type = "geometry_msgs/Twist" +GEOMETRY_MSGS_TWIST.field_tree = GEOMETRY_MSGS_TWIST_FIELD_TREE + +# --- TWIST STAMPED MESSAGE --- +GEOMETRY_MSGS_TWIST_STAMPED_FIELD_TREE = [ + RosField(name="header", ftype="std_msgs/Header", children=STD_MSGS_HEADER_FIELD_TREE), + RosField(name="twist", ftype="geometry_msgs/Twist", children=GEOMETRY_MSGS_TWIST_FIELD_TREE) +] + + +# --- POSE MESSAGE --- +GEOMETRY_MSGS_POSE_FIELD_TREE = [ + RosField(name="position", ftype="geometry_msgs/Point", children=GEOMETRY_MSGS_POINT_FIELD_TREE), + RosField(name="orientation", ftype="geometry_msgs/Quaternion", children=GEOMETRY_MSGS_QUATERNION_FIELD_TREE)] + +GEOMETRY_MSGS_POSE = RosTopicMsg() +GEOMETRY_MSGS_POSE.msg_type = "geometry_msgs/Pose" +GEOMETRY_MSGS_POSE.field_tree = GEOMETRY_MSGS_POSE_FIELD_TREE + +# --- POSE STAMPED MESSAGE --- +GEOMETRY_MSGS_POSE_STAMPED_FIELD_TREE = [ + RosField(name="header", ftype="std_msgs/Header", children=STD_MSGS_HEADER_FIELD_TREE), + RosField(name="pose", ftype="geometry_msgs/Pose", children=GEOMETRY_MSGS_POSE_FIELD_TREE) +] \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/mapping_node.py b/interfaces/acados_template/acados_template/ros2/mapping_node.py new file mode 100644 index 0000000000..774a4bd18e --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2/mapping_node.py @@ -0,0 +1,690 @@ +# 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. +# + +import json +import os +import re + +from typing import Optional, Union, TYPE_CHECKING, Literal +from itertools import chain +from casadi import SX, MX + +from .utils import ArchType, AcadosRosBaseOptions +from ..utils import make_object_json_dumpable, render_template + +# Avoid circular imports at runtime; only import these for type checking +if TYPE_CHECKING: + from ..acados_ocp import AcadosOcp + from ..acados_sim import AcadosSim + from ..acados_ocp_solver import AcadosOcpSolver + from ..acados_sim_solver import AcadosSimSolver + + + +def _parse_msg_type(msg_type: str) -> tuple[str, str]: + msg_type = msg_type.strip() + if "/" in msg_type: + # "pkg/Type" + pkg, typ = msg_type.split("/", 1) + elif "::" in msg_type: + # "pkg::Type" oder "pkg::msg::Type" + parts = [p for p in msg_type.split("::") if p] + if len(parts) < 2: + raise ValueError(f"Invalid msg_type format: {msg_type}") + pkg, typ = parts[0], parts[-1] + else: + raise ValueError(f"Invalid msg_type format: {msg_type}. Valid types are e.g.: 'std_msgs/Header', 'std_msgs::Header' or 'std_msgs::msg::Header'") + return pkg, typ + + +def _cpp_msg_type(msg_type: str) -> str: + pkg, typ = _parse_msg_type(msg_type) + return f'{pkg}::msg::{typ}' + + + +class RosField: + def __init__( + self, + name: str, + ftype: str, + is_array: bool = False, + array_size: Optional[int] = None, + children: list['RosField'] | None = None + ): + if not isinstance(name, str): + raise TypeError("RosField.name must be str") + if not isinstance(ftype, str): + raise TypeError("RosField.ftype must be str") + if not isinstance(is_array, bool): + raise TypeError("RosField.is_array must be bool") + if array_size is not None and not isinstance(array_size, int): + raise TypeError("RosField.array_size must be int") + if children is None: + children = [] + if not isinstance(children, list) or not all(isinstance(ch, RosField) for ch in children): + raise TypeError("RosField.children must be list[RosField]") + self.name = name + self.ftype = ftype + self.is_array = is_array + self.array_size = array_size + self.children = children + self.cpp_type = "" + self.needs_storage = False + + def to_dict(self) -> dict: + self.cpp_type = self.__get_cpp_type(self.ftype) + return { + "name": self.name, + "cpp_type": self.cpp_type, + "is_array": self.is_array, + "array_size": self.array_size, + "needs_storage": self.needs_storage, + "children": [c.to_dict() for c in self.children], + } + + def flatten(self, field: Optional['RosField'] = None) -> list['RosField']: + new_name = self.name if field is None else f"{field.name}.{self.name}" + field_copy = RosField(new_name, self.ftype, self.is_array, self.array_size, self.children) + + if not self.children: + return [field_copy] + + out: list['RosField'] = [] + for c in self.children: + out.extend(c.flatten(field_copy)) + return out + + @staticmethod + def __get_cpp_type(ftype: str): + match ftype: + case "float64": + return "double" + case "float32": + return "float" + case "int8": + return "int8_t" + case "int16": + return "int16_t" + case "int32": + return "int32_t" + + if "/" in ftype or "::" in ftype: + return _cpp_msg_type(ftype) + + return ftype + + +class RosTopicMsg: + def __init__(self): + self.__topic_name: str = "" + self.__msg_type: str = "" + self.__field_tree: list[RosField] = list() + self._flat_field_tree: list[RosField] = list() + + @property + def topic_name(self) -> str: + return self.__topic_name + + @property + def msg_type(self) -> str: + return self.__msg_type + + @property + def field_tree(self) -> list[RosField]: + return self.__field_tree + + @topic_name.setter + def topic_name(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid topic_name value, expected str.\n') + self.__topic_name = value + + @msg_type.setter + def msg_type(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid msg_type value, expected str.\n') + if not ("/" in value or "::" in value): + raise ValueError('Invalid msg_type format, expected "package/Message" or "package::Message".\n') + self.__msg_type = value + + @field_tree.setter + def field_tree(self, value: list[RosField]): + if not isinstance(value, list) or not all(isinstance(item, RosField) for item in value): + raise TypeError('Invalid field_tree value, expected list of RosField.\n') + self.__field_tree = value + + def flatten_field_tree(self): + self._flat_field_tree = list(chain.from_iterable(f.flatten() for f in self.field_tree)) + + def to_dict(self) -> dict: + return { + "topic_name": self.topic_name, + "msg_type": self.msg_type, + # "field_tree": [field.to_dict() for field in self.field_tree], + "flat_field_tree": [field.to_dict() for field in self._flat_field_tree] + } + + +class RosTopicMsgOutput(RosTopicMsg): + def __init__(self): + super().__init__() + self.__mapping: list[dict] = list() + self.__exec_topic: str = "" + self._needs_publish_lock: bool = False + + @property + def mapping(self) -> list[dict]: + return self.__mapping + + @property + def exec_topic(self) -> str: + return self.__exec_topic + + @mapping.setter + def mapping(self, value: list[tuple[str, str]]): + if not isinstance(value, list) or not all(isinstance(item, tuple) and len(item) == 2 and all(isinstance(i, str) for i in item) for item in value): + raise TypeError('Invalid mapping value, expected list of tuples (str, str).\n') + self.__mapping = [self.__parse_mapping_pair(src, dest) for src, dest in value] + + @exec_topic.setter + def exec_topic(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid exec_topic value, expected str.\n') + self.__exec_topic = value + + def to_dict(self) -> dict: + return super().to_dict() | { + "mapping": self.mapping, + "exec_topic": self.exec_topic, + "needs_publish_lock": self._needs_publish_lock} + + @classmethod + def from_msg( + cls, + base_msg: RosTopicMsg + ) -> 'RosTopicMsgOutput': + """ + Create a RosTopicMsgOutput instance from a base RosTopicMsg template. + """ + output_msg = cls() + output_msg.msg_type = base_msg.msg_type + output_msg.field_tree = base_msg.field_tree + output_msg.topic_name = base_msg.topic_name + return output_msg + + @staticmethod + def __parse_mapping_string(map_str: str) -> dict: + """Parses a string like 'field.name[index]' into a structured dict.""" + match = re.match(r"^(.*?)(?:\[(\d+)\])?$", map_str) + if not match: + raise ValueError(f"Invalid mapping format: '{map_str}'") + + base_name, index_str = match.groups() + index = int(index_str) if index_str is not None else None + + return {"full_name": map_str, "base_name": base_name, "index": index} + + def __parse_mapping_pair(self, source_str: str, dest_str: str) -> dict: + """Parses a source-destination pair into a structured dict.""" + source_parts = self.__parse_mapping_string(source_str) + dest_parts = self.__parse_mapping_string(dest_str) + + # Split source into topic and field + base_split = source_parts['base_name'].split('.', 1) + if len(base_split) != 2: + raise ValueError(f"Source mapping '{source_str}' must be in 'topic_name.field_name' format.") + + return { + "source": { + "full": source_str, + "topic": base_split[0], + "field": base_split[1], + "index": source_parts["index"] + }, + "dest": { + "full": dest_str, + "field": dest_parts["base_name"], + "index": dest_parts["index"] + } + } + + + +class RosTopicMapper(AcadosRosBaseOptions): + def __init__(self): + super().__init__() + self.package_name: str = "ros_mapper" + self.node_name: str = "" + self.namespace: str = "" + self.archtype: str = ArchType.NODE.value + self.__header_includes: set[str] = set() + self.__dependencies: set[str] = set() + + self.__in_msgs: list[RosTopicMsg] = [] + self.__out_msgs: list[RosTopicMsgOutput] = [] + + self.__ocp_json_file: str = "" + self.__sim_json_file: str = "" + self.__mapper_json_file = "ros_mapper.json" + + @property + def in_msgs(self) -> list[RosTopicMsg]: + return self.__in_msgs + + @property + def out_msgs(self) -> list[RosTopicMsgOutput]: + return self.__out_msgs + + @property + def ocp_json_file(self) -> str: + return self.__ocp_json_file + + @property + def sim_json_file(self) -> str: + return self.__sim_json_file + + @property + def mapper_json_file(self) -> str: + return self.__mapper_json_file + + @in_msgs.setter + def in_msgs(self, value: list[RosTopicMsg]): + if not isinstance(value, list) or not all(isinstance(item, RosTopicMsg) for item in value): + raise TypeError('Invalid in_msg value, expected list of RosTopicMsg.\n') + self.__in_msgs = value + + @out_msgs.setter + def out_msgs(self, value: list[RosTopicMsgOutput]): + if not isinstance(value, list ) or not all(isinstance(item, RosTopicMsgOutput) for item in value): + raise TypeError('Invalid out_msg value, expected list of RosTopicMsgOutput.\n') + self.__out_msgs = value + + @ocp_json_file.setter + def ocp_json_file(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid ocp_json_file value, expected str.\n') + self.__ocp_json_file = value + + @sim_json_file.setter + def sim_json_file(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid sim_json_file value, expected str.\n') + self.__sim_json_file = value + + @mapper_json_file.setter + def mapper_json_file(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid mapper_json_file value, expected str.\n') + self.__mapper_json_file = value + + + def check_consistency(self): + in_topic_index = {m.topic_name: m for m in self.in_msgs} + + for out_msg in self.out_msgs: + out_field_index = {f.name: f for f in out_msg._flat_field_tree} + + for mapping in out_msg.mapping: + source = mapping['source'] + dest = mapping['dest'] + + # Check source topic + in_msg = in_topic_index.get(source['topic']) + if in_msg is None: + raise ValueError(f"Source topic '{source['topic']}' in mapping '{source['full']}' not found in input messages.") + + in_field_index = {f.name: f for f in in_msg._flat_field_tree} + + # Check source field + source_field_obj = in_field_index.get(source['field']) + if source_field_obj is None: + raise ValueError(f"Source field '{source['field']}' not found in topic '{source['topic']}'. Available: {list(in_field_index.keys())}") + + # Check source indexing + if source['index'] is not None: + if not source_field_obj.is_array: + raise ValueError(f"Source field '{source['field']}' is not an array, but is being indexed in '{source['full']}'.") + if source_field_obj.array_size > 0 and source['index'] >= source_field_obj.array_size: + raise ValueError(f"Source index in '{source['full']}' is out of bounds. Size is {source_field_obj.array_size}.") + + # Check destination field + dest_field_obj = out_field_index.get(dest['field']) + if dest_field_obj is None: + raise ValueError(f"Destination field '{dest['field']}' not found in output message '{out_msg.topic_name}'. Available: {list(out_field_index.keys())}") + + # Check destination indexing + if dest['index'] is not None: + if not dest_field_obj.is_array: + raise ValueError(f"Destination field '{dest['field']}' is not an array, but is being indexed in '{dest['full']}'.") + if dest_field_obj.array_size > 0 and dest['index'] >= dest_field_obj.array_size: + raise ValueError(f"Destination index in '{dest['full']}' is out of bounds. Size is {dest_field_obj.array_size}.") + + + def finalize(self): + # topic normalize + flatten + for msg in self.in_msgs + self.out_msgs: + if msg.topic_name.startswith("/"): + msg.topic_name = msg.topic_name[1:] + msg.flatten_field_tree() + self.__add_types(msg.msg_type) + msg.msg_type = _cpp_msg_type(msg.msg_type) + + # execution topic normalize + for om in self.out_msgs: + if om.exec_topic.startswith("/"): + om.exec_topic = om.exec_topic[1:] + + self.check_none_values() + self.check_consistency() + + # compute if field needs_storage + for in_msg in self.in_msgs: + for in_field in in_msg._flat_field_tree: + full_field_name = f"{in_msg.topic_name}.{in_field.name}" + for out_msg in self.out_msgs: + is_used = any( + f"{mapping['source']['topic']}.{mapping['source']['field']}" == full_field_name + for mapping in out_msg.mapping + ) + if is_used and out_msg.exec_topic != in_msg.topic_name: + in_field.needs_storage = True + break + + # compute if out msg needs lock + for out_msg in self.out_msgs: + out_msg._needs_publish_lock = any( + m["source"]["topic"] != out_msg.exec_topic + for m in out_msg.mapping + ) + + + def to_dict(self): + return super().to_dict() | { + "in_msgs": [msg.to_dict() for msg in self.in_msgs], + "out_msgs": [msg.to_dict() for msg in self.out_msgs], + "ocp_json_file": self.ocp_json_file, + "sim_json_file": self.sim_json_file, + "mapper_json_file": self.mapper_json_file, + "header_includes": list(self.__header_includes), + "dependencies": list(self.__dependencies), + } + + + def dump_to_json(self) -> None: + dir_name = os.path.dirname(self.mapper_json_file) + if not dir_name: + self.mapper_json_file = os.path.join(self.generated_code_dir, self.mapper_json_file) + + os.makedirs(os.path.dirname(self.mapper_json_file), exist_ok=True) + with open(self.mapper_json_file, 'w') as f: + json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) + + + def render_templates(self): + if not os.path.exists(self.mapper_json_file): + raise FileNotFoundError(f"{self.mapper_json_file} not found!") + + template_list = self._get_ros_template_list() + + # Render templates + for tup in template_list: + output_dir = self.generated_code_dir if len(tup) <= 2 else tup[2] + template_glob = None if len(tup) <= 3 else tup[3] + render_template(tup[0], tup[1], output_dir, self.mapper_json_file, template_glob=template_glob) + + + def check_none_values(self): + non_values: list[str] = [] + + if not self.in_msgs: + non_values.append(f"input") + if not self.out_msgs: + non_values.append(f"output") + + for i, msg in enumerate(self.in_msgs): + if not msg.topic_name: + non_values.append(f"input[{i}].topic_name") + if not msg.msg_type: + non_values.append(f"input[{i}].msg_type") + if not msg._flat_field_tree: + non_values.append(f"input[{i}].flat_field_tree") + + for i, msg in enumerate(self.out_msgs): + if not msg.topic_name: + non_values.append(f"output[{i}].topic_name") + if not msg.msg_type: + non_values.append(f"output[{i}].msg_type") + if not msg._flat_field_tree: + non_values.append(f"output[{i}].flat_field_tree") + if msg.mapping is None or len(msg.mapping) == 0: + non_values.append(f"output[{i}].mapping") + if not msg.exec_topic: + non_values.append(f"output[{i}].exec_topic") + + if non_values: + raise ValueError("The fields 'in_msgs' and 'out_msgs' must be non-empty. Currently missing: " + ", ".join(non_values)) + + + def generate(self): + self.finalize() + self.dump_to_json() + self.render_templates() + + + @classmethod + def from_instances(cls, ocp_solver: 'AcadosOcpSolver', sim_solver: 'AcadosSimSolver'): + """ + Build a :py:class:`RosTopicMapper` by wiring OCP -> SIM controls (``u``) and + SIM -> OCP states (``x``) using the default interface message types. + + The mapper creates two input messages (SIM ``State``, OCP ``ControlInput``) + and two output messages (OCP ``State``, SIM ``ControlInput``). It then derives + field mappings based on symbolic element names of the respective CasADi + vectors and assigns execution topics so that each output is published when + its corresponding input arrives. + + :param ocp_solver: OCP solver instance whose OCP and model define the OCP side. + :type ocp_solver: acados_template.acados_ocp_solver.AcadosOcpSolver + :param sim_solver: Simulator solver instance whose simulator and model define the SIM side. + :type sim_solver: acados_template.acados_sim_solver.AcadosSimSolver + :returns: A configured topic mapper with populated inputs, outputs, mappings, + and references to the OCP/SIM JSON files. + :rtype: RosTopicMapper + + :raises ValueError: If required ROS options (e.g. ``package_name``, topics) are + missing in either solver, or if message field trees are incomplete at finalize-time. + """ + ocp = ocp_solver.acados_ocp + sim = sim_solver.acados_sim + obj = cls() + + # State and Control names + ocp_x_names = _elem_names(ocp.model.x) + sim_x_names = _elem_names(sim.model.x) + ocp_u_names = _elem_names(ocp.model.u) + sim_u_names = _elem_names(sim.model.u) + + # --- Build input messages --- + in_sim_state = build_default_state(sim, direction_out=False) + in_ocp_ctrl = build_default_control(ocp, direction_out=False) + obj.in_msgs = [in_sim_state, in_ocp_ctrl] + + # --- Build output messages --- + out_ocp_state = build_default_state(ocp, direction_out=True) + out_sim_ctrl = build_default_control(sim, direction_out=True) + + # Map SIM x -> OCP x + x_map_pairs = _compute_mapping( + src_topic=in_sim_state.topic_name, + src_name="x", + src_labels=sim_x_names, + dst_name="x", + dst_labels=ocp_x_names, + ) + out_ocp_state.mapping = x_map_pairs + out_ocp_state.exec_topic = in_sim_state.topic_name + + # Map OCP u -> SIM u + u_map_pairs = _compute_mapping( + src_topic=in_ocp_ctrl.topic_name, + src_name="u", + src_labels=ocp_u_names, + dst_name="u", + dst_labels=sim_u_names, + ) + out_sim_ctrl.mapping = u_map_pairs + out_sim_ctrl.exec_topic = in_ocp_ctrl.topic_name + + obj.out_msgs = [out_ocp_state, out_sim_ctrl] + obj.ocp_json_file = ocp.json_file + obj.sim_json_file = sim.json_file + return obj + + + def _get_ros_template_list(self) -> list: + template_list = [] + + acados_template_path = os.path.dirname(os.path.dirname(__file__)) + ros_template_glob = os.path.join(acados_template_path, 'ros2_templates', '**', '*') + + # --- Simulator Package --- + ros_pkg_dir = os.path.join('ros_mapper_templates') + package_dir = os.path.join(self.generated_code_dir, self.package_name) + template_file = os.path.join(ros_pkg_dir, 'README.in.md') + template_list.append((template_file, 'README.md', package_dir, ros_template_glob)) + template_file = os.path.join(ros_pkg_dir, 'CMakeLists.in.txt') + template_list.append((template_file, 'CMakeLists.txt', package_dir, ros_template_glob)) + template_file = os.path.join(ros_pkg_dir, 'package.in.xml') + template_list.append((template_file, 'package.xml', package_dir, ros_template_glob)) + + # # Header + include_dir = os.path.join(package_dir, 'include', self.package_name) + template_file = os.path.join(ros_pkg_dir, 'utils.in.hpp') + template_list.append((template_file, 'utils.hpp', include_dir, ros_template_glob)) + template_file = os.path.join(ros_pkg_dir, 'node.in.h') + template_list.append((template_file, 'node.h', include_dir, ros_template_glob)) + + # Source + src_dir = os.path.join(package_dir, 'src') + template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') + template_list.append((template_file, 'node.cpp', src_dir, ros_template_glob)) + + # Test + test_dir = os.path.join(package_dir, 'test') + template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') + template_list.append((template_file, f'test_{self.package_name}.launch.py', test_dir, ros_template_glob)) + return template_list + + + def __add_types(self, msg_type: str): + pkg, typ = _parse_msg_type(msg_type) + self.__header_includes.add(f"{pkg}/msg/{self.camel_to_snake(typ)}.hpp") + self.__dependencies.add(pkg) + + + +# --- From Instances Helpers --- +def _size(sym: Union[SX, MX]) -> int: + return int(sym.numel()) + + +def _elem_names(sym: Union[SX, MX]) -> list[str]: + n = _size(sym) + return [str(sym[i].name()) for i in range(n)] + + +def _compute_mapping( + src_topic: str, + src_name: str, + src_labels: list[str], + dst_name: str, + dst_labels: list[str] +) -> list[tuple[str, str]]: + """Return mapping pairs [(f"{src_topic}.{src_name}[i]","{dst_name}[j]")].""" + if src_labels == dst_labels: + return [(f"{src_topic}.{src_name}", f"{dst_name}")] + + pairs: list[tuple[str, str]] = [] + + # Label-based match + for j, lbl in enumerate(dst_labels): + i = src_labels.get(lbl.lower()) + if i is not None: + pairs.append((f"{src_topic}.{src_name}[{i}]", f"{dst_name}[{j}]")) + + return pairs + + +def build_default_state( + solver_instance: Union['AcadosOcp', 'AcadosSim'], + direction_out: bool = False +) -> Union[RosTopicMsg, RosTopicMsgOutput]: + """ + Creates a standard ocp- or sim-interface state message of an + in- or outgoing message, dependent on the setting. + """ + if not (hasattr(solver_instance, "ros_opts") and getattr(solver_instance, "ros_opts") is not None): + raise ValueError(f"Field 'ros_opts' is not set in the solver {solver_instance.__class__.__name__}") + + m = RosTopicMsgOutput() if direction_out else RosTopicMsg() + m.topic_name = solver_instance.ros_opts.state_topic + m.msg_type = f"{solver_instance.ros_opts.package_name}_interface/State" + m.field_tree = [ + RosField(name="header", ftype="std_msgs/Header"), + RosField(name="x", ftype="float64", is_array=True, array_size=solver_instance.model.x.rows()), + ] + if direction_out: + m.field_tree.append(RosField(name="status", ftype="int8")) + m.flatten_field_tree() + return m + + +def build_default_control( + solver_instance: Union['AcadosOcp', 'AcadosSim'], + direction_out: bool = False +) -> Union[RosTopicMsg, RosTopicMsgOutput]: + """ + Creates a standard ocp- or sim-interface control message of an + in- or outgoing message, dependent on the setting. + """ + if not (hasattr(solver_instance, "ros_opts") and getattr(solver_instance, "ros_opts") is not None): + raise ValueError(f"Field 'ros_opts' is not set in the solver {solver_instance.__class__.__name__}") + + m = RosTopicMsgOutput() if direction_out else RosTopicMsg() + m.topic_name = solver_instance.ros_opts.control_topic + m.msg_type = f"{solver_instance.ros_opts.package_name}_interface/ControlInput" + m.field_tree = [ + RosField(name="header", ftype="std_msgs/Header"), + RosField(name="u", ftype="float64", is_array=True, array_size=solver_instance.model.u.rows()), + ] + if not direction_out: + m.field_tree.append(RosField(name="status", ftype="int8")) + m.flatten_field_tree() + return m diff --git a/interfaces/acados_template/acados_template/ros2/ocp_node.py b/interfaces/acados_template/acados_template/ros2/ocp_node.py index 863567a678..cf6fc55ddc 100644 --- a/interfaces/acados_template/acados_template/ros2/ocp_node.py +++ b/interfaces/acados_template/acados_template/ros2/ocp_node.py @@ -27,7 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from .utils import ControlLoopExec, ArchType, AcadosRosBaseOptions +from .utils import ArchType, AcadosRosBaseOptions # --- Ros Options --- class AcadosOcpRosOptions(AcadosRosBaseOptions): @@ -37,12 +37,12 @@ def __init__(self): self.node_name: str = "" self.namespace: str = "" self.archtype: str = ArchType.NODE.value - self.control_loop_executor: str = ControlLoopExec.TIMER.value - self.__control_topic = "ocp_control" - self.__state_topic = "ocp_state" - self.__parameters_topic = "ocp_params" - self.__reference_topic = "ocp_reference" + self.__control_topic: str = "ocp_control" + self.__state_topic: str = "ocp_state" + self.__parameters_topic: str = "ocp_params" + self.__reference_topic: str = "ocp_reference" + self.__threads: int = 1 @property def control_topic(self) -> str: @@ -60,6 +60,10 @@ def parameters_topic(self) -> str: def reference_topic(self) -> str: return self.__reference_topic + @property + def threads(self) -> str: + return self.__threads + @control_topic.setter def control_topic(self, value: str): if not isinstance(value, str): @@ -84,22 +88,17 @@ def reference_topic(self, value: str): raise TypeError('Invalid reference_topic value, expected str.\n') self.__reference_topic = value + @threads.setter + def threads(self, value: int): + if not isinstance(value, int): + raise TypeError('Invalid threads value, expected int.\n') + self.__threads = value + def to_dict(self) -> dict: return super().to_dict() | { "control_topic": self.control_topic, "state_topic": self.state_topic, "parameters_topic": self.parameters_topic, "reference_topic": self.reference_topic, - } - - -if __name__ == "__main__": - ros_opt = AcadosOcpRosOptions() - - # ros_opt.node_name = "my_node" - ros_opt.package_name = "that_package" - ros_opt.namespace = "/my_namespace" - # ros_opt.control_loop_executor = ControlLoopExec.TOPIC - # ros_opt.archtype = ArchType.LIFECYCLE_NODE - - print(ros_opt.to_dict()) + "threads": self.threads, + } \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/sim_node.py b/interfaces/acados_template/acados_template/ros2/sim_node.py index 0ac6cc4e0f..dfd0a6b654 100644 --- a/interfaces/acados_template/acados_template/ros2/sim_node.py +++ b/interfaces/acados_template/acados_template/ros2/sim_node.py @@ -27,7 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from .utils import ControlLoopExec, ArchType, AcadosRosBaseOptions +from .utils import ArchType, AcadosRosBaseOptions # --- Ros Options --- @@ -38,7 +38,6 @@ def __init__(self): self.node_name: str = "" self.namespace: str = "" self.archtype: str = ArchType.NODE.value - self.control_loop_executor: str = ControlLoopExec.TIMER.value self.__control_topic = "sim_control" self.__state_topic = "sim_state" @@ -62,7 +61,7 @@ def state_topic(self, value: str): if not isinstance(value, str): raise TypeError('Invalid state_topic value, expected str.\n') self.__state_topic = value - + def to_dict(self) -> dict: return super().to_dict() | { "control_topic": self.control_topic, diff --git a/interfaces/acados_template/acados_template/ros2/utils.py b/interfaces/acados_template/acados_template/ros2/utils.py index ef2c42fdc5..979b3c4977 100644 --- a/interfaces/acados_template/acados_template/ros2/utils.py +++ b/interfaces/acados_template/acados_template/ros2/utils.py @@ -4,12 +4,6 @@ from typing import Union -class ControlLoopExec(str, Enum): - TOPIC = "topic" - TIMER = "timer" - SRV = "srv" - ACTION = "action" - class ArchType(str, Enum): NODE = "node" LIFECYCLE_NODE = "lifecycle_node" @@ -18,12 +12,17 @@ class ArchType(str, Enum): class AcadosRosBaseOptions: + _NOT_IMPLEMENTED_ARCHTYPES: set[ArchType] = { + ArchType.LIFECYCLE_NODE, + ArchType.ROS2_CONTROLLER, + ArchType.NAV2_CONTROLLER} + def __init__(self): self.__package_name: str = "acados_base" self.__node_name: str = "acados_base_node" self.__namespace: str = "" + self.__generated_code_dir: str = "ros_generated_code" self.__archtype: str = ArchType.NODE.value - self.__control_loop_executor: str = ControlLoopExec.TIMER.value @property def package_name(self) -> str: @@ -38,74 +37,54 @@ def namespace(self) -> str: return self.__namespace @property - def archtype(self) -> str: - return self.__archtype + def generated_code_dir(self) -> str: + return self.__generated_code_dir @property - def control_loop_executor(self) -> str: - return self.__control_loop_executor + def archtype(self) -> str: + return self.__archtype @package_name.setter - def package_name(self, package_name: str): - if not isinstance(package_name, str): + def package_name(self, value: str): + if not isinstance(value, str): raise TypeError('Invalid package_name value, expected str.\n') - self.__package_name = package_name + self.__package_name = value @node_name.setter - def node_name(self, node_name: str): - if not isinstance(node_name, str): + def node_name(self, value: str): + if not isinstance(value, str): raise TypeError('Invalid node_name value, expected str.\n') - self.__node_name = node_name + self.__node_name = value + + @generated_code_dir.setter + def generated_code_dir(self, value: str): + if not isinstance(value, str): + raise TypeError('Invalid generated_code_dir value, expected str.\n') + self.__generated_code_dir = value @namespace.setter - def namespace(self, namespace: str): - if not isinstance(namespace, str): + def namespace(self, value: str): + if not isinstance(value, str): raise TypeError('Invalid namespace value, expected str.\n') - self.__namespace = namespace + self.__namespace = value @archtype.setter - def archtype(self, node_archtype: Union[ArchType, str]): + def archtype(self, value: Union[ArchType, str]): try: - if isinstance(node_archtype, ArchType): - archtype_enum = node_archtype - elif isinstance(node_archtype, str): - archtype_enum = ArchType(node_archtype) + if isinstance(value, ArchType): + archtype_enum = value + elif isinstance(value, str): + archtype_enum = ArchType(value) else: raise TypeError() except (ValueError, TypeError): valid_types = [e.value for e in ArchType] raise TypeError(f"Invalid node_archtype. Expected one of {valid_types} or a ArchType enum member.") - not_implemented = [ - ArchType.ROS2_CONTROLLER, - ArchType.NAV2_CONTROLLER, - ArchType.LIFECYCLE_NODE - ] - if archtype_enum in not_implemented: - raise NotImplementedError(f"Archtype '{archtype_enum.value}' is not implemented yet.") + if archtype_enum in type(self)._NOT_IMPLEMENTED_ARCHTYPES: + raise NotImplementedError(f"Archtype '{archtype_enum.value}' is not implemented for {type(self).__name__} yet.") self.__archtype = archtype_enum.value - @control_loop_executor.setter - def control_loop_executor(self, control_loop_executor: Union[ControlLoopExec, str]) -> None: - try: - if isinstance(control_loop_executor, ControlLoopExec): - control_loop_executor_enum = control_loop_executor - elif isinstance(control_loop_executor, str): - control_loop_executor_enum = ControlLoopExec(control_loop_executor) - else: - raise TypeError() - except (ValueError, TypeError): - valid_types = [e.value for e in ControlLoopExec] - raise TypeError(f"Invalid control_loop_executor. Expected one of {valid_types} or a ControlLoopExec enum member.") - - not_implemented = [ - ControlLoopExec.SRV, - ControlLoopExec.ACTION - ] - if control_loop_executor_enum in not_implemented: - raise NotImplementedError(f"Control loop executor '{control_loop_executor_enum.value}' is not implemented yet.") - self.__control_loop_executor = control_loop_executor_enum.value - def to_dict(self) -> dict: if self.node_name == "": self.node_name = self.package_name + "_node" @@ -116,8 +95,8 @@ def to_dict(self) -> dict: "package_name": package_name_snake, "node_name": node_name_snake, "namespace": namespace_snake, + "generated_code_dir": self.generated_code_dir, "archtype": self.archtype, - "control_loop_executor": self.control_loop_executor } @staticmethod diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt index 9df9e2d06e..064a4d6943 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt @@ -31,7 +31,6 @@ add_executable({{ ros_opts.node_name }} # --- TARGET CONFIGURATION --- if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # Dies bindet die Optionen direkt an den Node und ist die moderne Variante target_compile_options({{ ros_opts.node_name }} PRIVATE -Wall -Wextra -Wpedantic) endif() diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp index 44695127a9..411a0ca136 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/config.in.hpp @@ -12,6 +12,8 @@ namespace {{ ros_opts.package_name }} struct {{ ClassName }}Config { double ts{ {{ solver_options.Tsim }} }; + int threads{ {{ ros_opts.threads | default(value=1) }} }; + bool verbose{ false }; }; } // namespace {{ ros_opts.package_name }} diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp index 93b4d36f54..71ead0e863 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp @@ -17,13 +17,19 @@ namespace {{ ros_opts.package_name }} {%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} {%- endif %} {%- set has_slack = dims.ns > 0 or dims.ns_0 > 0 or dims.ns_e > 0 %} +{%- set use_multithreading = ros_opts.threads is defined and ros_opts.threads > 1 %} {{ ClassName }}::{{ ClassName }}() - : Node("{{ ros_opts.node_name }}") + : Node("{{ ros_opts.node_name }}"), control_timer_(nullptr) { RCLCPP_INFO(this->get_logger(), "Initializing {{ ros_opts.node_name | replace(from="_", to=" ") | title }}..."); // --- default values --- config_ = {{ ClassName }}Config(); + {%- if constraints.has_x0 %} + current_x_ = { {{- constraints.lbx_0 | join(sep=', ') -}} }; + {%- else %} + current_x_.fill(0.0); + {%- endif %} {%- if dims.ny_0 > 0 %} current_yref_0_ = { {{- cost.yref_0 | join(sep=', ') -}} }; {%- endif %} @@ -37,6 +43,14 @@ namespace {{ ros_opts.package_name }} current_p_ = { {{- parameter_values | join(sep=', ') -}} }; {%- endif %} + {%- if use_multithreading %} + // --- Multithreading --- + timer_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + services_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto cb_options = rclcpp::SubscriptionOptions(); + cb_options.callback_group = services_group_; + {%- endif %} + // --- Parameters --- this->declare_parameters(); this->setup_parameter_handlers(); @@ -47,14 +61,17 @@ namespace {{ ros_opts.package_name }} // --- Subscriber --- state_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::State>( "{{ state_topic }}", 10, - std::bind(&{{ ClassName }}::state_callback, this, std::placeholders::_1)); + std::bind(&{{ ClassName }}::state_callback, this, std::placeholders::_1) + {%- if use_multithreading -%}, cb_options {%- endif -%}); references_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::References>( "{{ references_topic }}", 10, - std::bind(&{{ ClassName }}::references_callback, this, std::placeholders::_1)); + std::bind(&{{ ClassName }}::references_callback, this, std::placeholders::_1) + {%- if use_multithreading -%}, cb_options {%- endif -%}); {%- if dims.np > 0 %} parameters_sub_ = this->create_subscription<{{ ros_opts.package_name }}_interface::msg::Parameters>( "{{ parameters_topic }}", 10, - std::bind(&{{ ClassName }}::parameters_callback, this, std::placeholders::_1)); + std::bind(&{{ ClassName }}::parameters_callback, this, std::placeholders::_1) + {%- if use_multithreading -%}, cb_options {%- endif -%}); {%- endif %} // --- Publisher --- @@ -68,6 +85,9 @@ namespace {{ ros_opts.package_name }} } {{ ClassName }}::~{{ ClassName }}() { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} RCLCPP_INFO(this->get_logger(), "Shutting down and freeing Acados solver memory."); if (ocp_capsule_) { int status = {{ model.name }}_acados_free(ocp_capsule_); @@ -84,6 +104,9 @@ namespace {{ ros_opts.package_name }} // --- Core Methods --- void {{ ClassName }}::initialize_solver() { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} ocp_capsule_ = {{ model.name }}_acados_create_capsule(); int status = {{ model.name }}_acados_create(ocp_capsule_); if (status) { @@ -91,11 +114,12 @@ void {{ ClassName }}::initialize_solver() { rclcpp::shutdown(); } - ocp_nlp_config_ = {{ model.name }}_acados_get_nlp_config(ocp_capsule_); - ocp_nlp_dims_ = {{ model.name }}_acados_get_nlp_dims(ocp_capsule_); ocp_nlp_in_ = {{ model.name }}_acados_get_nlp_in(ocp_capsule_); ocp_nlp_out_ = {{ model.name }}_acados_get_nlp_out(ocp_capsule_); + ocp_nlp_sens_ = {{ model.name }}_acados_get_sens_out(ocp_capsule_); + ocp_nlp_config_ = {{ model.name }}_acados_get_nlp_config(ocp_capsule_); ocp_nlp_opts_ = {{ model.name }}_acados_get_nlp_opts(ocp_capsule_); + ocp_nlp_dims_ = {{ model.name }}_acados_get_nlp_dims(ocp_capsule_); RCLCPP_INFO(this->get_logger(), "acados solver initialized successfully."); } @@ -117,7 +141,9 @@ void {{ ClassName }}::control_loop() { {%- endif %} { + {%- if use_multithreading %} std::scoped_lock lock(data_mutex_); + {%- endif %} x0 = current_x_; {%- if dims.ny_0 > 0 %} yref0 = current_yref_0_; @@ -132,38 +158,43 @@ void {{ ClassName }}::control_loop() { p = current_p_; {%- endif %} } + + { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); - // Update solver - this->set_x0(x0.data()); + {%- endif %} + // Update solver + this->set_x0(x0.data()); + {%- if dims.ny_0 > 0 %} + this->set_yref0(yref0.data()); + {%- endif %} + {%- if dims.ny > 0 %} + this->set_yrefs(yref.data()); + {%- endif %} + {%- if dims.ny_e > 0 %} + this->set_yref_e(yrefN.data()); + {%- endif %} + {%- if dims.np > 0 %} + this->set_ocp_parameters(p.data(), p.size()); + {%- endif %} - {%- if dims.ny_0 > 0 %} - this->set_yref0(yref0.data()); - {%- endif %} - {%- if dims.ny > 0 %} - this->set_yrefs(yref.data()); - {%- endif %} - {%- if dims.ny_e > 0 %} - this->set_yref_e(yrefN.data()); - {%- endif %} - {%- if dims.np > 0 %} - this->set_ocp_parameters(p.data(), p.size()); - {%- endif %} + // Solve OCP + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + int status; + if (first_solve_) { + this->warmstart_solver_states(x0.data()); + status = this->ocp_solve(); + } + else { + status = this->feedback_rti_solve(); + } - // Solve OCP - {%- if solver_options.nlp_solver_type == "SQP_RTI" %} - int status; - if (first_solve_) { - this->warmstart_solver_states(x0.data()); - status = this->ocp_solve(); - } - else { - status = this->feedback_rti_solve(); + {%- else %} + int status = this->ocp_solve(); + {%- endif %} + this->solver_status_behaviour(status); } - - {%- else %} - int status = this->ocp_solve(); - {%- endif %} - this->solver_status_behaviour(status); } void {{ ClassName }}::solver_status_behaviour(int status) { @@ -179,39 +210,83 @@ void {{ ClassName }}::solver_status_behaviour(int status) { } else { first_solve_ = true; - } - {%- endif %} - - // reset solver if nan is detected - if (status == ACADOS_NAN_DETECTED) { + if (config_.verbose) { + {{ model.name }}_acados_print_stats(ocp_capsule_); + } + RCLCPP_INFO(this->get_logger(), "Resetting acados solver memory..."); {{ model.name }}_acados_reset(ocp_capsule_, 1); } + {%- endif %} } // --- ROS Callbacks --- void {{ ClassName }}::state_callback(const {{ ros_opts.package_name }}_interface::msg::State::SharedPtr msg) { + if (std::any_of(msg->x.begin(), msg->x.end(), [](double val){ return !std::isfinite(val); })) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "State callback received NaN/Inf in 'x'. Ignoring message."); + return; + } + + {%- if use_multithreading %} std::scoped_lock lock(data_mutex_); - std::copy_n(msg->x.begin(), {{ model.name | upper }}_NX, current_x_.begin()); + {%- endif %} + current_x_ = msg->x; + RCLCPP_DEBUG_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "State callback: x=" << current_x_); } void {{ ClassName }}::references_callback(const {{ ros_opts.package_name }}_interface::msg::References::SharedPtr msg) { - std::scoped_lock lock(data_mutex_); {%- if dims.ny_0 > 0 %} - std::copy_n(msg->yref_0.begin(), {{ model.name | upper }}_NY0, current_yref_0_.begin()); + if (std::any_of(msg->yref_0.begin(), msg->yref_0.end(), [](double val){ return !std::isfinite(val); })) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Reference callback received NaN/Inf in 'yref_0'. Ignoring message."); + return; + } {%- endif %} {%- if dims.ny > 0 %} - std::copy_n(msg->yref.begin(), {{ model.name | upper }}_NY, current_yref_.begin()); + if (std::any_of(msg->yref.begin(), msg->yref.end(), [](double val){ return !std::isfinite(val); })) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Reference callback received NaN/Inf in 'yref'. Ignoring message."); + return; + } {%- endif %} {%- if dims.ny_e > 0 %} - std::copy_n(msg->yref_e.begin(), {{ model.name | upper }}_NYN, current_yref_e_.begin()); + if (std::any_of(msg->yref_e.begin(), msg->yref_e.end(), [](double val){ return !std::isfinite(val); })) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Reference callback received NaN/Inf in 'yref_e'. Ignoring message."); + return; + } {%- endif %} + + {%- if use_multithreading %} + std::scoped_lock lock(data_mutex_); + {%- endif %} + {%- if dims.ny_0 > 0 %} current_yref_0_ = msg->yref_0; {% endif %} + {%- if dims.ny > 0 %} current_yref_ = msg->yref; {% endif %} + {%- if dims.ny_e > 0 %} current_yref_e_ = msg->yref_e; {% endif %} + {%- if dims.ny_0 > 0 %} RCLCPP_DEBUG_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Refs callback: yref_0=" << current_yref_0_); {% endif %} + {%- if dims.ny > 0 %} RCLCPP_DEBUG_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Refs callback: yref=" << current_yref_); {% endif %} + {%- if dims.ny_e > 0 %} RCLCPP_DEBUG_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Refs callback: yref_e=" << current_yref_e_); {% endif %} } {%- if dims.np > 0 %} void {{ ClassName }}::parameters_callback(const {{ ros_opts.package_name }}_interface::msg::Parameters::SharedPtr msg) { + if (std::any_of(msg->p.begin(), msg->p.end(), [](double val){ return !std::isfinite(val); })) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Parameter callback received NaN/Inf in 'p'. Ignoring message."); + return; + } + + {%- if use_multithreading %} std::scoped_lock lock(data_mutex_); - std::copy_n(msg->p.begin(), {{ model.name | upper }}_NP, current_p_.begin()); + {%- endif %} + current_p_ = msg->p; + RCLCPP_DEBUG_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Params callback: p=" << current_p_); } {%- endif %} @@ -220,14 +295,13 @@ void {{ ClassName }}::parameters_callback(const {{ ros_opts.package_name }}_inte void {{ ClassName }}::publish_input(const std::array& u0, int status) { auto control_input = std::make_unique<{{ ros_opts.package_name }}_interface::msg::ControlInput>(); control_input->header.stamp = this->get_clock()->now(); - control_input->header.frame_id = "{{ ros_opts.node_name }}_control_input"; control_input->status = status; - std::copy_n(u0.begin(), {{ model.name | upper }}_NU, control_input->u.begin()); + control_input->u = u0; control_input_pub_->publish(std::move(control_input)); } -// --- Parameter Handling Methods --- +// --- ROS Parameter --- void {{ ClassName }}::setup_parameter_handlers() { {%- if dims.nh_0 > 0 or dims.nphi_0 > 0 or dims.nsh_0 > 0 or dims.nsphi_0 > 0 %} // Initial Constraints @@ -383,6 +457,16 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- endif %} // Solver Options + parameter_handlers_["{{ ros_opts.package_name }}.solver_options.print_level"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult&) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + int print_level = p.as_int(); + ocp_nlp_solver_opts_set(ocp_nlp_config_, ocp_nlp_opts_, "print_level", &print_level); + }; + + // Ros Configs parameter_handlers_["{{ ros_opts.package_name }}.ts"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->config_.ts = p.as_double(); @@ -393,6 +477,10 @@ void {{ ClassName }}::setup_parameter_handlers() { res.successful = false; } }; + parameter_handlers_["{{ ros_opts.package_name }}.verbose"] = + [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult&) { + this->config_.verbose = p.as_bool(); + }; } void {{ ClassName }}::declare_parameters() { @@ -427,11 +515,16 @@ void {{ ClassName }}::declare_parameters() { {%- endif %} // Solver Options + this->declare_parameter("{{ ros_opts.package_name }}.solver_options.print_level", {{ 0 }}); + + // Ros Configs this->declare_parameter("{{ ros_opts.package_name }}.ts", {{ solver_options.Tsim }}); + this->declare_parameter("{{ ros_opts.package_name }}.verbose", false); } void {{ ClassName }}::load_parameters() { this->get_parameter("{{ ros_opts.package_name }}.ts", config_.ts); + this->get_parameter("{{ ros_opts.package_name }}.verbose", config_.verbose); } void {{ ClassName }}::apply_all_parameters_to_solver() { @@ -471,7 +564,7 @@ rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( parameter_handlers_.at(param_name)(param, result); if (!result.successful) break; } else { - result.reason = "Update for unknown parameter '%s' received.", param_name.c_str(); + result.reason = "Update for unknown parameter '" + param_name + "' received."; result.successful = false; } } @@ -515,14 +608,19 @@ void {{ ClassName }}::update_constraint( std::array vec{}; std::copy_n(values.begin(), N, vec.begin()); - for (int stage : stages) { - int status = ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, stage, field, vec.data()); - - if (status != ACADOS_SUCCESS) { - result.successful = false; - result.reason = "Acados solver failed to set cost field '" + std::string(field) + - "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; - return; + { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + for (int stage : stages) { + int status = ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, stage, field, vec.data()); + + if (status != ACADOS_SUCCESS) { + result.successful = false; + result.reason = "Acados solver failed to set cost field '" + std::string(field) + + "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; + return; + } } } } @@ -558,37 +656,45 @@ void {{ ClassName }}::update_cost( RCLCPP_INFO_STREAM(this->get_logger(), "update cost field '" << field << "' values = " << vec); } - for (int stage : stages) { - int status = ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, field, data_ptr); - if (status != ACADOS_SUCCESS) { - result.successful = false; - result.reason = "Acados solver failed to set cost field '" + std::string(field) + - "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; - return; + { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + for (int stage : stages) { + int status = ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, field, data_ptr); + if (status != ACADOS_SUCCESS) { + result.successful = false; + result.reason = "Acados solver failed to set cost field '" + std::string(field) + + "' for stage " + std::to_string(stage) + " (error code: " + std::to_string(status) + ")"; + return; + } } } } -// --- Helpers --- +// --- ROS Timer --- void {{ ClassName }}::start_control_timer(double period_seconds) { - if (period_seconds <= 0.0) period_seconds = 0.02; + if (control_timer_) control_timer_->cancel(); + if (period_seconds <= 0.0) { + period_seconds = 0.02; + RCLCPP_WARN(this->get_logger(), "Non-positive control period specified. Using default: %f seconds.", period_seconds); + } + auto period = std::chrono::duration_cast( std::chrono::duration(period_seconds)); control_timer_ = this->create_wall_timer( period, - std::bind(&{{ ClassName }}::control_loop, this)); + std::bind(&{{ ClassName }}::control_loop, this) + {%- if use_multithreading %}, timer_group_{%- endif -%}); } -// --- Acados Helpers --- +// --- Acados Solver --- {%- if solver_options.nlp_solver_type == 'SQP_RTI' %} -void {{ ClassName }}::warmstart_solver_states(double *x0) { - for (int i = 1; i <= {{ model.name | upper }}_N; ++i) { - ocp_nlp_out_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, ocp_nlp_in_, i, "x", x0); - } -} - int {{ ClassName }}::prepare_rti_solve() { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} int phase = PREPARATION; ocp_nlp_sqp_rti_opts_set(ocp_nlp_config_, ocp_nlp_opts_, "rti_phase", &phase); int status = {{ model.name }}_acados_solve(ocp_capsule_); @@ -600,6 +706,9 @@ int {{ ClassName }}::prepare_rti_solve() { } int {{ ClassName }}::feedback_rti_solve() { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} int phase = FEEDBACK; ocp_nlp_sqp_rti_opts_set(ocp_nlp_config_, ocp_nlp_opts_, "rti_phase", &phase); int status = {{ model.name }}_acados_solve(ocp_capsule_); @@ -611,6 +720,9 @@ int {{ ClassName }}::feedback_rti_solve() { {%- endif %} int {{ ClassName }}::ocp_solve() { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} int status = {{ model.name }}_acados_solve(ocp_capsule_); if (status != ACADOS_SUCCESS) { RCLCPP_ERROR(this->get_logger(), "Solver failed with status: %d", status); @@ -618,32 +730,53 @@ int {{ ClassName }}::ocp_solve() { return status; } + +// --- Acados Getter --- void {{ ClassName }}::get_input(double* u, int stage) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} ocp_nlp_out_get(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, stage, "u", u); } void {{ ClassName }}::get_state(double* x, int stage) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} ocp_nlp_out_get(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, stage, "x", x); } + +// --- Acados Setter --- void {{ ClassName }}::set_x0(double* x0) { - ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "lbx", x0); - ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "ubx", x0); + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + int lbx_status = ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "lbx", x0); + int ubx_status = ocp_nlp_constraints_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, ocp_nlp_out_, 0, "ubx", x0); + check_acados_status("set_x0 (lbx)", 0, lbx_status); + check_acados_status("set_x0 (ubx)", 0, ubx_status); +} + +void {{ ClassName }}::set_yref(double* yref, int stage) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + int status = ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, "yref", yref); + check_acados_status("set_yref (yref)", stage, status); } {%- if dims.ny_0 > 0 %} void {{ ClassName }}::set_yref0(double* yref0) { - ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, 0, "yref", yref0); + this->set_yref(yref0, 0); } - {%- endif %} {%- if dims.ny > 0 %} -void {{ ClassName }}::set_yref(double* yref, int stage) { - ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, stage, "yref", yref); -} - void {{ ClassName }}::set_yrefs(double* yref) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} for (int i = 1; i < {{ model.name | upper }}_N; i++) { this->set_yref(yref, i); } @@ -652,21 +785,49 @@ void {{ ClassName }}::set_yrefs(double* yref) { {%- if dims.ny_e > 0 %} void {{ ClassName }}::set_yref_e(double* yrefN) { - ocp_nlp_cost_model_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_in_, {{ model.name | upper }}_N, "yref", yrefN); + this->set_yref(yrefN, {{ model.name | upper }}_N); } {%- endif %} {%- if dims.np > 0 %} void {{ ClassName }}::set_ocp_parameter(double* p, size_t np, int stage) { - {{ model.name }}_acados_update_params(ocp_capsule_, stage, p, np); + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + int status = {{ model.name }}_acados_update_params(ocp_capsule_, stage, p, np); + check_acados_status("set_ocp_parameter (p)", stage, status); } void {{ ClassName }}::set_ocp_parameters(double* p, size_t np) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} for (int i = 0; i <= {{ model.name | upper }}_N; i++) { this->set_ocp_parameter(p, np, i); } } {%- endif %} +{%- if solver_options.nlp_solver_type == 'SQP_RTI' %} + +void {{ ClassName }}::warmstart_solver_states(double *x0) { + {%- if use_multithreading %} + std::lock_guard lock(solver_mutex_); + {%- endif %} + for (int i = 1; i <= {{ model.name | upper }}_N; ++i) { + ocp_nlp_out_set(ocp_nlp_config_, ocp_nlp_dims_, ocp_nlp_out_, ocp_nlp_in_, i, "x", x0); + } +} +{%- endif %} + + +// --- Acados Helper --- +bool {{ ClassName }}::check_acados_status(const char* field, int stage, int status) { + if (status != ACADOS_SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "%s failed at stage %d: %d", field, stage, status); + return false; + } + return true; +} } // namespace {{ ros_opts.package_name }} @@ -674,8 +835,21 @@ void {{ ClassName }}::set_ocp_parameters(double* p, size_t np) { // --- Main --- int main(int argc, char **argv) { rclcpp::init(argc, argv); + + // Suppress ROS timer logging + rcutils_logging_set_logger_level("rcl", RCUTILS_LOG_SEVERITY_WARN); + rcutils_logging_set_logger_level("rclcpp", RCUTILS_LOG_SEVERITY_WARN); + auto node = std::make_shared<{{ ros_opts.package_name }}::{{ ClassName }}>(); +{%- if use_multithreading %} + RCLCPP_INFO(node->get_logger(), "Using MultiThreadedExecutor with %d threads.", {{ ros_opts.threads }}); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), {{ ros_opts.threads }}); + executor.add_node(node); + executor.spin(); +{%- else %} + RCLCPP_INFO(node->get_logger(), "Using SingleThreadedExecutor."); rclcpp::spin(node); +{%- endif %} rclcpp::shutdown(); return 0; } diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h index 2de93eb961..35b5aff8eb 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h @@ -2,9 +2,12 @@ #define {{ ros_opts.node_name | upper }}_H #include +#include #include #include #include +#include +#include #include // ROS2 message includes @@ -32,6 +35,7 @@ namespace {{ ros_opts.package_name }} { {%- set ClassName = ros_opts.node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +{%- set use_multithreading = ros_opts.threads is defined and ros_opts.threads > 1 %} class {{ ClassName }} : public rclcpp::Node { private: // --- ROS Subscriptions --- @@ -52,16 +56,23 @@ class {{ ClassName }} : public rclcpp::Node { // --- Acados Solver --- {{ model.name }}_solver_capsule *ocp_capsule_; - ocp_nlp_config* ocp_nlp_config_; - ocp_nlp_dims* ocp_nlp_dims_; ocp_nlp_in* ocp_nlp_in_; ocp_nlp_out* ocp_nlp_out_; + ocp_nlp_out* ocp_nlp_sens_; + ocp_nlp_config* ocp_nlp_config_; void* ocp_nlp_opts_; + ocp_nlp_dims* ocp_nlp_dims_; + {%- if use_multithreading %} - // --- Data and States --- + // --- Multithreading --- + rclcpp::CallbackGroup::SharedPtr timer_group_; + rclcpp::CallbackGroup::SharedPtr services_group_; std::mutex data_mutex_; - {{ ClassName }}Config config_; + std::recursive_mutex solver_mutex_; + {%- endif %} + // --- Data and States --- + {{ ClassName }}Config config_; {%- if solver_options.nlp_solver_type == "SQP_RTI" %} bool first_solve_{true}; {%- endif %} @@ -100,7 +111,7 @@ class {{ ClassName }} : public rclcpp::Node { // --- ROS Publisher --- void publish_input(const std::array& u0, int status); - // --- Parameter Handling Methods --- + // --- ROS Parameter --- void setup_parameter_handlers(); void declare_parameters(); void load_parameters(); @@ -129,20 +140,21 @@ class {{ ClassName }} : public rclcpp::Node { const char* field, const std::vector& stages); - // --- Helpers --- - void start_control_timer(double period_seconds = 0.02); + // --- ROS Timer --- + void start_control_timer(double period_seconds); - // --- Acados Helpers --- + // --- Acados Solver --- {%- if solver_options.nlp_solver_type == "SQP_RTI" %} - void warmstart_solver_states(double *x0); int prepare_rti_solve(); int feedback_rti_solve(); {%- endif %} int ocp_solve(); + // --- Acados Getter --- void get_input(double* u, int stage); void get_state(double* x, int stage); + // --- Acados Setter --- void set_x0(double* x0); {%- if dims.ny_0 > 0 %} void set_yref0(double* yref0); @@ -158,6 +170,15 @@ class {{ ClassName }} : public rclcpp::Node { void set_ocp_parameter(double* p, size_t np, int stage); void set_ocp_parameters(double* p, size_t np); {%- endif %} + {%- if solver_options.nlp_solver_type == "SQP_RTI" %} + void warmstart_solver_states(double *x0); + {%- endif %} + + // --- Helpers --- + bool check_acados_status( + const char* field, + int stage, + int status); }; } // namespace {{ ros_opts.package_name }} diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py index 2e8d7cd33e..7319fad647 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py @@ -16,12 +16,12 @@ {%- endif %} {%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} {%- if ns %} -{%- set control_input_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} +{%- set control_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} {%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} {%- set references_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.reference_topic %} {%- set parameters_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.parameters_topic %} {%- else %} -{%- set control_input_topic = "/" ~ ros_opts.control_topic %} +{%- set control_topic = "/" ~ ros_opts.control_topic %} {%- set state_topic = "/" ~ ros_opts.state_topic %} {%- set references_topic = "/" ~ ros_opts.reference_topic %} {%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} @@ -112,47 +112,33 @@ def test_set_solver_options(self, proc_info): def test_subscribing(self, proc_info): """Test if the node subscribes to all expected topics.""" - try: - self.wait_for_subscription('{{ state_topic }}') - except TimeoutError: - self.fail("Node has NOT subscribed to '{{ state_topic }}'.") - - try: - self.wait_for_subscription('{{ references_topic }}') - except TimeoutError: - self.fail("Node has NOT subscribed to '{{ references_topic }}'.") - + self.wait_for_subscription('{{ state_topic }}') + self.wait_for_subscription('{{ references_topic }}') {%- if dims.np > 0 %} - try: - self.wait_for_subscription('{{ parameters_topic }}') - except TimeoutError: - self.fail("Node has NOT subscribed to '{{ parameters_topic }}'.") + self.wait_for_subscription('{{ parameters_topic }}') {%- endif %} def test_publishing(self, proc_info): """Test if the node publishes to all expected topics.""" - try: - self.wait_for_publisher('{{ control_input_topic }}') - except TimeoutError: - self.fail("Node has NOT published to '{{ control_input_topic }}'.") + self.wait_for_publisher('{{ control_topic }}') - def wait_for_subscription(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): - end_time = time.time() + timeout + threshold + def wait_for_subscription(self, topic: str, timeout: float = 2.0): + end_time = time.time() + timeout while time.time() < end_time: subs = self.node.get_subscriptions_info_by_topic(topic) if subs: return True - time.sleep(0.05) - raise TimeoutError(f"No subscriber found on {topic} within {timeout}s") + time.sleep(0.1) + self.fail(f"Node has NOT subscribed to '{topic}'.") - def wait_for_publisher(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): - end_time = time.time() + timeout + threshold + def wait_for_publisher(self, topic: str, timeout: float = 2.0): + end_time = time.time() + timeout while time.time() < end_time: pubs = self.node.get_publishers_info_by_topic(topic) if pubs: return True - time.sleep(0.05) - raise TimeoutError(f"No publisher found on {topic} within {timeout}s") + time.sleep(0.1) + self.fail(f"Node has NOT published to '{topic}'.") def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): """Run a subprocess command and return its output.""" diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/CMakeLists.in.txt new file mode 100644 index 0000000000..a13f99511f --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/CMakeLists.in.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.16) +project({{ package_name }}) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +# --- ROS DEPENDENCIES --- +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +{%- for dep in dependencies %} +find_package({{ dep }} REQUIRED) +{%- endfor %} + +# --- EXECUTABLE --- +add_executable({{ node_name }} + src/node.cpp +) + +# --- TARGET CONFIGURATION --- +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options({{ node_name }} PRIVATE -Wall -Wextra -Wpedantic) +endif() + +target_include_directories({{ node_name }} PUBLIC + $ + $ +) + +# --- DEPENDENCIES --- +ament_target_dependencies({{ node_name }} + rclcpp + {%- for dep in dependencies %} + {{ dep }} + {%- endfor %} +) + +# --- INSTALLATIONS --- +install(TARGETS + {{ node_name }} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + include/ + DESTINATION include +) + +# --- TESTS --- +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test( + test/test_{{ package_name }}.launch.py + TARGET test_{{ package_name }} + TIMEOUT 180 + ) +endif() + +ament_package() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/README.in.md new file mode 100644 index 0000000000..e1c4f18315 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/README.in.md @@ -0,0 +1,25 @@ +# {{ package_name | replace(from="_", to=" ") | title }} + +This package is generated by the [acados_template](https://github.com/acados/acados) package. + +## Overview +This package contains the ROS node generation for acados, which is a high-performance solver for optimal control problems. The generated nodes can be used to interface with acados solvers in a ROS environment. + + +## Installation +To install this package, you can use the following command: +```bash +rosdep install --from-paths src --ignore-src -r -y +``` + +```bash +colcon build --packages-select {{ package_name }} && source install/setup.bash +``` + +## Usage +After building the package, you can run the generated nodes using: +```bash +ros2 run {{ package_name }} {{ node_name }} +``` + + diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.cpp new file mode 100644 index 0000000000..b4e6f6183b --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.cpp @@ -0,0 +1,92 @@ +#include "{{ package_name }}/node.h" + + +namespace {{ package_name }} +{ +{% set ClassName = node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +{{ ClassName }}::{{ ClassName }}() + : Node("{{ node_name }}") +{ + RCLCPP_INFO(this->get_logger(), "Initializing {{ node_name | replace(from="_", to=" ") | title }}..."); + + // --- Subscriptions --- + {%- for in_msg in in_msgs %} + {{ in_msg.topic_name }}_sub_ = this->create_subscription<{{ in_msg.msg_type }}>( + "{{ in_msg.topic_name }}", 3, + std::bind(&{{ ClassName }}::{{ in_msg.topic_name }}_callback, this, std::placeholders::_1)); + {%- endfor %} + + // --- Publishers --- + {%- for out_msg in out_msgs %} + {{ out_msg.topic_name }}_pub_ = this->create_publisher<{{ out_msg.msg_type }}>( + "{{ out_msg.topic_name }}", 3); + {%- endfor %} +} + +{{ ClassName }}::~{{ ClassName }}() { + RCLCPP_INFO(this->get_logger(), "Shutting down {{ node_name | replace(from="_", to=" ") | title }}."); +} + + +// --- ROS Callbacks --- +{%- for in_msg in in_msgs %} +void {{ ClassName }}::{{ in_msg.topic_name }}_callback(const {{ in_msg.msg_type }}::SharedPtr msg) { + {%- set fields_to_store = in_msg.flat_field_tree | filter(attribute="needs_storage", value=true) %} + {%- if fields_to_store | length > 0 %} + { + std::scoped_lock lock(data_mutex_); + {%- for field in fields_to_store %} + this->{{ in_msg.topic_name }}_{{ field.name | replace(from=".", to="_") }}_ = msg->{{ field.name }}; + {%- endfor %} + } + {%- endif %} + + // --- Publish msgs --- + {%- for out_msg in out_msgs %} + {%- if out_msg.exec_topic == in_msg.topic_name %} + { + auto out_msg_ptr = std::make_unique<{{ out_msg.msg_type }}>(); + + {%- if out_msg.needs_publish_lock %} + std::scoped_lock lock(data_mutex_); + {%- endif %} + + {%- for map in out_msg.mapping %} + {%- set src = map.source %} + {%- set dest = map.dest %} + + {#- Source accessor #} + {%- if src.topic == in_msg.topic_name %} + {%- set source_accessor = "msg->" ~ src.field %} + {%- else %} + {%- set source_accessor = "this->" ~ src.topic ~ "_" ~ src.field ~ "_" | replace(from=".", to="_") %} + {%- endif %} + {%- if src.index or src.index == 0 %} + {%- set source_accessor = source_accessor ~ "[" ~ src.index ~ "]" %} + {%- endif %} + + {#- Destination accessor #} + {%- set dest_accessor = "out_msg_ptr->" ~ dest.field %} + {%- if dest.index or dest.index == 0 %} + {%- set dest_accessor = dest_accessor ~ "[" ~ dest.index ~ "]" %} + {%- endif %} + {{ dest_accessor }} = {{ source_accessor }}; + {%- endfor %} + {{ out_msg.topic_name }}_pub_->publish(std::move(out_msg_ptr)); + } + {%- endif %} + {%- endfor %} +} +{% endfor %} + +} // namespace {{ package_name }} + + +// --- Main --- +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared<{{ package_name }}::{{ ClassName }}>(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.h new file mode 100644 index 0000000000..81b6f9a985 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/node.in.h @@ -0,0 +1,67 @@ +#ifndef {{ node_name | upper }}_H +#define {{ node_name | upper }}_H + +#include +#include +#include +#include +#include + +// ROS2 message includes +{%- for header_incl in header_includes %} +#include "{{ header_incl }}" +{%- endfor %} + +// Package includes +#include "{{ package_name }}/utils.hpp" + + +namespace {{ package_name }} +{ + +{%- set ClassName = node_name | replace(from="_", to=" ") | title | replace(from=" ", to="") %} +class {{ ClassName }} : public rclcpp::Node { +private: + // --- ROS Subscriptions --- + {%- for in_msg in in_msgs %} + rclcpp::Subscription<{{ in_msg.msg_type }}>::SharedPtr {{ in_msg.topic_name }}_sub_; + {%- endfor %} + + // --- ROS Publishers --- + {%- for out_msg in out_msgs %} + rclcpp::Publisher<{{ out_msg.msg_type }}>::SharedPtr {{ out_msg.topic_name }}_pub_; + {%- endfor %} + + + // --- Data and States --- + std::mutex data_mutex_; + {%- for in_msg in in_msgs %} + {%- set fields_to_store = in_msg.flat_field_tree | filter(attribute="needs_storage", value=true) %} + {%- if fields_to_store | length > 0 %} + // {{ in_msg.topic_name }} + {%- for field in fields_to_store %} + {%- if field.is_array and field.array_size == 0 %} + std::vector<{{ field.cpp_type }}> {{ in_msg.topic_name }}_{{ field.name | replace(from=".", to="_") }}_; + {%- elif field.is_array and field.array_size > 0 %} + std::array<{{ field.cpp_type }}, {{ field.array_size }}> {{ in_msg.topic_name }}_{{ field.name | replace(from=".", to="_") }}_; + {%- else %} + {{ field.cpp_type }} {{ in_msg.topic_name }}_{{ field.name | replace(from=".", to="_") }}_; + {%- endif %} + {%- endfor %} + {%- endif %} + {% endfor %} + +public: + {{ ClassName }}(); + ~{{ ClassName }}(); + +private: + // --- ROS Callbacks --- + {%- for in_msg in in_msgs %} + void {{ in_msg.topic_name }}_callback(const {{ in_msg.msg_type }}::SharedPtr msg); + {%- endfor %} +}; + +} // namespace {{ package_name }} + +#endif // {{ node_name | upper }}_H \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/package.in.xml new file mode 100644 index 0000000000..f689d3f5fd --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/package.in.xml @@ -0,0 +1,24 @@ + + + + {{ package_name }} + 0.1.0 + A Generic ROS2 node to map messages + Josua Lindemann + MIT + + ament_cmake + + rclcpp + {%- for dep in dependencies %} + {{ dep }} + {%- endfor %} + + launch_testing + launch_testing_ament_cmake + python3-pytest + + + ament_cmake + + \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/test.launch.in.py new file mode 100644 index 0000000000..4a31b1abe1 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/test.launch.in.py @@ -0,0 +1,128 @@ +import re +from typing import Union +from unittest import result +import rclpy +import unittest +import launch +import time +import launch_testing +import pytest +import subprocess +from launch_ros.actions import Node + +{%- for m in out_msgs | concat(with=in_msgs) %} +{%- set parts = m.msg_type | split(pat="::") %} +from {{ parts[0] }}.{{ parts[1] }} import {{ parts[2] }} +{%- endfor %} +{%- set ns = namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} + +@pytest.mark.launch_test +def generate_test_description(): + """Generate launch description for node testing.""" + start_{{ node_name }} = Node( + package='{{ package_name }}', + executable='{{ node_name }}', + name='{{ node_name }}' + ) + + return launch.LaunchDescription([ + start_{{ node_name }}, + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()]), + ]) + + +class GeneratedNodeTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('generated_node_test') + + def tearDown(self): + self.node.destroy_node() + + def test_subscribing(self, proc_info): + """Test if the node subscribes to all expected topics.""" + {%- for m in in_msgs %} + self.wait_for_subscription('{{ m.topic_name }}') + {%- endfor %} + + def test_publishing(self, proc_info): + """Test if the node publishes to all expected topics.""" + {%- for m in out_msgs %} + self.wait_for_publisher('{{ m.topic_name }}') + {%- endfor %} + + def wait_for_subscription(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + subs = self.node.get_subscriptions_info_by_topic(topic) + if subs: + return True + time.sleep(0.05) + self.fail(f"Node has NOT subscribed to '{topic}'.") + + def wait_for_publisher(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): + end_time = time.time() + timeout + threshold + while time.time() < end_time: + pubs = self.node.get_publishers_info_by_topic(topic) + if pubs: + return True + time.sleep(0.05) + self.fail(f"Node has NOT published to '{topic}'.") + + def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + output = get_parameter(param_name) + numbers = [float(x) for x in re.findall(r"[-+]?\d*\.\d+|\d+", output)] + if isinstance(expected_value, list): + self.assertListEqual(numbers, expected_value, f"Parameter {param_name} has the wrong value! Got {numbers}") + else: + self.assertEqual(numbers[0], expected_value, f"Parameter {param_name} has the wrong value! Got {numbers[0]}") + + def __check_parameter_set(self, param_name: str, new_value: Union[list[float], float]): + """Run a subprocess command and return its output.""" + try: + set_parameter(param_name, new_value) + self.__check_parameter_get(param_name, new_value) + except subprocess.CalledProcessError as e: + self.fail(f"Failed to set parameter {param_name}.\n" + f"Exit-Code: {e.returncode}\n" + f"Stderr: {e.stderr}\n" + f"Stdout: {e.stdout}") + + +def get_parameter(param_name: str): + """Run a subprocess command and return its output.""" + cmd = ['ros2', 'param', 'get', '{{ node_name }}', param_name] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stdout + +def set_parameter(param_name: str, value: Union[list[float], float]): + """Run a subprocess command to set a parameter.""" + if isinstance(value, list): + value_str = "[" + ",".join(map(str, value)) + "]" + else: + value_str = str(value) + + cmd = ['ros2', 'param', 'set', '{{ node_name }}', param_name, value_str] + result = subprocess.run( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True + ) + return result.stderr \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/utils.in.hpp b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/utils.in.hpp new file mode 100644 index 0000000000..c58d8b3409 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ros_mapper_templates/utils.in.hpp @@ -0,0 +1,30 @@ +#ifndef {{ package_name | upper }}_UTILS_HPP +#define {{ package_name | upper }}_UTILS_HPP + +#include +#include +#include + +template +std::ostream& operator<<(std::ostream& os, const double (&arr)[N]) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i + 1 < N) os << ", "; + } + os << "]"; + return os; +} + +template +std::ostream& operator<<(std::ostream& os, const std::array& arr) { + os << "["; + for (size_t i = 0; i < N; ++i) { + os << arr[i]; + if (i < N - 1) os << ", "; + } + os << "]"; + return os; +} + +#endif // {{ package_name | upper }}_UTILS_HPP diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt index 5982d09817..6afb2e29ba 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/CMakeLists.in.txt @@ -11,13 +11,13 @@ find_package(rosidl_default_generators REQUIRED) # --- INTERFACES --- set(MSG_FILES - "msg/State.msg" - "msg/ControlInput.msg" + "msg/State.msg" + "msg/ControlInput.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} - ${MSG_FILES} - DEPENDENCIES std_msgs + ${MSG_FILES} + DEPENDENCIES std_msgs ) ament_package() diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg index fd4efacb5c..fab1f1dcb7 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/ControlInput.in.msg @@ -2,4 +2,4 @@ std_msgs/Header header # control vector -float64[{{ dims.nu }}] u \ No newline at end of file +float64[{{ dims.nu }}] u diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg index e1ec27e10c..68e859093f 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_interface_templates/State.in.msg @@ -5,4 +5,4 @@ std_msgs/Header header float64[{{ dims.nx }}] x # status, 0 = OK -uint8 status \ No newline at end of file +uint8 status diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md index 7760474c0d..e3d7c938be 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/README.in.md @@ -1,6 +1,6 @@ # {{ ros_opts.package_name | replace(from="_", to=" ") | title }} -This package is generated by the [acados_template](https://github.com/acados/acados) package. +This package is generated by the [acados_template](https://github.com/acados/acados) package. ## Overview This package contains the ROS node generation for acados, which is a high-performance solver for optimal control problems. The generated nodes can be used to interface with acados solvers in a ROS environment. @@ -21,5 +21,3 @@ After building the package, you can run the generated nodes using: ```bash ros2 run {{ ros_opts.package_name }} {{ ros_opts.node_name }} ``` - - diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp index 631b9cf127..5bbf36c149 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp @@ -158,7 +158,7 @@ rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( // template // void {{ ClassName }}::get_and_check_array_param( -// const std::string& param_name, +// const std::string& param_name, // std::array& destination // ) { // auto param_value = this->get_parameter(param_name).as_double_array(); @@ -173,7 +173,7 @@ rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( // template // void {{ ClassName }}::update_param_array( -// const rclcpp::Parameter& param, +// const rclcpp::Parameter& param, // std::array& destination_array, // rcl_interfaces::msg::SetParametersResult& result // ) { diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml index 12fc9ed426..7340c8970d 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/package.in.xml @@ -20,4 +20,4 @@ ament_cmake - \ No newline at end of file + diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py index a76061cf19..750e786329 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py @@ -68,22 +68,22 @@ def test_publishing(self, proc_info): """Test if the node publishes to all expected topics.""" self.wait_for_publisher('{{ state_topic }}') - def wait_for_subscription(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): - end_time = time.time() + timeout + threshold + def wait_for_subscription(self, topic: str, timeout: float = 2.0): + end_time = time.time() + timeout while time.time() < end_time: subs = self.node.get_subscriptions_info_by_topic(topic) if subs: return True - time.sleep(0.05) + time.sleep(0.1) self.fail(f"Node has NOT subscribed to '{topic}'.") - def wait_for_publisher(self, topic: str, timeout: float = 1.0, threshold: float = 0.5): - end_time = time.time() + timeout + threshold + def wait_for_publisher(self, topic: str, timeout: float = 2.0): + end_time = time.time() + timeout while time.time() < end_time: pubs = self.node.get_publishers_info_by_topic(topic) if pubs: return True - time.sleep(0.05) + time.sleep(0.1) self.fail(f"Node has NOT published to '{topic}'.") def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): @@ -134,4 +134,4 @@ def set_parameter(param_name: str, value: Union[list[float], float]): text=True, check=True ) - return result.stderr \ No newline at end of file + return result.stderr From 3230c7eef4732b5716e6545538830e91260462ee Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 24 Sep 2025 11:36:33 +0200 Subject: [PATCH 143/164] Add getter for solver options (#1642) - core: remove dims form opts_get to match opts_set and other modules - C interface: add `ocp_nlp_solver_opts_get` - Python: add `AcadosOcpSolver.options_get` --- acados/ocp_nlp/ocp_nlp_common.h | 2 +- acados/ocp_nlp/ocp_nlp_ddp.c | 2 +- acados/ocp_nlp/ocp_nlp_sqp.c | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 7 +++++- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 18 ++++++------- .../acados_template/acados_ocp_solver.py | 25 +++++++++++++++++++ 7 files changed, 44 insertions(+), 14 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index df28631faf..fcd2e47d95 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -108,7 +108,7 @@ typedef struct ocp_nlp_config void (*config_initialize_default)(void *config); // general getter void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_); - void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_); + void (*opts_get)(void *config_, void *opts_, const char *field, void *return_value_); void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_); // void (*terminate)(void *config, void *mem, void *work); diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index ebc30b0e23..62c8d70c4b 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -1014,7 +1014,7 @@ void ocp_nlp_ddp_get(void *config_, void *dims_, void *mem_, const char *field, } -void ocp_nlp_ddp_opts_get(void *config_, void *dims_, void *opts_, +void ocp_nlp_ddp_opts_get(void *config_, void *opts_, const char *field, void *return_value_) { ocp_nlp_ddp_opts *opts = opts_; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 69ca2ac1f2..988c53c300 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -182,7 +182,7 @@ void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, con } -void ocp_nlp_sqp_opts_get(void *config_, void *dims_, void *opts_, +void ocp_nlp_sqp_opts_get(void *config_, void *opts_, const char *field, void *return_value_) { // ocp_nlp_config *config = config_; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 2d110a898d..743541353a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -1357,7 +1357,7 @@ void ocp_nlp_sqp_rti_get(void *config_, void *dims_, void *mem_, } -void ocp_nlp_sqp_rti_opts_get(void *config_, void *dims_, void *opts_, +void ocp_nlp_sqp_rti_opts_get(void *config_, void *opts_, const char *field, void *return_value_) { // ocp_nlp_config *config = config_; @@ -1368,6 +1368,11 @@ void ocp_nlp_sqp_rti_opts_get(void *config_, void *dims_, void *opts_, void **value = return_value_; *value = opts->nlp_opts; } + else if (!strcmp("as_rti_level", field)) + { + int *value = return_value_; + *value = opts->as_rti_level; + } else { printf("\nerror: field %s not available in ocp_nlp_sqp_rti_opts_get\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 9374219f7c..4de9afbe5d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -213,7 +213,7 @@ void ocp_nlp_sqp_wfqp_opts_set_at_stage(void *config_, void *opts_, size_t stage -void ocp_nlp_sqp_wfqp_opts_get(void *config_, void *dims_, void *opts_, +void ocp_nlp_sqp_wfqp_opts_get(void *config_, void *opts_, const char *field, void *return_value_) { // ocp_nlp_config *config = config_; diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 2c00e56fd8..8646bff4a2 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -1283,10 +1283,10 @@ void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *fi } -// void ocp_nlp_solver_opts_get(ocp_nlp_config *config, void *opts_, const char *field, void *value) -// { -// config->opts_get(config, opts_, field, value); -// } +void ocp_nlp_solver_opts_get(ocp_nlp_config *config, void *opts_, const char *field, void *value) +{ + config->opts_get(config, opts_, field, value); +} void ocp_nlp_solver_opts_set_at_stage(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) @@ -1425,7 +1425,7 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out * ocp_nlp_dims *dims = solver->dims; config->get(config, solver->dims, solver->mem, "nlp_mem", &nlp_mem); - config->opts_get(config, solver->dims, solver->opts, "nlp_opts", &nlp_opts); + config->opts_get(config, solver->opts, "nlp_opts", &nlp_opts); config->work_get(config, solver->dims, solver->work, "nlp_work", &nlp_work); ocp_nlp_cost_compute(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); @@ -1440,7 +1440,7 @@ void ocp_nlp_eval_constraints(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nl ocp_nlp_dims *dims = solver->dims; config->get(config, solver->dims, solver->mem, "nlp_mem", &nlp_mem); - config->opts_get(config, solver->dims, solver->opts, "nlp_opts", &nlp_opts); + config->opts_get(config, solver->opts, "nlp_opts", &nlp_opts); config->work_get(config, solver->dims, solver->work, "nlp_work", &nlp_work); ocp_nlp_eval_constraints_common(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); @@ -1456,7 +1456,7 @@ void ocp_nlp_eval_params_jac(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp ocp_nlp_dims *dims = solver->dims; config->get(config, solver->dims, solver->mem, "nlp_mem", &nlp_mem); - config->opts_get(config, solver->dims, solver->opts, "nlp_opts", &nlp_opts); + config->opts_get(config, solver->opts, "nlp_opts", &nlp_opts); config->work_get(config, solver->dims, solver->work, "nlp_work", &nlp_work); ocp_nlp_params_jac_compute(config, dims, nlp_in, nlp_opts, nlp_mem, nlp_work); @@ -1611,7 +1611,7 @@ void ocp_nlp_get_at_stage(ocp_nlp_solver *solver, int stage, const char *field, if (!strcmp(field, "P") || !strcmp(field, "K") || !strcmp(field, "Lr") || !strcmp(field, "p")) { ocp_nlp_opts *nlp_opts; - config->opts_get(config, dims, solver->opts, "nlp_opts", &nlp_opts); + config->opts_get(config, solver->opts, "nlp_opts", &nlp_opts); ocp_qp_xcond_solver_config *xcond_solver_config = config->qp_solver; @@ -1777,7 +1777,7 @@ void ocp_nlp_get_from_iterate(ocp_nlp_solver *solver, int iter, int stage, const 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); + config->opts_get(config, solver->opts, "nlp_opts", &nlp_opts); if (!nlp_opts->store_iterates) { diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 9b86d90d18..4e1db67f77 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -332,6 +332,7 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__acados_lib.ocp_nlp_eval_solution_sens_adj_p.restype = None self.__acados_lib.ocp_nlp_solver_opts_set.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_solver_opts_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_get.argtypes = [c_void_p, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] @@ -2359,6 +2360,30 @@ def options_set(self, field_, value_): return + def options_get(self, field_: str) -> Union[int, float]: + """ + Get options of the solver. + + :param field: string, possible values are: + 'as_rti_level', to be extended. + """ + int_fields = ['as_rti_level'] + if field_ == 'as_rti_level': + if self.__solver_options['nlp_solver_type'] != "SQP_RTI": + raise ValueError("as_rti_level only available for SQP_RTI") + + if field_ in int_fields: + value_ctypes = c_int(0) + else: + raise RuntimeError(f"Unknown field {field_}") + + field = field_.encode('utf-8') + self.__acados_lib.ocp_nlp_solver_opts_get(self.nlp_config, self.nlp_opts, field, byref(value_ctypes)) + + return value_ctypes.value + + + def set_params_sparse(self, stage_: int, idx_values_: np.ndarray, param_values_): """ set parameters of the solvers external function partially: From 55bbd79fd4ee54972b9a241addea329a02d8ff08 Mon Sep 17 00:00:00 2001 From: Florian Messerer <37663021+fmesserer@users.noreply.github.com> Date: Wed, 24 Sep 2025 14:29:03 +0200 Subject: [PATCH 144/164] Update zoro example (#1644) dataclasses using default factories; create all plots when calling main, show only at end; cleaned dataclasses. --- .../diff_drive/diff_drive_utils.py | 7 +- .../zoRO_example/diff_drive/main.py | 19 +- .../zoRO_example/diff_drive/mpc_parameters.py | 230 ++++-------------- 3 files changed, 55 insertions(+), 201 deletions(-) diff --git a/examples/acados_python/zoRO_example/diff_drive/diff_drive_utils.py b/examples/acados_python/zoRO_example/diff_drive/diff_drive_utils.py index 578b2bb5a3..9e644a0b6d 100644 --- a/examples/acados_python/zoRO_example/diff_drive/diff_drive_utils.py +++ b/examples/acados_python/zoRO_example/diff_drive/diff_drive_utils.py @@ -92,8 +92,6 @@ def plot_timings(timing_dict, use_custom_update: bool): plt.savefig(fig_filename, bbox_inches='tight', transparent=True, pad_inches=0.05) print(f"stored figure in {fig_filename}") - plt.show() - def plot_timing_comparison(timings_list, label_list): fig = plt.figure(figsize=(6.0, 1.8)) @@ -126,8 +124,6 @@ def plot_timing_comparison(timings_list, label_list): plt.savefig(fig_filename, bbox_inches='tight', transparent=True, pad_inches=0.05) print(f"stored figure in {fig_filename}") - plt.show() - def ellipsoid_surface_2D(P, n=100): lam, V = np.linalg.eig(P) @@ -138,7 +134,7 @@ def ellipsoid_surface_2D(P, n=100): def plot_trajectory(cfg: MPCParam, traj_ref:np.ndarray, traj_zo:np.ndarray, P_matrices=None, closed_loop=True): - fig = plt.figure(1) + fig = plt.figure() ax = fig.add_subplot(1,1,1) for idx_obs in range(cfg.num_obs): circ_label = "Obstacles" if idx_obs == 0 else None @@ -174,7 +170,6 @@ def plot_trajectory(cfg: MPCParam, traj_ref:np.ndarray, traj_zo:np.ndarray, P_ma fig_filename = os.path.join("figures", "diff_drive_sim_trajectory.pdf") plt.savefig(fig_filename, bbox_inches='tight', transparent=True, pad_inches=0.05) print(f"stored figure in {fig_filename}") - plt.show() def compute_min_dis(cfg:MPCParam, s:np.ndarray) -> float: diff --git a/examples/acados_python/zoRO_example/diff_drive/main.py b/examples/acados_python/zoRO_example/diff_drive/main.py index 5ff4f5953a..d2d41101c3 100644 --- a/examples/acados_python/zoRO_example/diff_drive/main.py +++ b/examples/acados_python/zoRO_example/diff_drive/main.py @@ -28,6 +28,7 @@ import numpy as np import casadi +import matplotlib.pyplot as plt from diff_drive_zoro_mpc import ZoroMPCSolver from mpc_parameters import MPCParam, PathTrackingParam @@ -73,8 +74,8 @@ def run_closed_loop_simulation(use_custom_update: bool, n_executions: int = 1): track_spline = TrackSpline(spline_control_points, spline_seg_length) cfg_path = PathTrackingParam() path_tracking_solver = NominalPathTrackingSolver(track_spline, cfg_path=cfg_path, cfg_traj=cfg_zo) - x_init = np.array([0.0, cfg_path._v_s_0]) - x_e = np.array([1.0, cfg_path._v_s_e]) + x_init = np.array([0.0, cfg_path.v_s_0]) + x_e = np.array([1.0, cfg_path.v_s_e]) path_tracking_solver.solve(x_init=x_init, x_e=x_e) time_prep = [] @@ -168,8 +169,8 @@ def solve_single_zoro_problem_visualize_uncertainty(): track_spline = TrackSpline(spline_control_points, spline_seg_length) cfg_path = PathTrackingParam() path_tracking_solver = NominalPathTrackingSolver(track_spline, cfg_path=cfg_path, cfg_traj=cfg_zo) - x_init = np.array([0.0, cfg_path._v_s_0]) - x_e = np.array([1.0, cfg_path._v_s_e]) + x_init = np.array([0.0, cfg_path.v_s_0]) + x_e = np.array([1.0, cfg_path.v_s_e]) path_tracking_solver.solve(x_init=x_init, x_e=x_e) # zoro solution @@ -215,10 +216,7 @@ def plot_result_timing_comparison(n_executions: int): slow_timings = load_results(get_results_filename(use_custom_update=False, n_executions=n_executions))['timings'] plot_timing_comparison([fast_timings, slow_timings], ['zoRO-24', 'zoRO-21']) -def timing_comparison(): - n_executions = 50 - # run_closed_loop_simulation(use_custom_update=False, n_executions=n_executions) - # run_closed_loop_simulation(use_custom_update=True, n_executions=n_executions) +def timing_comparison(n_executions: int): plot_result_timings(n_executions=n_executions, use_custom_update=True) plot_result_timings(n_executions=n_executions, use_custom_update=False) @@ -231,7 +229,8 @@ def timing_comparison(): run_closed_loop_simulation(use_custom_update=False, n_executions=n_executions) compare_results(n_executions=n_executions) - # plot_result_trajectory(n_executions=n_executions, use_custom_update=True) - # timing_comparison() + plot_result_trajectory(n_executions=n_executions, use_custom_update=True) + timing_comparison(n_executions=n_executions) solve_single_zoro_problem_visualize_uncertainty() + plt.show() \ No newline at end of file diff --git a/examples/acados_python/zoRO_example/diff_drive/mpc_parameters.py b/examples/acados_python/zoRO_example/diff_drive/mpc_parameters.py index ac5de8cc43..24785a801d 100644 --- a/examples/acados_python/zoRO_example/diff_drive/mpc_parameters.py +++ b/examples/acados_python/zoRO_example/diff_drive/mpc_parameters.py @@ -32,154 +32,58 @@ @dataclass class MPCParam(): # dimensions - _nx: int=5 - _nu: int=2 - _nw: int=5 - _delta_t: float=0.1 - _n_hrzn: int=20 + nx: int=5 + nu: int=2 + nw: int=5 + delta_t: float=0.1 + n_hrzn: int=20 # matrix of the cost function - _Q: np.ndarray=np.zeros(0) - _R: np.ndarray=np.zeros(0) - _Q_e: np.ndarray=np.zeros(0) + Q: np.ndarray=field(default_factory=lambda: np.zeros(0)) + R: np.ndarray=field(default_factory=lambda: np.zeros(0)) + Q_e: np.ndarray=field(default_factory=lambda: np.zeros(0)) # constraints - _num_state_cstr: int=2 - _min_forward_velocity: float=0. - _max_forward_velocity: float=1.0 - _max_angular_velocity: float=1.0 - _min_forward_acceleration: float=-1.0 - _max_forward_acceleration: float=0.3 - _max_angular_acceleration: float=2.84 - _term_forward_velocity: float=0.05 - _term_angular_velocity: float=0.05 - - # feedback matrix - _fdbk_k: float=6.0 - _fdbk_K_mat: np.ndarray=np.zeros(0) + num_state_cstr: int=2 + min_forward_velocity: float=0. + max_forward_velocity: float=1.0 + max_angular_velocity: float=1.0 + min_forward_acceleration: float=-1.0 + max_forward_acceleration: float=0.3 + max_angular_acceleration: float=2.84 + term_forward_velocity: float=0.05 + term_angular_velocity: float=0.05 + + # feedback gain scalar parameter (full structured matrix defined below) + fdbk_k: float=6.0 # uncertainty / distrubance - _unc_jac_G_mat: np.ndarray=np.zeros(0) - _W_mat: np.ndarray=np.zeros(0) - _P0_mat: np.ndarray=np.zeros(0) + unc_jac_G_mat: np.ndarray=field(default_factory=lambda: np.zeros(0)) + W_mat: np.ndarray=field(default_factory=lambda: np.zeros(0)) + P0_mat: np.ndarray=field(default_factory=lambda: np.zeros(0)) # obstacles - _num_obs: int=3 - _obs_radius: np.ndarray=np.array([1.0, 0.7, 0.55]) - _obs_pos: np.ndarray=np.array([[0.0, 1.0], [3.0, 0.68], [7.0, 1.2]]) + num_obs: int=3 + _obs_radius: np.ndarray=field(default_factory=lambda: np.array([1.0, 0.7, 0.55])) + _obs_pos: np.ndarray=field(default_factory=lambda: np.array([[0.0, 1.0], [3.0, 0.68], [7.0, 1.2]])) # zoRO - _backoff_eps: float=1e-8 - _zoRO_iter: int=2 - _use_custom_update: bool=True + backoff_eps: float=1e-8 + zoRO_iter: int=2 + use_custom_update: bool=True def __post_init__(self): - self._Q: np.eye(self._nx) - self._R: np.eye(self._nu) * 1e-1 - self._Q_e: np.eye(self._nx) - - self._fdbk_K_mat = np.array([[0., 0., 0., self._fdbk_k, 0.], \ - [0., 0., 0., 0., self._fdbk_k]]) - self._unc_jac_G_mat = np.eye(self._nx) - self._W_mat = np.diag([2.0e-06, 2.0e-06, 4.0e-06, 1.5e-03, 7.0e-03]) - self._P0_mat = np.diag([2.0e-06, 2.0e-06, 4.0e-06, 1.5e-03, 7.0e-03]) - - @property - def nx(self)->int: - return self._nx - - @property - def nu(self)->int: - return self._nu - - @property - def nw(self)->int: - return self._nw - - @property - def delta_t(self)->float: - return self._delta_t - - @property - def n_hrzn(self)->int: - return self._n_hrzn - - @property - def Q(self)->np.ndarray: - return np.eye(self._nx) - - @property - def R(self)->np.ndarray: - return np.eye(self._nu) * 1e-1 - - @property - def Q_e(self)->np.ndarray: - return np.eye(self._nx) - - @property - def num_state_cstr(self)->int: - return self._num_state_cstr - - @property - def min_forward_velocity(self)->float: - return self._min_forward_velocity - - @property - def max_forward_velocity(self)->float: - return self._max_forward_velocity - - @property - def max_angular_velocity(self)->float: - return self._max_angular_velocity - - @property - def min_forward_acceleration(self)->float: - return self._min_forward_acceleration - - @property - def max_forward_acceleration(self)->float: - return self._max_forward_acceleration - - @property - def max_angular_acceleration(self)->float: - return self._max_angular_acceleration - - @property - def term_forward_velocity(self)->float: - return self._term_forward_velocity - - @property - def term_angular_velocity(self)->float: - return self._term_angular_velocity - - @property - def fdbk_k(self)->float: - return self._fdbk_k + self.Q = np.eye(self.nx) + self.R = np.eye(self.nu) * 1e-1 + self.Q_e = np.eye(self.nx) + self.unc_jac_G_mat = np.eye(self.nx) + self.W_mat = np.diag([2.0e-06, 2.0e-06, 4.0e-06, 1.5e-03, 7.0e-03]) + self.P0_mat = np.diag([2.0e-06, 2.0e-06, 4.0e-06, 1.5e-03, 7.0e-03]) @property def fdbk_K_mat(self)->np.ndarray: - return np.array([[0., 0., 0., self._fdbk_k, 0.], \ - [0., 0., 0., 0., self._fdbk_k]]) - - @property - def unc_jac_G_mat(self)->np.ndarray: - return np.eye(self._nw) - - @property - def W_mat(self)->np.ndarray: - return self._W_mat - - @property - def P0_mat(self)->np.ndarray: - return self._P0_mat - - @property - def num_obs(self)->int: - return self._num_obs - - @num_obs.setter - def num_obs(self, n:int): - self._num_obs = n + return np.array([[0., 0., 0., self.fdbk_k, 0.], \ + [0., 0., 0., 0., self.fdbk_k]]) @property def obs_radius(self)->np.ndarray: @@ -187,7 +91,7 @@ def obs_radius(self)->np.ndarray: @obs_radius.setter def obs_radius(self, radius: np.ndarray): - assert radius.size == self._num_obs + assert radius.size == self.num_obs self._obs_radius = radius @property @@ -196,59 +100,15 @@ def obs_pos(self)->np.ndarray: @obs_pos.setter def obs_pos(self, pos: np.ndarray): - assert pos.size[0] == self._num_obs and pos.size[1] == 2 + assert pos.size[0] == self.num_obs and pos.size[1] == 2 self._obs_pos = pos - @property - def backoff_eps(self)->float: - return self._backoff_eps - - @property - def zoRO_iter(self)->int: - return self._zoRO_iter - - @zoRO_iter.setter - def zoRO_iter(self, n: int): - self._zoRO_iter = n - - @property - def use_custom_update(self)->bool: - return self._use_custom_update - - @use_custom_update.setter - def use_custom_update(self, val: bool): - self._use_custom_update = val @dataclass class PathTrackingParam: - _nx: int=2 - _nu: int=1 - _nu_wT: int=2 - _n_hrzn: int=500 - _v_s_0: float=0.001 - _v_s_e: float=0.001 - - - @property - def nx(self)->int: - return self._nx - - @property - def nu(self)->int: - return self._nu - - @property - def n_hrzn(self)->int: - return self._n_hrzn - - @property - def nu_wT(self)->int: - return self._nu_wT - - @property - def v_s_0(self) -> float: - return self._v_s_0 - - @property - def v_s_e(self) -> float: - return self._v_s_e \ No newline at end of file + nx: int=2 + nu: int=1 + nu_wT: int=2 + n_hrzn: int=500 + v_s_0: float=0.001 + v_s_e: float=0.001 From 6b5ba63ff07a256a748d1c8c25badf8ba5ef501e Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Wed, 24 Sep 2025 14:31:47 +0200 Subject: [PATCH 145/164] Python: Serialization of CasADi expression in `AcadosModel` (#1639) Add `serialize`, `deserialize` and `from_dict`, `to_dict` methods to `AcadosModel`. --------- Co-authored-by: Jonathan Frey --- .github/workflows/full_build.yml | 1 + .../acados_python/tests/test_serialize.py | 145 ++++++++++++++++++ .../acados_template/acados_model.py | 88 ++++++++++- .../acados_template/acados_multiphase_ocp.py | 9 +- .../acados_template/acados_ocp.py | 51 +++++- .../acados_template/acados_sim.py | 6 +- .../acados_template/acados_template/utils.py | 17 +- 7 files changed, 297 insertions(+), 20 deletions(-) create mode 100644 examples/acados_python/tests/test_serialize.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 0dc268683b..d0a40e694b 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -129,6 +129,7 @@ jobs: source ${{ github.workspace }}/acadosenv/bin/activate cd ${{ github.workspace }}/examples/acados_python/tests python test_rti_sqp_residuals.py + python test_serialize.py cd ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ocp python test_casadi_formulation.py diff --git a/examples/acados_python/tests/test_serialize.py b/examples/acados_python/tests/test_serialize.py new file mode 100644 index 0000000000..70a26c251c --- /dev/null +++ b/examples/acados_python/tests/test_serialize.py @@ -0,0 +1,145 @@ +# +# 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.; +# + +import sys +sys.path.insert(0, '../pendulum_on_cart/common') + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel +from pendulum_model import export_pendulum_ode_model +import numpy as np +import scipy.linalg +from utils import plot_pendulum +import casadi as ca + +def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_num_hess=0, + integrator_type='ERK'): + print(f"using: cost_type {cost_type}, integrator_type {integrator_type}") + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + Tf = 1.0 + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + N_horizon = 20 + + ocp.solver_options.N_horizon = N_horizon + + # set cost + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + x = ocp.model.x + u = ocp.model.u + + cost_W = scipy.linalg.block_diag(Q_mat, R_mat) + + if cost_type == 'LS': + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vx[:nx,:nx] = np.eye(nx) + + Vu = np.zeros((ny, nu)) + Vu[4,0] = 1.0 + ocp.cost.Vu = Vu + + ocp.cost.Vx_e = np.eye(nx) + + elif cost_type == 'NONLINEAR_LS': + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + ocp.model.cost_y_expr = ca.vertcat(x, u) + ocp.model.cost_y_expr_e = x + + elif cost_type == 'EXTERNAL': + ocp.cost.cost_type = 'EXTERNAL' + ocp.cost.cost_type_e = 'EXTERNAL' + + ocp.model.cost_expr_ext_cost = ca.vertcat(x, u).T @ cost_W @ ca.vertcat(x, u) + ocp.model.cost_expr_ext_cost_e = x.T @ Q_mat @ x + + else: + raise Exception('Unknown cost_type. Possible values are \'LS\' and \'NONLINEAR_LS\'.') + + if cost_type in ['LS', 'NONLINEAR_LS']: + ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref_e = np.zeros((ny_e, )) + ocp.cost.W_e = Q_mat + ocp.cost.W = cost_W + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + x0 = np.array([0.0, np.pi, 0.0, 0.0]) + ocp.constraints.x0 = x0 + ocp.constraints.idxbu = np.array([0]) + + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.hessian_approx = hessian_approximation + ocp.solver_options.regularize_method = 'CONVEXIFY' + ocp.solver_options.integrator_type = integrator_type + + if ocp.solver_options.integrator_type == 'GNSF': + import json + with open('../pendulum_on_cart/common/' + model.name + '_gnsf_functions.json', 'r') as f: + gnsf_dict = json.load(f) + ocp.gnsf_model = gnsf_dict + + # set prediction horizon + ocp.solver_options.tf = Tf + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.ext_cost_num_hess = ext_cost_use_num_hess + + model_dict = ocp.model.to_dict() + + model_from_dict = AcadosModel.from_dict(model_dict) + ocp.model = model_from_dict + + solver = AcadosOcpSolver(ocp, json_file=f"acados_ocp_{model.name}.json") + + + +if __name__ == '__main__': + for integrator_type in ['GNSF', 'ERK', 'IRK']: + for cost_type in ['EXTERNAL', 'LS', 'NONLINEAR_LS']: + hessian_approximation = 'GAUSS_NEWTON' + ext_cost_use_num_hess = 1 + main(cost_type=cost_type, hessian_approximation=hessian_approximation, + ext_cost_use_num_hess=ext_cost_use_num_hess, integrator_type=integrator_type) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 202998b592..5da0814efc 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -28,7 +28,8 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from typing import Union +from typing import Union, Tuple, List +import inspect, warnings import casadi as ca import numpy as np @@ -964,5 +965,88 @@ def reformulate_with_polynomial_control(self, degree: int) -> None: def _has_custom_hess(self) -> bool: return not (is_empty(self.cost_expr_ext_cost_custom_hess_0) and - is_empty(self.cost_expr_ext_cost_custom_hess) and + is_empty(self.cost_expr_ext_cost_custom_hess) and is_empty(self.cost_expr_ext_cost_custom_hess_e)) + + + def serialize(self) -> Tuple[str, List[str]]: + """ + Serialize the CasADi expressions. + """ + + serializer = ca.StringSerializer() + expression_names = [] + + for k, _ in inspect.getmembers(type(self), lambda v: isinstance(v, property)): + v = getattr(self, k) + if isinstance(v, (ca.SX, ca.MX)): + serializer.pack(v) + expression_names.append(k) + + return serializer.encode(), expression_names + + + def deserialize(self, s: str, expression_names: list) -> None: + """ + Deserialize the CasADi expressions. + """ + + deserializer = ca.StringDeserializer(s) + + for name in expression_names: + setattr(self, name, deserializer.unpack()) + + + def to_dict(self) -> dict: + """ + Convert the AcadosModel to a dictionary. + """ + + model_dict = {} + + for k, _ in inspect.getmembers(type(self), lambda v: isinstance(v, property)): + v = getattr(self, k) + if isinstance(v, (ca.SX, ca.MX)): + model_dict[k] = repr(v) # only for debugging + else: + model_dict[k] = v + + model_dict['serialized_expressions'], model_dict['expression_names'] = self.serialize() + + return model_dict + + + @classmethod + def from_dict(cls, model_dict: dict) -> 'AcadosModel': + """ + Create an AcadosModel from a dictionary. + Values that correspond to the empty list are ignored. + """ + + model = cls() + + expression_names = model_dict.get('expression_names') + serialized_expressions = model_dict.get('serialized_expressions') + + if expression_names is None or serialized_expressions is None: + raise ValueError("Dictionary does not contain serialized expressions.") + + # loop over all properties + for attr, _ in inspect.getmembers(type(model), lambda v: isinstance(v, property)): + + value = model_dict.get(attr) + + # expressions are expected to be None + if value is None and attr not in expression_names: + warnings.warn(f"Attribute {attr} not in dictionary.") + else: + try: + # check whether value is not the empty list and not a CasADi symbol/expression + if not (isinstance(value, list) and not value) and not attr in expression_names: + setattr(model, attr, value) + except Exception as e: + Exception("Failed to load attribute {attr} from dictionary:\n" + repr(e)) + + model.deserialize(model_dict['serialized_expressions'], model_dict['expression_names']) + + return model diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index 9282eb8528..f7034126dd 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -356,8 +356,10 @@ def to_dict(self) -> dict: ocp_dict[key]=dict(getattr(self, key).__dict__) if isinstance(v, list): for i, item in enumerate(v): - if isinstance(item, (AcadosModel, AcadosOcpDims, AcadosOcpConstraints, AcadosOcpCost)): + if isinstance(item, (AcadosOcpDims, AcadosOcpConstraints, AcadosOcpCost)): ocp_dict[key][i] = format_class_dict(dict(item.__dict__)) + if isinstance(item, (AcadosModel,)): + ocp_dict[key][i] = item.to_dict() ocp_dict = format_class_dict(ocp_dict) @@ -370,6 +372,11 @@ def to_dict(self) -> dict: def dump_to_json(self) -> None: + """ + Dumps the OCP description in JSON format to AcadosOcp.json_file. + + NOTE: Please make sure to call `AcadosOcp.make_consistent()` before dumping to json. + """ ocp_nlp_dict = self.to_dict() with open(self.json_file, 'w') as f: json.dump(ocp_nlp_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 4e67807366..f22df2d080 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1573,9 +1573,9 @@ def to_dict(self) -> dict: # convert acados classes to dicts for key, v in ocp_dict.items(): - if isinstance(v, (AcadosModel, AcadosOcpDims, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpOptions, ZoroDescription)): + if isinstance(v, (AcadosOcpDims, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpOptions, ZoroDescription)): ocp_dict[key] = dict(getattr(self, key).__dict__) - if isinstance(v, AcadosOcpRosOptions): + if isinstance(v, (AcadosOcpRosOptions, AcadosModel)): ocp_dict[key] = v.to_dict() ocp_dict = format_class_dict(ocp_dict) @@ -1745,6 +1745,13 @@ def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, self.cost.Vx_0, self.cost.Vu_0, self.cost.Vz_0, yref_0, W_0) + self.cost.Vx_0 = np.zeros((0,0)) + self.cost.Vu_0 = np.zeros((0,0)) + self.cost.Vz_0 = np.zeros((0,0)) + self.cost.W_0 = np.zeros((0,0)) + self.model.cost_y_expr_0 = [] + self.cost.yref_0 = np.zeros((0,)) + elif self.cost.cost_type_0 == "NONLINEAR_LS": self.model.cost_expr_ext_cost_0 = \ self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_0, yref_0, W_0) @@ -1752,10 +1759,18 @@ def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess_0 = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_0, yref_0, W_0, self.model.x, self.model.u, self.model.z) + self.cost.W_0 = np.zeros((0,0)) + self.model.cost_y_expr_0 = [] + self.cost.yref_0 = np.zeros((0,)) + elif self.cost.cost_type_0 == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_0 = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0, self.model.cost_y_expr_0, yref_0) + self.model.cost_r_in_psi_expr_0 = [] + self.model.cost_psi_expr_0 = [] + self.model.cost_y_expr_0 = [] + self.cost.yref_0 = np.zeros((0,)) if self.cost.cost_type_0 is not None: self.cost.cost_type_0 = 'EXTERNAL' @@ -1795,17 +1810,33 @@ def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.S self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, self.cost.Vx, self.cost.Vu, self.cost.Vz, yref, W) + self.cost.Vx = np.zeros((0,0)) + self.cost.Vu = np.zeros((0,0)) + self.cost.Vz = np.zeros((0,0)) + self.cost.W = np.zeros((0,0)) + self.model.cost_y_expr = [] + self.cost.yref = np.zeros((0,)) + elif self.cost.cost_type == "NONLINEAR_LS": self.model.cost_expr_ext_cost = \ self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, W) if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr, yref, W, self.model.x, self.model.u, self.model.z) + self.cost.W = np.zeros((0,0)) + self.model.cost_y_expr = [] + self.cost.yref = np.zeros((0,)) + elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr, self.model.cost_y_expr, yref) + self.model.cost_r_in_psi_expr = [] + self.model.cost_psi_expr = [] + self.model.cost_y_expr = [] + self.cost.yref = np.zeros((0,)) + self.cost.cost_type = 'EXTERNAL' @@ -1842,16 +1873,32 @@ def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, self.cost.Vx_e, None, None, yref_e, W_e) + + self.cost.Vx_e = np.zeros((0,0)) + self.cost.W_e = np.zeros((0,0)) + self.model.cost_y_expr_e = [] + self.cost.yref_e = np.zeros((0,)) + elif self.cost.cost_type_e == "NONLINEAR_LS": self.model.cost_expr_ext_cost_e = \ self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_e, yref_e, W_e) if cost_hessian == 'GAUSS_NEWTON': self.model.cost_expr_ext_cost_custom_hess_e = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_e, yref_e, W_e, self.model.x, [], self.model.z) + self.cost.W_e = np.zeros((0,0)) + self.model.cost_y_expr_e = [] + self.cost.yref_e = np.zeros((0,)) + elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": self.model.cost_expr_ext_cost_e = \ self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e, self.model.cost_y_expr_e, yref_e) + + self.model.cost_r_in_psi_expr_e = [] + self.model.cost_psi_expr_e = [] + self.model.cost_y_expr_e = [] + self.cost.yref_e = np.zeros((0,)) + self.cost.cost_type_e = 'EXTERNAL' diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 868f1969ea..efa182eb4e 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -411,9 +411,9 @@ def to_dict(self) -> dict: # convert acados classes to dicts for key, v in sim_dict.items(): # skip non dict attributes - if isinstance(v, (AcadosSim, AcadosSimDims, AcadosSimOptions, AcadosModel)): - sim_dict[key]=dict(getattr(self, key).__dict__) - if isinstance(v, AcadosSimRosOptions): + if isinstance(v, (AcadosSim, AcadosSimDims, AcadosSimOptions)): + sim_dict[key] = dict(getattr(self, key).__dict__) + if isinstance(v, (AcadosModel, AcadosSimRosOptions)): sim_dict[key] = v.to_dict() return format_class_dict(sim_dict) diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index ffe77a392b..ca8a29211f 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -360,22 +360,15 @@ def casadi_expr_to_string(expr) -> str: string += f"{expr[ii,:]}\n" return string -## Conversion functions def make_object_json_dumpable(input): + ''' + Convert numpy arrays and CasADi DM objects to lists before JSON dump. + NOTE: Serialization of CasADi MX and SX objects requires a StringSerializer and is now implemented in AcadosModel. + ''' if isinstance(input, (np.ndarray)): return input.tolist() - elif isinstance(input, (SX)): - try: - return input.serialize() - # for more readable json output: - # return casadi_expr_to_string(input) - except: # for older CasADi versions - return '' - elif isinstance(input, (MX)): - # NOTE: MX expressions can not be serialized, only Functions. - return input.__str__() elif isinstance(input, (DM)): - return input.full() + return input.full().tolist() else: raise TypeError(f"Cannot make input of type {type(input)} dumpable.") From dca421d29ff9ea6a1097b4a855c70f9d1ced859e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 24 Sep 2025 16:25:32 +0200 Subject: [PATCH 146/164] Migrate Python tests from CMake to Github actions (#1643) Parallel execution of tests via CMake resulted in issues. Python tests were defined in multiple places. Now python test definition is in `.github/*.yml` files and parallel execution is done by splitting in multiple jobs. --- .../workflows/c_test_blasfeo_reference.yml | 3 +- .github/workflows/core_build.yml | 6 +- .github/workflows/ext_dep_off.yml | 3 +- .github/workflows/full_build.yml | 418 ++++++++++++++++- CMakeLists.txt | 3 +- docs/installation/index.md | 1 - interfaces/CMakeLists.txt | 420 ------------------ 7 files changed, 409 insertions(+), 445 deletions(-) diff --git a/.github/workflows/c_test_blasfeo_reference.yml b/.github/workflows/c_test_blasfeo_reference.yml index 5a485b71e9..49d3acdd70 100644 --- a/.github/workflows/c_test_blasfeo_reference.yml +++ b/.github/workflows/c_test_blasfeo_reference.yml @@ -12,7 +12,6 @@ on: env: BUILD_TYPE: Release ACADOS_UNIT_TESTS: ON - ACADOS_PYTHON: OFF ACADOS_OCTAVE: OFF ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON @@ -43,7 +42,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DCMAKE_POLICY_VERSION_MINIMUM=3.5 + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DCMAKE_POLICY_VERSION_MINIMUM=3.5 - name: Build & Install working-directory: ${{runner.workspace}}/build diff --git a/.github/workflows/core_build.yml b/.github/workflows/core_build.yml index d489e64af1..f10308c0d6 100644 --- a/.github/workflows/core_build.yml +++ b/.github/workflows/core_build.yml @@ -9,7 +9,6 @@ on: env: BUILD_TYPE: Release - ACADOS_PYTHON: ON ACADOS_OCTAVE: ON ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON @@ -35,6 +34,10 @@ jobs: id: vars run: echo "PREFIX=build-${{ github.run_id }}-${{ github.run_attempt }}" >> $GITHUB_OUTPUT + - name: Check versions + run: | + cmake --version + - name: Submodule fingerprint id: submods run: | @@ -72,7 +75,6 @@ jobs: -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP \ -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES \ -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP \ - -DACADOS_PYTHON=$ACADOS_PYTHON \ -DACADOS_OCTAVE=OFF \ -DACADOS_WITH_OPENMP=ON \ -DACADOS_NUM_THREADS=1 diff --git a/.github/workflows/ext_dep_off.yml b/.github/workflows/ext_dep_off.yml index 5c553df5a3..165167003f 100644 --- a/.github/workflows/ext_dep_off.yml +++ b/.github/workflows/ext_dep_off.yml @@ -12,7 +12,6 @@ on: env: BUILD_TYPE: Debug ACADOS_UNIT_TESTS: OFF - ACADOS_PYTHON: OFF ACADOS_OCTAVE: OFF ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON @@ -43,7 +42,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{runner.workspace}}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DEXT_DEP=OFF -DBLASFEO_EXAMPLES=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DEXT_DEP=OFF -DBLASFEO_EXAMPLES=OFF -DCMAKE_POLICY_VERSION_MINIMUM=3.5 - name: Build & Install working-directory: ${{runner.workspace}}/build diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index d0a40e694b..e48b84f84a 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -15,7 +15,7 @@ jobs: # ===== PYTHON TESTS ===== - python_interface: + python_basic_tests: needs: call_core_build runs-on: ubuntu-22.04 @@ -33,22 +33,408 @@ jobs: - name: Setup Python Environment uses: ./.github/actions/setup-python-environment - - name: Run CMake python tests (ctest) - working-directory: ${{ github.workspace }}/build + - name: Getting started examples + working-directory: ${{ github.workspace }}/examples/acados_python/getting_started shell: bash run: | source ${{ github.workspace }}/acadosenv/bin/activate - ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4; + python minimal_example_closed_loop.py + python minimal_example_sim.py + python minimal_example_ocp.py - - name: Rerun failed tests with verbose output - if: failure() - working-directory: ${{ github.workspace }}/build + - name: Getting started examples + working-directory: ${{ github.workspace }}/examples/acados_python/getting_started + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python minimal_example_closed_loop.py + python minimal_example_sim.py + python minimal_example_ocp.py + + - name: tests + working-directory: ${{ github.workspace }}/examples/acados_python/tests + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python reset_test.py + python static_lib_test.py + python test_cython_ctypes.py + python main_test.py + python test_detect_constraints.py + python test_cost_integration_euler.py + python test_cost_integration_value.py + python armijo_test.py + python pcond_getters_test.py + python test_nan_globalization.py + + - name: tests pt. 2 + working-directory: ${{ github.workspace }}/examples/acados_python/tests + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python soft_constraint_test.py + python test_parametric_nonlinear_constraint_h.py + python test_sim_dae.py + python sparse_param_test.py + python regularization_test.py + python one_sided_constraints_test.py + + - name: time varying examples + working-directory: ${{ github.workspace }}/examples/acados_python/time_varying + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python test_time_varying_irk.py + python test_polynomial_controls_and_penalties.py + python test_mocp_qp.py + + - name: pendulum MHE + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/mhe + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python minimal_example_mhe.py + python minimal_example_mhe_with_noisy_param.py + python minimal_example_mhe_with_param.py + python closed_loop_mhe_ocp.py + + + - name: chain + working-directory: ${{ github.workspace }}/examples/acados_python/chain_mass + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + python minimal_example_sim.py + + + py_ocp_tests: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: pendulum OCP + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/ocp + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python minimal_example_ocp_cmake.py + python nonuniform_discretization_example.py + python example_sqp_rti_loop.py + python simulink_example.py + python ocp_example_h_init_contraints.py + python slack_min_formulation.py + python minimal_example_ocp_reuse_code.py + python example_optimal_value_derivative.py + python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=IRK + python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=ERK --QP_SOLVER=PARTIAL_CONDENSING_QPDUNES + python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=GNSF + python time_optimal_swing_up.py + python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE DISCRETE --BUILD_SYSTEM cmake + + - name: AcadosCasadi + working-directory: ${{ github.workspace }}/examples/acados_python/casadi_tests + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python test_casadi_get_set.py + python test_casadi_parametric.py + python test_casadi_p_in_constraint_and_cost.py + python test_casadi_constraint.py + python test_casadi_slack_in_h.py + python test_casadi_single_shooting.py + python test_casadi_LICQ_violation.py + python test_casadi_closed_loop.py + + py_non_ocp: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: dense NLPs + working-directory: ${{ github.workspace }}/examples/acados_python/non_ocp_nlp + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python maratos_test_problem.py + python adaptive_eps_reg_test.py + python qpscaling_test.py + + + py_real_examples: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: pmsm + working-directory: ${{ github.workspace }}/examples/acados_python/pmsm_example + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + + - name: rsm + working-directory: ${{ github.workspace }}/examples/acados_python/rsm_example + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + + - name: quadrotor_nav + working-directory: ${{ github.workspace }}/examples/acados_python/quadrotor_nav + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + + - name: race_cars + working-directory: ${{ github.workspace }}/examples/acados_python/race_cars + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + + py_ddp_and_globalization: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: DDP + working-directory: ${{ github.workspace }}/examples/acados_python/unconstrained_ocps/linear_dynamics_qp_ocp + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python acados_unconstrained_QP.py + + - name: DDP Chen Allgöwer + working-directory: ${{ github.workspace }}/examples/acados_python/unconstrained_ocps/chen_allgoewer_unconstrained_ocp + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python chen_allgoewer_ocp.py + + - name: Hour Glass P2P Motion + working-directory: ${{ github.workspace }}/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python hour_glass_time_optimal_p2p_motion.py + + - name: DDP Rockit Hello World + working-directory: ${{ github.workspace }}/examples/acados_python/unconstrained_ocps/rockit_hello_world + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python rockit_hello_world_ocp.py + + # globalization + - name: Hock-Schittkowsky + working-directory: ${{ github.workspace }}/examples/acados_python/hock_schittkowsky + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python hs015_test.py + python hs016_test.py + python hs074_constraint_scaling.py + python hs099.py + + + python_cmake_tests: + needs: call_core_build + runs-on: ubuntu-22.04 + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: 'recursive' + + - name: Setup Test Environment + uses: ./.github/actions/setup-test-environment + with: + artifact-name-prefix: ${{ needs.call_core_build.outputs.artifact-name-prefix }} + + - name: Setup Python Environment + uses: ./.github/actions/setup-python-environment + + - name: python_custom_update_example + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/custom_update + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python example_custom_rti_loop.py + + - name: python_as_rti_example + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/as_rti + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python as_rti_closed_loop_example.py + + - name: python_fast_zoro_example + working-directory: ${{ github.workspace }}/examples/acados_python/zoRO_example + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python pendulum_on_cart/minimal_example_zoro.py + + - name: python_zoro_diff_drive_example + working-directory: ${{ github.workspace }}/examples/acados_python/zoRO_example + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python diff_drive/main.py + + - name: python_convex_ocp_with_onesided_constraints + working-directory: ${{ github.workspace }}/examples/acados_python/convex_ocp_with_onesided_constraints + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main_convex_onesided.py + + - name: python_pendulum_ext_sim_example + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart/sim + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python extensive_example_sim.py + + - name: cython_pendulum_closed_loop_example + working-directory: ${{ github.workspace }}/examples/acados_python/pendulum_on_cart + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python cython_example_closed_loop.py + + - name: py_time_optimal_cython_ctypes + working-directory: ${{ github.workspace }}/examples/acados_python/crane + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python time_optimal_example.py + + - name: python_test_generic_impl_dyn + working-directory: ${{ github.workspace }}/examples/acados_python/generic_impl_dyn + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python minimal_example_ocp_generic_impl_dyn.py + + - name: python_test_reset_timing + working-directory: ${{ github.workspace }}/examples/acados_python/timing_example + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python reset_timing.py + + - name: py_sqp_wfqp_linear_problem_obstacle_avoidance + working-directory: ${{ github.workspace }}/examples/acados_python/linear_mass_model + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python sqp_wfqp_test.py + + - name: py_sqp_wfqp_inconsistent_linearization + working-directory: ${{ github.workspace }}/examples/acados_python/inconsistent_qp_linearization + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python inconsistent_qp_linearization_test.py + + - name: python_convex_test_problem_globalization + working-directory: ${{ github.workspace }}/examples/acados_python/convex_problem_globalization_needed + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python convex_problem_globalization_necessary.py + + - name: python_max_iter_termination_test + working-directory: ${{ github.workspace }}/examples/acados_python/max_iter_test + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python max_iter_test.py + + - name: python_OCP_maratos_test_problem_globalization + working-directory: ${{ github.workspace }}/examples/acados_python/linear_mass_model + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python linear_mass_test_problem.py + + - name: py_qpscaling_slacked + working-directory: ${{ github.workspace }}/examples/acados_python/linear_mass_model + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python test_qpscaling_slacked.py + + - name: python_multiphase_nonlinear_constraints + working-directory: ${{ github.workspace }}/examples/acados_python/multiphase_nonlinear_constraints + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python main.py + + - name: python_OSQP_test + if: env.ACADOS_WITH_OSQP == 'ON' + working-directory: ${{ github.workspace }}/examples/acados_python/tests shell: bash run: | - echo "--- CTest failed. Re-running all failing tests with verbose output: ---" source ${{ github.workspace }}/acadosenv/bin/activate - ctest -C Release --rerun-failed -V - exit 1 + python test_osqp.py python_interface_new_casadi_and_py2octave: @@ -94,7 +480,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF - name: Export Paths for octave working-directory: ${{ github.workspace }} @@ -217,7 +603,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -280,7 +666,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -345,7 +731,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -404,7 +790,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF - name: Run Simulink closed-loop test uses: matlab-actions/run-command@v2 @@ -481,7 +867,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{ github.workspace }}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=$ACADOS_OCTAVE - name: Run CMake Octave tests (ctest) working-directory: ${{ github.workspace }}/build diff --git a/CMakeLists.txt b/CMakeLists.txt index 9cc0969016..e410d432e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,7 +92,6 @@ option(ACADOS_WITH_QPDUNES "qpDUNES solver" OFF) option(ACADOS_WITH_OSQP "OSQP solver" OFF) # Interfaces option(ACADOS_OCTAVE "Octave Interface tests" OFF) -option(ACADOS_PYTHON "Python Interface tests" OFF) # Options to use libraries found via find_package option(ACADOS_WITH_SYSTEM_BLASFEO "If ON, use blasfeo found via find_package(blasfeo) instead of compiling it" OFF) mark_as_advanced(ACADOS_WITH_SYSTEM_BLASFEO) @@ -266,7 +265,7 @@ if(ACADOS_LINT) endif() endif() -if(ACADOS_OCTAVE OR ACADOS_PYTHON) +if(ACADOS_OCTAVE) add_subdirectory(${PROJECT_SOURCE_DIR}/interfaces) endif() diff --git a/docs/installation/index.md b/docs/installation/index.md index 497842dd9c..1cc3c511ce 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -55,7 +55,6 @@ Adjust these options based on your requirements. | `ACADOS_UNIT_TESTS` | Compile unit tests | `OFF` | | `ACADOS_EXAMPLES` | Compile C examples | `OFF` | | `ACADOS_OCTAVE` | Octave interface CMake tests | `OFF` | -| `ACADOS_PYTHON` | Python interface CMake tests (Note: Python interface installation is independent of this) | `OFF` | | `BUILD_SHARED_LIBS` | Build shared libraries | `ON` (non-Windows)| diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 129f59bb80..da0d9d228b 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -29,7 +29,6 @@ # -# OCTAVE if(ACADOS_OCTAVE) # new minimal examples add_test(NAME octave_new_ocp @@ -136,422 +135,3 @@ if(ACADOS_OCTAVE) set_tests_properties(octave_test_OSQP PROPERTIES DEPENDS octave_test_qpDUNES) endif() endif() - - -### PYTHON ### -if(ACADOS_PYTHON) - # Minimal examples - add_test(NAME python_pendulum_closed_loop_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/getting_started - python minimal_example_closed_loop.py) - add_test(NAME python_pendulum_sim_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/getting_started - python minimal_example_sim.py) - add_test(NAME python_pendulum_ocp_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/getting_started - python minimal_example_ocp.py) - - add_test(NAME python_test_generic_impl_dyn - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/generic_impl_dyn - python minimal_example_ocp_generic_impl_dyn.py) - add_test(NAME python_test_reset - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python reset_test.py) - add_test(NAME python_test_static_lib - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python static_lib_test.py) - add_test(NAME python_test_reset_timing - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/timing_example - python reset_timing.py) - add_test(NAME python_test_cython_vs_ctypes - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_cython_ctypes.py) - add_test(NAME python_test_ocp - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python main_test.py) - add_test(NAME python_test_detect_constraints - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_detect_constraints.py) - - add_test(NAME python_test_cost_integration_euler - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_cost_integration_euler.py) - - add_test(NAME python_test_cost_integration_value - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_cost_integration_value.py) - - add_test(NAME python_pendulum_ocp_example_reuse_code - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python minimal_example_ocp_reuse_code.py) - - # add_test(NAME python_solution_sensitivities_and_exact_hess - # COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/solution_sensitivities - # python test_solution_sens_and_exact_hess.py) - - # add_test(NAME python_value_function_gradient - # COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/solution_sensitivities - # python value_gradient_example.py) - - # add_test(NAME python_policy_gradient - # COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/solution_sensitivities - # python policy_gradient_example.py) - - add_test(NAME python_time_varying_irk - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/time_varying - python test_time_varying_irk.py) - - add_test(NAME test_polynomial_controls_and_penalties - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/time_varying - python test_polynomial_controls_and_penalties.py) - - add_test(NAME py_mocp_qp_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/time_varying - python test_mocp_qp.py) - - -# Tests for DDP -add_test(NAME py_ddp_solve_qp - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/unconstrained_ocps/linear_dynamics_qp_ocp - python acados_unconstrained_QP.py) - -add_test(NAME py_ddp_chen_allgoewer - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/unconstrained_ocps/chen_allgoewer_unconstrained_ocp - python chen_allgoewer_ocp.py) - -add_test(NAME py_hour_glass_p2p_motion - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/unconstrained_ocps/hour_glass_p2p_motion - python hour_glass_time_optimal_p2p_motion.py) - -add_test(NAME py_ddp_rockit_hello_world - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/unconstrained_ocps/rockit_hello_world - python rockit_hello_world_ocp.py) - -# Tests for SQP_WITH_FEASIBLE_QP -add_test(NAME py_sqp_wfqp_linear_problem_obstacle_avoidance - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/linear_mass_model - python sqp_wfqp_test.py) - -add_test(NAME py_sqp_wfqp_inconsistent_linearization - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/inconsistent_qp_linearization - python inconsistent_qp_linearization_test.py) - -add_test(NAME py_sqp_wfqp_problem_hs015 - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky - python hs015_test.py) - -add_test(NAME py_sqp_wfqp_problem_hs016 - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky - python hs016_test.py) - -add_test(NAME py_sqp_wfqp_problem_hs074_constraint_scaling - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky - python hs074_constraint_scaling.py) - -add_test(NAME py_hs099_tol_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/hock_schittkowsky - python hs099.py) - - -# CMake test -add_test(NAME python_pendulum_ocp_example_cmake - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python minimal_example_ocp_cmake.py) - add_test(NAME python_nonuniform_discretization_ocp_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python nonuniform_discretization_example.py) - add_test(NAME python_rti_loop_ocp_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_sqp_rti_loop.py) - # Python Simulink - add_test(NAME python_render_simulink_wrapper - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python simulink_example.py) - add_test(NAME python_constraints_expression_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python ocp_example_h_init_contraints.py) - - add_test(NAME python_slack_min_formulation - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python slack_min_formulation.py) - - add_test(NAME python_chain_ocp - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/chain_mass - python main.py) - - add_test(NAME python_chain_sim - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/chain_mass - python minimal_example_sim.py) - - # Maratos test problem with different globalization options - add_test(NAME python_maratos_test_problem_globalization - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp - python maratos_test_problem.py) - - add_test(NAME python_test_adaptive_reg - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp - python adaptive_eps_reg_test.py) - - add_test(NAME py_qp_scaling_non_ocp - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/non_ocp_nlp - python qpscaling_test.py) - - # Convex test problem where full step SQP does not converge, but globalized SQP does - add_test(NAME python_convex_test_problem_globalization - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_problem_globalization_needed - python convex_problem_globalization_necessary.py) - - add_test(NAME python_max_iter_termination_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/max_iter_test - python max_iter_test.py) - - # Simple OCP with Maratos effect - add_test(NAME python_OCP_maratos_test_problem_globalization - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/linear_mass_model - python linear_mass_test_problem.py) - - add_test(NAME py_qpscaling_slacked - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/linear_mass_model - python test_qpscaling_slacked.py) - - # Armijo test problem - add_test(NAME python_armijo_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python armijo_test.py) - - add_test(NAME python_pcond_getters_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python pcond_getters_test.py) - - # Test NaN in globalization - add_test(NAME python_test_nan_globalization - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_nan_globalization.py) - - # Multiphase nonlinear constraint test problem - add_test(NAME python_multiphase_nonlinear_constraints - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/multiphase_nonlinear_constraints - python main.py) - - # OSQP test - if(ACADOS_WITH_OSQP) - add_test(NAME python_OSQP_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_osqp.py) - endif() - - # MHE examples - add_test(NAME python_pendulum_mhe_example_minimal - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/mhe - python minimal_example_mhe.py) - add_test(NAME python_pendulum_mhe_example_noisy_param - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/mhe - python minimal_example_mhe_with_noisy_param.py) - add_test(NAME python_pendulum_mhe_example_param - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/mhe - python minimal_example_mhe_with_param.py) - add_test(NAME python_pendulum_mhe_ocp_closed_loop_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/mhe - python closed_loop_mhe_ocp.py) - - add_test(NAME python_custom_update_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/custom_update - python example_custom_rti_loop.py) - - add_test(NAME python_as_rti_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/as_rti - python as_rti_closed_loop_example.py) - - add_test(NAME python_fast_zoro_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/zoRO_example - python pendulum_on_cart/minimal_example_zoro.py) - - add_test(NAME python_zoro_diff_drive_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/zoRO_example - python diff_drive/main.py) - - add_test(NAME python_convex_ocp_with_onesided_constraints - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_ocp_with_onesided_constraints - python main_convex_onesided.py) - - # casadi_examples - add_test(NAME python_casadi_get_set_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_get_set.py) - add_test(NAME python_test_casadi_parametric - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_parametric.py) - add_test(NAME python_test_casadi_p_in_constraint_and_cost - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_p_in_constraint_and_cost.py) - add_test(NAME python_test_casadi_constraint - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_constraint.py) - add_test(NAME python_test_casadi_slack_in_h - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_slack_in_h.py) - add_test(NAME python_test_casadi_single_shooting - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_single_shooting.py) - add_test(NAME python_test_casadi_LICQ_violation - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_LICQ_violation.py) - add_test(NAME python_test_casadi_closed_loop - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests - python test_casadi_closed_loop.py) - - # Sim - add_test(NAME python_pendulum_ext_sim_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/sim - python extensive_example_sim.py - ) - - add_test(NAME cython_pendulum_closed_loop_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart - python cython_example_closed_loop.py) - add_test(NAME py_time_optimal_cython_ctypes - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/crane - python time_optimal_example.py) - add_test(NAME pendulum_optimal_value_gradient - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_optimal_value_derivative.py) - - # example_ocp_dynamics_formulations all versions - add_test(NAME python_pendulum_ocp_IRK - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=IRK) - - if(ACADOS_WITH_QPDUNES) - add_test(NAME python_pendulum_ocp_ERK_qpDUNES - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=ERK --QP_SOLVER=PARTIAL_CONDENSING_QPDUNES) - endif() - - add_test(NAME python_pendulum_ocp_GNSF - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE=GNSF) - - add_test(NAME py_qp_scaling_time_opt_swingup - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python time_optimal_swing_up.py) - - # CMake and solver=DISCRETE test - add_test(NAME python_example_ocp_dynamics_formulations_cmake - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pendulum_on_cart/ocp - python example_ocp_dynamics_formulations.py --INTEGRATOR_TYPE DISCRETE --BUILD_SYSTEM cmake) - - add_test(NAME python_pendulum_soft_constraints_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python soft_constraint_test.py) - add_test(NAME python_pendulum_parametric_nonlinear_constraint_h_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_parametric_nonlinear_constraint_h.py) - add_test(NAME python_test_sim_dae - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python test_sim_dae.py) - add_test(NAME python_sparse_param_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python sparse_param_test.py) - add_test(NAME python_regularization_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python regularization_test.py) - add_test(NAME python_one_sided_constraints_test - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests - python one_sided_constraints_test.py) - - - add_test(NAME python_pmsm_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pmsm_example - python main.py) - add_test(NAME python_quadrotor_nav - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/quadrotor_nav - python main.py) - add_test(NAME python_race_cars - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/race_cars - python main.py) - add_test(NAME python_rsm_example - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/rsm_example - python main.py) - - # Force serial execution for all the below specified tests despite multiple CPU aviliablity (conflicting object file naming) - # TODO use unique object file names to allow full parallization, and remove forced serialization section below - # Directory zoRO_example - set_tests_properties(python_fast_zoro_example PROPERTIES DEPENDS python_zoro_diff_drive_example) - - # casadi_tests - set_tests_properties(python_casadi_get_set_example PROPERTIES DEPENDS python_test_casadi_p_in_constraint_and_cost) - set_tests_properties(python_test_casadi_p_in_constraint_and_cost PROPERTIES DEPENDS python_test_casadi_parametric) - set_tests_properties(python_test_casadi_constraint PROPERTIES DEPENDS python_casadi_get_set_example) - set_tests_properties(python_test_casadi_slack_in_h PROPERTIES DEPENDS python_test_casadi_constraint) - set_tests_properties(python_test_casadi_single_shooting PROPERTIES DEPENDS python_test_casadi_slack_in_h) - set_tests_properties(python_test_casadi_LICQ_violation PROPERTIES DEPENDS python_test_casadi_single_shooting) - set_tests_properties(python_test_casadi_closed_loop PROPERTIES DEPENDS python_test_casadi_LICQ_violation) - - # Directory getting_started - set_tests_properties(python_pendulum_sim_example PROPERTIES DEPENDS python_pendulum_ocp_example) - set_tests_properties(python_pendulum_closed_loop_example PROPERTIES DEPENDS python_pendulum_sim_example) - - # Directory non_ocp_nlp - set_tests_properties(python_maratos_test_problem_globalization PROPERTIES DEPENDS python_test_adaptive_reg) - set_tests_properties(python_test_adaptive_reg PROPERTIES DEPENDS py_qp_scaling_non_ocp) - - # Directory acados_python/tests - set_tests_properties(python_test_cython_vs_ctypes PROPERTIES DEPENDS python_test_reset) - set_tests_properties(python_test_reset PROPERTIES DEPENDS python_test_ocp) - set_tests_properties(python_test_ocp PROPERTIES DEPENDS python_test_cost_integration_euler) - set_tests_properties(python_test_cost_integration_euler PROPERTIES DEPENDS python_one_sided_constraints_test) - set_tests_properties(python_one_sided_constraints_test PROPERTIES DEPENDS python_test_cost_integration_value) - set_tests_properties(python_test_cost_integration_value PROPERTIES DEPENDS python_armijo_test) - set_tests_properties(python_armijo_test PROPERTIES DEPENDS python_pendulum_soft_constraints_example) - set_tests_properties(python_pendulum_soft_constraints_example PROPERTIES DEPENDS python_pendulum_parametric_nonlinear_constraint_h_test) - set_tests_properties(python_pendulum_parametric_nonlinear_constraint_h_test PROPERTIES DEPENDS python_test_nan_globalization) - set_tests_properties(python_test_nan_globalization PROPERTIES DEPENDS python_regularization_test) - set_tests_properties(python_regularization_test PROPERTIES DEPENDS python_test_sim_dae) - set_tests_properties(python_test_sim_dae PROPERTIES DEPENDS python_sparse_param_test) - set_tests_properties(python_sparse_param_test PROPERTIES DEPENDS python_test_detect_constraints) - set_tests_properties(python_test_detect_constraints PROPERTIES DEPENDS python_test_static_lib) - set_tests_properties(python_test_static_lib PROPERTIES DEPENDS python_pcond_getters_test) - - if(ACADOS_WITH_OSQP) - set_tests_properties(python_test_detect_constraints PROPERTIES DEPENDS python_OSQP_test) - endif() - - # Directory acados_python/chain_mass - set_tests_properties(python_chain_ocp PROPERTIES DEPENDS python_chain_sim) - - # Directory pendulum_on_cart/sim - set_tests_properties(python_pendulum_ext_sim_example PROPERTIES DEPENDS python_pendulum_sim_example_cmake) - - # Directory pendulum_on_cart/mhe - set_tests_properties(python_pendulum_mhe_example_minimal PROPERTIES DEPENDS python_pendulum_mhe_example_noisy_param) - set_tests_properties(python_pendulum_mhe_example_noisy_param PROPERTIES DEPENDS python_pendulum_mhe_example_param) - set_tests_properties(python_pendulum_mhe_example_param PROPERTIES DEPENDS python_pendulum_mhe_ocp_closed_loop_example) - - # # Directory pendulum_on_cart/solution_sensitivities - # set_tests_properties(python_solution_sensitivities_and_exact_hess PROPERTIES DEPENDS python_value_function_gradient) - # set_tests_properties(python_value_function_gradient PROPERTIES DEPENDS python_policy_gradient) - - # Directory time_varying - set_tests_properties(python_time_varying_irk PROPERTIES DEPENDS test_polynomial_controls_and_penalties) - set_tests_properties(test_polynomial_controls_and_penalties PROPERTIES DEPENDS py_mocp_qp_test) - - # Directory linear_mass_model - set_tests_properties(python_OCP_maratos_test_problem_globalization PROPERTIES DEPENDS py_qpscaling_slacked) - - # Directory pendulum_on_cart/ocp - set_tests_properties(python_pendulum_ocp_example_reuse_code PROPERTIES DEPENDS python_nonuniform_discretization_ocp_example) - set_tests_properties(python_nonuniform_discretization_ocp_example PROPERTIES DEPENDS python_rti_loop_ocp_example) - set_tests_properties(python_rti_loop_ocp_example PROPERTIES DEPENDS python_render_simulink_wrapper) - set_tests_properties(python_render_simulink_wrapper PROPERTIES DEPENDS python_constraints_expression_example) - set_tests_properties(python_constraints_expression_example PROPERTIES DEPENDS python_slack_min_formulation) - set_tests_properties(python_slack_min_formulation PROPERTIES DEPENDS pendulum_optimal_value_gradient) - set_tests_properties(pendulum_optimal_value_gradient PROPERTIES DEPENDS python_pendulum_ocp_IRK) - set_tests_properties(python_pendulum_ocp_IRK PROPERTIES DEPENDS python_pendulum_ocp_GNSF) - set_tests_properties(python_pendulum_ocp_GNSF PROPERTIES DEPENDS python_example_ocp_dynamics_formulations_cmake) - set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS py_qp_scaling_time_opt_swingup) - - if(ACADOS_WITH_QPDUNES) - set_tests_properties(python_example_ocp_dynamics_formulations_cmake PROPERTIES DEPENDS python_pendulum_ocp_ERK_qpDUNES) - endif() - -endif() From 434675f2e3c8e04b1aa1320e3d3dd6e4d46d365e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 24 Sep 2025 17:01:04 +0200 Subject: [PATCH 147/164] `AcadosOcpSolver.get_residuals()`: change default to `recompute=False`, fixes AS-RTI, breaking! (#1640) - add warning for AS-RTI, as recomputing residual breaks the reuse of the linearization which is essential for AS-RTI to work correctly. - test AS-RTI with `get_residuals()` --- .../acados_python/p_global_example/example_p_global.py | 10 +++++----- .../as_rti/as_rti_closed_loop_example.py | 3 +++ .../custom_update/example_custom_rti_loop.py | 2 +- .../pendulum_on_cart/ocp/example_sqp_rti_loop.py | 6 +++--- .../pendulum_on_cart/ros2/example_ros_minimal_ocp.py | 6 +++--- examples/acados_python/tests/test_rti_sqp_residuals.py | 2 +- .../acados_template/acados_ocp_solver.py | 9 ++++++++- .../acados_template/acados_ocp_solver_pyx.pyx | 2 +- 8 files changed, 25 insertions(+), 15 deletions(-) diff --git a/examples/acados_python/p_global_example/example_p_global.py b/examples/acados_python/p_global_example/example_p_global.py index 393f037f0c..6b6428fbd6 100644 --- a/examples/acados_python/p_global_example/example_p_global.py +++ b/examples/acados_python/p_global_example/example_p_global.py @@ -211,11 +211,11 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True, with_matla N_horizon = 20 # set options - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 - ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP + ocp.solver_options.nlp_solver_type = 'SQP_RTI' ocp.solver_options.ext_fun_compile_flags += ' -I' + ca.GlobalOptions.getCasadiIncludePath() + ' -ffast-math -march=native' if code_export_directory is not None: ocp.code_export_directory = code_export_directory @@ -249,7 +249,7 @@ def main(use_cython=False, lut=True, use_p_global=True, blazing=True, with_matla for i in range(20): status = ocp_solver.solve() # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - residuals+= list(ocp_solver.get_residuals()) + residuals+= list(ocp_solver.get_residuals(recompute=True)) timing += ocp_solver.get_stats("time_lin") # plot results @@ -315,8 +315,8 @@ def main_mocp(lut=True, use_p_global=True, with_matlab_templates=False): timing = 0 for i in range(20): status = ocp_solver.solve() - # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - residuals+= list(ocp_solver.get_residuals()) + # ocp_solver.print_statistics() + residuals+= list(ocp_solver.get_residuals(recompute=True)) timing += ocp_solver.get_stats('time_lin') return residuals, timing diff --git a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py index 5432e47551..691e1bf573 100644 --- a/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py +++ b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py @@ -177,6 +177,9 @@ def main(algorithm='RTI', as_rti_iter=1): t[i] = ocp_solver.get_stats('time_tot') + # test getting residuals + _ = ocp_solver.get_residuals() + if status not in [0, 2, 5]: raise Exception(f'acados returned status {status}. Exiting.') # simulate system diff --git a/examples/acados_python/pendulum_on_cart/custom_update/example_custom_rti_loop.py b/examples/acados_python/pendulum_on_cart/custom_update/example_custom_rti_loop.py index 82eb738621..a6c6b91d21 100644 --- a/examples/acados_python/pendulum_on_cart/custom_update/example_custom_rti_loop.py +++ b/examples/acados_python/pendulum_on_cart/custom_update/example_custom_rti_loop.py @@ -118,7 +118,7 @@ def main(use_cython=False): status = ocp_solver.solve() ocp_solver.custom_update(data) # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - residuals = ocp_solver.get_residuals() + residuals = ocp_solver.get_residuals(recompute=True) # print("residuals after ", i, "SQP_RTI iterations:\n", residuals) if max(residuals) < tol: break diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_sqp_rti_loop.py b/examples/acados_python/pendulum_on_cart/ocp/example_sqp_rti_loop.py index 005ece4858..a5677e9fca 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_sqp_rti_loop.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_sqp_rti_loop.py @@ -125,8 +125,8 @@ def main(): else: status = ocp_solver.solve() # ocp_solver.custom_update(np.array([])) - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - residuals = ocp_solver.get_residuals() + ocp_solver.print_statistics() + residuals = ocp_solver.get_residuals(recompute=True) print("residuals after ", i, "SQP_RTI iterations:\n", residuals) if max(residuals) < tol: break @@ -141,7 +141,7 @@ def main(): 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() cost = ocp_solver.get_cost() print("cost function value of solution = ", cost) diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py index dfd7c1e20d..847880358e 100644 --- a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py @@ -49,7 +49,7 @@ def create_minimal_ocp(export_dir: str, N: int = 20, Tf: float = 1.0, Fmax: floa # set model model = export_pendulum_ode_model() ocp.model = model - + nx = model.x.rows() nu = model.u.rows() ny = nx + nu @@ -114,7 +114,7 @@ def main(): Fmax = 80 Tf = 1.0 N = 20 - + export_dir = os.path.join(script_dir, 'generated_ocp') ocp = create_minimal_ocp(export_dir, N, Tf, Fmax) ocp_solver = AcadosOcpSolver(ocp, json_file = str(os.path.join(export_dir, 'acados_ocp.json'))) @@ -137,7 +137,7 @@ def main(): else: status = ocp_solver.solve() ocp_solver.print_statistics() - residuals = ocp_solver.get_residuals() + residuals = ocp_solver.get_residuals(recompute=True) print("residuals after ", i, "SQP_RTI iterations:\n", residuals) if max(residuals) < tol: break diff --git a/examples/acados_python/tests/test_rti_sqp_residuals.py b/examples/acados_python/tests/test_rti_sqp_residuals.py index 23d7b652db..e006d78df1 100644 --- a/examples/acados_python/tests/test_rti_sqp_residuals.py +++ b/examples/acados_python/tests/test_rti_sqp_residuals.py @@ -154,7 +154,7 @@ def main(nlp_solver_type="SQP"): assert res_next[2] == res_ineq_all[-1] assert res_next[3] == res_comp_all[-1] # overwrite res_next - res_next = solver.get_residuals() + res_next = solver.get_residuals(recompute=True) print(f"res_next: {res_next}") if max([res_stat_all[-1], res_eq_all[-1], res_ineq_all[-1], res_comp_all[-1]]) < TOL: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 4e1db67f77..b305f4a309 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -35,6 +35,7 @@ import shutil import sys import time +import warnings from ctypes import (POINTER, byref, c_char_p, c_double, c_int, c_bool, c_void_p, cast) @@ -1710,13 +1711,19 @@ def get_residuals(self, recompute=False): Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. The residuals has to be computed for SQP_RTI solver, since it is not available by default. + :param recompute: if True, recompute the residuals with respect to most recent problem data. Note: this can overwrite previous problem linearization in memory which are needed for AS-RTI to work properly! + - res_stat: stationarity residual - res_eq: residual wrt equality constraints (dynamics) - res_ineq: residual wrt inequality constraints (constraints) - res_comp: residual wrt complementarity conditions """ # compute residuals if RTI - if self.__solver_options['nlp_solver_type'] == 'SQP_RTI' or recompute: + if recompute: + if self.__solver_options['nlp_solver_type'] == 'SQP_RTI': + as_rti_level = self.options_get('as_rti_level') + if as_rti_level != 4: # not standard RTI + warnings.warn(f"Calling get_residuals() with recompute==True for AS-RTI can overwrite previous problem linearization in memory which are needed for AS-RTI to work properly!") self.__acados_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) # create output array diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index 7c159768a5..f281d07f71 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -636,7 +636,7 @@ cdef class AcadosOcpSolverCython: Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. """ # compute residuals if RTI - if self.nlp_solver_type == 'SQP_RTI' or recompute: + if recompute: acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) # create output array From 5d28e564470fd1d747a2ad8bce12090f8aba7958 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Tue, 30 Sep 2025 15:08:32 +0200 Subject: [PATCH 148/164] Minor fixes for ROS2 node template (#1634) @FreyJo Following your email, some propositions related to the generation of ROS2 nodes (see PR #1622): - Add env-hooks to add Acados libs to `LD_LIBRARY_PATH` automatically in ROS2 workspaces - Fix the setter for the period that is currently called twice at startup and has potentially undefined behaviors without verbose. For instance, if the parameter `Ts` is set first, the timer is started even if the solver is not yet ready. The proposed behavior is: ```bash [INFO] [1758106827.699133978] [pendulum_on_cart_ocp_node]: Initializing Pendulum On Cart Ocp Node... [INFO] [1758106827.701181092] [pendulum_on_cart_ocp_node]: acados solver initialized successfully. [INFO] [1758106827.701251389] [pendulum_on_cart_ocp_node]: update cost field 'W' mat(flat) = [2000, 0, 0, 0, 0, 2000, 0, 0, 0, 0, 0.02, 0, 0, 0, 0, 0.02] [INFO] [1758106827.701277944] [pendulum_on_cart_ocp_node]: update cost field 'W' mat(flat) = [2000, 0, 0, 0, 0, 0, 2000, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0, 0, 0.02] [INFO] [1758106827.701301180] [pendulum_on_cart_ocp_node]: update cost field 'W' mat(flat) = [2000, 0, 0, 0, 0, 0, 2000, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0, 0, 0.02] [INFO] [1758106827.701325255] [pendulum_on_cart_ocp_node]: Starting control loop with period 0.05s. [INFO] [1758106836.983087828] [pendulum_on_cart_ocp_node]: update period 'Ts' = 0.02s [WARN] [1758106836.983174809] [pendulum_on_cart_ocp_node]: Control timer already running, restarting... [INFO] [1758106836.983194966] [pendulum_on_cart_ocp_node]: Starting control loop with period 0.02s. [INFO] [1758106848.128546294] [pendulum_on_cart_ocp_node]: update period 'Ts' = -0.1s [WARN] [1758106848.128632952] [pendulum_on_cart_ocp_node]: Control period must be positive, defaulting to 0.02s. [WARN] [1758106848.128653340] [pendulum_on_cart_ocp_node]: Control timer already running, restarting... [INFO] [1758106848.128672411] [pendulum_on_cart_ocp_node]: Starting control loop with period 0.02s. ``` - Upon a period change (ROS2 parameter), also change the integration period (see commit [8439271](https://github.com/acados/acados/pull/1634/commits/8439271dc3431cf797b6535024b74cb4447a0baf)) On this last point, should it be done for the OCP solver also? --------- Co-authored-by: Jonathan Frey --- .../acados_template/acados_ocp.py | 6 ++ .../acados_template/acados_sim.py | 6 ++ .../ocp_node_templates/CMakeLists.in.txt | 3 + .../export_acados_path.sh.in | 1 + .../ocp_node_templates/node.in.cpp | 81 ++++++++++++------- .../ocp_node_templates/node.in.h | 12 ++- .../ocp_node_templates/test.launch.in.py | 10 +-- .../sim_node_templates/CMakeLists.in.txt | 4 + .../export_acados_path.sh.in | 1 + .../sim_node_templates/node.in.cpp | 44 ++++++++-- .../sim_node_templates/node.in.h | 5 ++ .../sim_node_templates/test.launch.in.py | 2 +- 12 files changed, 131 insertions(+), 44 deletions(-) create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/export_acados_path.sh.in create mode 100644 interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/export_acados_path.sh.in diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index f22df2d080..252b72b9f4 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1344,6 +1344,12 @@ def _get_ros_template_list(self) -> list: template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') template_list.append((template_file, 'node.cpp', src_dir, ros_template_glob)) + # Hooks + hooks_dir = os.path.join(package_dir, 'env-hooks') + template_file = os.path.join(ros_pkg_dir, 'export_acados_path.sh.in') + # Note: still a ".in" file, because it needs to be rendered by colcon + template_list.append((template_file, 'export_acados_path.sh.in', hooks_dir, ros_template_glob)) + # Test test_dir = os.path.join(package_dir, 'test') template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index efa182eb4e..4a2850c014 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -474,6 +474,12 @@ def _get_ros_template_list(self) -> list: template_file = os.path.join(ros_pkg_dir, 'node.in.cpp') template_list.append((template_file, 'node.cpp', src_dir, ros_template_glob)) + # Hooks + hooks_dir = os.path.join(package_dir, 'env-hooks') + template_file = os.path.join(ros_pkg_dir, 'export_acados_path.sh.in') + # Note: still a ".in" file, because it needs to be rendered by colcon build + template_list.append((template_file, 'export_acados_path.sh.in', hooks_dir, ros_template_glob)) + # Test test_dir = os.path.join(package_dir, 'test') template_file = os.path.join(ros_pkg_dir, 'test.launch.in.py') diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt index 064a4d6943..04a913acfb 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/CMakeLists.in.txt @@ -92,6 +92,9 @@ install(FILES DESTINATION lib ) +# --- EXPORT HOOKS --- +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/export_acados_path.sh.in") + # --- TESTS --- if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/export_acados_path.sh.in b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/export_acados_path.sh.in new file mode 100644 index 0000000000..d04e8b5cf6 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/export_acados_path.sh.in @@ -0,0 +1 @@ +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"@ACADOS_LIB_DIR@" diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp index 71ead0e863..f13503a71c 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp @@ -158,7 +158,7 @@ void {{ ClassName }}::control_loop() { p = current_p_; {%- endif %} } - + { {%- if use_multithreading %} std::lock_guard lock(solver_mutex_); @@ -318,7 +318,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- set suffix = "_NSPHI0" %} {%- endif %} {%- set constraint_size = model.name ~ suffix | upper %} - parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + parameter_handlers_["constraints.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", std::vector{0}); }; @@ -353,7 +353,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- set suffix = "_NSG" %} {%- endif %} {%- set constraint_size = model.name ~ suffix | upper %} - parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + parameter_handlers_["constraints.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { auto stages = range(1, {{ model.name | upper }}_N); this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", stages); @@ -385,7 +385,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- set suffix = "_NSGN" %} {%- endif %} {%- set constraint_size = model.name ~ suffix | upper %} - parameter_handlers_["{{ ros_opts.package_name }}.constraints.{{ field }}"] = + parameter_handlers_["constraints.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_constraint<{{ constraint_size }}>(p, res, "{{ field }}", std::vector{ {{- model.name | upper -}}_N}); }; @@ -395,20 +395,20 @@ void {{ ClassName }}::setup_parameter_handlers() { // Weights {%- if dims.ny_0 > 0 %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.W_0"] = + parameter_handlers_["cost.W_0"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_cost<{{ model.name | upper }}_NY0>(p, res, "W", std::vector{0}); }; {%- endif %} {%- if dims.ny > 0 %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.W"] = + parameter_handlers_["cost.W"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { auto stages = range(1, {{ model.name | upper }}_N); this->update_cost<{{ model.name | upper }}_NY>(p, res, "W", stages); }; {%- endif %} {%- if dims.ny_e > 0 %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.W_e"] = + parameter_handlers_["cost.W_e"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_cost<{{ model.name | upper }}_NYN>(p, res, "W", std::vector{ {{- model.name | upper -}}_N}); }; @@ -420,7 +420,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- for field, param in cost %} {%- set field_l = field | lower %} {%- if param and (field_l is starting_with('z')) and (field is ending_with('_0')) %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + parameter_handlers_["cost.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_cost<{{ model.name | upper }}_NS0>(p, res, "{{ field }}", std::vector{0}); }; @@ -433,7 +433,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- for field, param in cost %} {%- set field_l = field | lower %} {%- if param and (field_l is starting_with('z')) and (field is not ending_with('_0')) and (field is not ending_with('_e')) %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + parameter_handlers_["cost.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { auto stages = range(1, {{ model.name | upper }}_N); this->update_cost<{{ model.name | upper }}_NS>(p, res, "{{ field }}", stages); @@ -447,7 +447,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- for field, param in cost %} {%- set field_l = field | lower %} {%- if param and (field_l is starting_with('z')) and (field is ending_with('_e')) %} - parameter_handlers_["{{ ros_opts.package_name }}.cost.{{ field }}"] = + parameter_handlers_["cost.{{ field }}"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { this->update_cost<{{ model.name | upper }}_NSN>(p, res, "{{ field }}", std::vector{ {{- model.name | upper -}}_N}); }; @@ -457,7 +457,7 @@ void {{ ClassName }}::setup_parameter_handlers() { {%- endif %} // Solver Options - parameter_handlers_["{{ ros_opts.package_name }}.solver_options.print_level"] = + parameter_handlers_["solver_options.print_level"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult&) { {%- if use_multithreading %} std::lock_guard lock(solver_mutex_); @@ -467,9 +467,14 @@ void {{ ClassName }}::setup_parameter_handlers() { }; // Ros Configs - parameter_handlers_["{{ ros_opts.package_name }}.ts"] = + parameter_handlers_["ts"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult& res) { - this->config_.ts = p.as_double(); + this->set_period(p.as_double()); + // Restart timer with the new period + if (!this->is_running()) { + // Assumes that the node is not yet ready and that the timer will be started later + return; + } try { this->start_control_timer(this->config_.ts); } catch (const std::exception& e) { @@ -477,7 +482,7 @@ void {{ ClassName }}::setup_parameter_handlers() { res.successful = false; } }; - parameter_handlers_["{{ ros_opts.package_name }}.verbose"] = + parameter_handlers_["verbose"] = [this](const rclcpp::Parameter& p, rcl_interfaces::msg::SetParametersResult&) { this->config_.verbose = p.as_bool(); }; @@ -487,14 +492,14 @@ void {{ ClassName }}::declare_parameters() { // Constraints {%- for field, param in constraints %} {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and ('bx_0' not in field) %} - this->declare_parameter("{{ ros_opts.package_name }}.constraints.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); + this->declare_parameter("constraints.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); {%- endif %} {%- endfor %} // Weights {%- for field, param in cost %} {%- if param and (field is starting_with('W')) %} - this->declare_parameter("{{ ros_opts.package_name }}.cost.{{ field }}", std::vector{ + this->declare_parameter("cost.{{ field }}", std::vector{ {%- set n_diag = param | length -%} {%- for i in range(end=n_diag) -%} {{- param[i][i] -}} @@ -509,22 +514,22 @@ void {{ ClassName }}::declare_parameters() { {%- for field, param in cost %} {%- set field_l = field | lower %} {%- if param and (field_l is starting_with('z')) %} - this->declare_parameter("{{ ros_opts.package_name }}.cost.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); + this->declare_parameter("cost.{{ field }}", std::vector{ {{- param | join(sep=', ') -}} }); {%- endif %} {%- endfor %} {%- endif %} // Solver Options - this->declare_parameter("{{ ros_opts.package_name }}.solver_options.print_level", {{ 0 }}); + this->declare_parameter("solver_options.print_level", {{ 0 }}); // Ros Configs - this->declare_parameter("{{ ros_opts.package_name }}.ts", {{ solver_options.Tsim }}); - this->declare_parameter("{{ ros_opts.package_name }}.verbose", false); + this->declare_parameter("ts", {{ solver_options.Tsim }}); + this->declare_parameter("verbose", false); } void {{ ClassName }}::load_parameters() { - this->get_parameter("{{ ros_opts.package_name }}.ts", config_.ts); - this->get_parameter("{{ ros_opts.package_name }}.verbose", config_.verbose); + this->get_parameter("ts", config_.ts); + this->get_parameter("verbose", config_.verbose); } void {{ ClassName }}::apply_all_parameters_to_solver() { @@ -672,14 +677,32 @@ void {{ ClassName }}::update_cost( } } -// --- ROS Timer --- -void {{ ClassName }}::start_control_timer(double period_seconds) { - if (control_timer_) control_timer_->cancel(); - if (period_seconds <= 0.0) { - period_seconds = 0.02; - RCLCPP_WARN(this->get_logger(), "Non-positive control period specified. Using default: %f seconds.", period_seconds); +// --- Helpers --- +void {{ ClassName }}::set_period(double period_seconds) { + if (config_.ts == period_seconds) { + // Nothing to do + return; } + RCLCPP_INFO_STREAM( + this->get_logger(), "update period 'Ts' = " << period_seconds << "s"); + this->config_.ts = period_seconds; + // Check period validity + if (this->config_.ts <= 0.0) { + this->config_.ts = {{ solver_options.time_steps[0] }}; + RCLCPP_WARN(this->get_logger(), + "Control period must be positive, defaulting to {{ solver_options.time_steps[0] }}s, the first time step of the OCP."); + } +} +void {{ ClassName }}::start_control_timer(double period_seconds) { + this->set_period(period_seconds); + // if timer already exists, restart with new period + if (this->is_running()) { + RCLCPP_WARN(this->get_logger(), "Control timer already running, restarting..."); + control_timer_->cancel(); + } + RCLCPP_INFO_STREAM(this->get_logger(), "Starting control loop with period " << period_seconds << "s."); + // create timer auto period = std::chrono::duration_cast( std::chrono::duration(period_seconds)); control_timer_ = this->create_wall_timer( @@ -835,7 +858,7 @@ bool {{ ClassName }}::check_acados_status(const char* field, int stage, int stat // --- Main --- int main(int argc, char **argv) { rclcpp::init(argc, argv); - + // Suppress ROS timer logging rcutils_logging_set_logger_level("rcl", RCUTILS_LOG_SEVERITY_WARN); rcutils_logging_set_logger_level("rclcpp", RCUTILS_LOG_SEVERITY_WARN); diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h index 35b5aff8eb..2f3b2c6b87 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h @@ -140,8 +140,12 @@ class {{ ClassName }} : public rclcpp::Node { const char* field, const std::vector& stages); - // --- ROS Timer --- - void start_control_timer(double period_seconds); + // --- Helpers --- + void set_period(double period_seconds); + void start_control_timer(double period_seconds = {{ solver_options.time_steps[0] }}); + bool is_running() const { + return control_timer_ && !control_timer_->is_canceled(); + } // --- Acados Solver --- {%- if solver_options.nlp_solver_type == "SQP_RTI" %} @@ -176,8 +180,8 @@ class {{ ClassName }} : public rclcpp::Node { // --- Helpers --- bool check_acados_status( - const char* field, - int stage, + const char* field, + int stage, int status); }; diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py index 7319fad647..db2de2950e 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py @@ -64,7 +64,7 @@ def test_set_constraints(self, proc_info): """ {%- for field, param in constraints %} {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and ('bx_0' not in field) %} - param_name = "{{ ros_opts.package_name }}.constraints.{{ field }}" + param_name = "constraints.{{ field }}" expected_value = [{{- param | join(sep=', ') -}}] self.__check_parameter_set(param_name, expected_value) {%- endif %} @@ -77,7 +77,7 @@ def test_set_cost(self, proc_info): # --- Weights --- {%- for field, param in cost %} {%- if param and (field is starting_with('W')) %} - param_name = "{{ ros_opts.package_name }}.cost.{{ field }}" + param_name = "cost.{{ field }}" expected_value = [ {%- set n_diag = param | length -%} {%- for i in range(end=n_diag) -%} @@ -94,7 +94,7 @@ def test_set_cost(self, proc_info): {%- for field, param in cost %} {%- set field_l = field | lower %} {%- if param and (field_l is starting_with('z')) %} - param_name = "{{ ros_opts.package_name }}.cost.{{ field }}" + param_name = "cost.{{ field }}" expected_value = [{{- param | join(sep=', ') -}}] self.__check_parameter_set(param_name, expected_value) {%- endif %} @@ -106,7 +106,7 @@ def test_set_solver_options(self, proc_info): Test if solver options compile-time declared default parameters. """ # --- Solver Options --- - param_name = "{{ ros_opts.package_name }}.ts" + param_name = "ts" expected_value = {{ solver_options.Tsim }} self.__check_parameter_set(param_name, expected_value) @@ -117,7 +117,7 @@ def test_subscribing(self, proc_info): {%- if dims.np > 0 %} self.wait_for_subscription('{{ parameters_topic }}') {%- endif %} - + def test_publishing(self, proc_info): """Test if the node publishes to all expected topics.""" self.wait_for_publisher('{{ control_topic }}') diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt index 80bbcc426b..616fc1e6b1 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/CMakeLists.in.txt @@ -66,6 +66,10 @@ install(FILES DESTINATION lib ) +# --- EXPORT HOOKS --- +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/export_acados_path.sh.in") + + # --- TESTS --- if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/export_acados_path.sh.in b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/export_acados_path.sh.in new file mode 100644 index 0000000000..d04e8b5cf6 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/export_acados_path.sh.in @@ -0,0 +1 @@ +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"@ACADOS_LIB_DIR@" diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp index 5bbf36c149..d807a7a981 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.cpp @@ -116,9 +116,14 @@ void {{ ClassName }}::publish_state(const std::arrayconfig_.ts = p.as_double(); + this->set_integration_period(p.as_double()); + // Restart timer with the new period + if (!this->is_running()) { + // Assumes that the node is not yet ready and that the timer will be started later + return; + } try { this->start_integration_timer(this->config_.ts); } catch (const std::exception& e) { @@ -129,11 +134,11 @@ void {{ ClassName }}::setup_parameter_handlers() { } void {{ ClassName }}::declare_parameters() { - this->declare_parameter("{{ ros_opts.package_name }}.ts", {{ solver_options.Tsim }}); + this->declare_parameter("solver_options.Tsim", {{ solver_options.Tsim }}); } void {{ ClassName }}::load_parameters() { - this->get_parameter("{{ ros_opts.package_name }}.ts", config_.ts); + this->get_parameter("solver_options.Tsim", config_.ts); } rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( @@ -191,8 +196,34 @@ rcl_interfaces::msg::SetParametersResult {{ ClassName }}::on_parameter_update( // --- Helpers --- +void {{ ClassName }}::set_integration_period(double period_seconds) { + if (config_.ts == period_seconds) { + // Nothing to do + return; + } + RCLCPP_INFO_STREAM( + this->get_logger(), "update integration period 'Ts' = " << period_seconds << "s"); + this->config_.ts = period_seconds; + // Check period validity + if (this->config_.ts <= 0.0) { + this->config_.ts = 0.02; + RCLCPP_WARN(this->get_logger(), + "Integration period must be positive, defaulting to 0.02s."); + } + // Update sim solver integration time + this->set_t_sim(this->config_.ts); +} + void {{ ClassName }}::start_integration_timer(double period_seconds) { - if (period_seconds <= 0.0) period_seconds = 0.02; + this->set_integration_period(period_seconds); + // if timer already exists, restart with new period + if (this->is_running()) { + RCLCPP_WARN(this->get_logger(), "Integration timer already running, restarting..."); + integration_timer_->cancel(); + } + RCLCPP_INFO_STREAM(this->get_logger(), + "Starting integration loop with period " << period_seconds << "s."); + // create timer auto period = std::chrono::duration_cast( std::chrono::duration(period_seconds)); integration_timer_ = this->create_wall_timer( @@ -218,6 +249,9 @@ void {{ ClassName }}::set_u(double* u) { sim_in_set(sim_config_, sim_dims_, sim_in_, "u", u); } +void {{ ClassName }}::set_t_sim(double T_sim) { + sim_in_set(sim_config_, sim_dims_, sim_in_, "T", &T_sim); +} } // namespace {{ ros_opts.package_name }} diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h index 56ee7bb254..f39a1068a2 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/node.in.h @@ -79,12 +79,17 @@ class {{ ClassName }} : public rclcpp::Node { rcl_interfaces::msg::SetParametersResult on_parameter_update(const std::vector& params); // --- Helpers --- + void set_integration_period(double period_seconds); void start_integration_timer(double period_seconds = 0.02); + bool is_running() const { + return integration_timer_ && !integration_timer_->is_canceled(); + } // --- Acados Helpers --- int sim_solve(); void get_next_state(double* xn); void set_u(double* u); + void set_t_sim(double T_sim); }; } // namespace {{ ros_opts.package_name }} diff --git a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py index 750e786329..c5cf401337 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py +++ b/interfaces/acados_template/acados_template/ros2_templates/sim_node_templates/test.launch.in.py @@ -56,7 +56,7 @@ def test_parameters_set(self, proc_info): Test if all compile-time declared default parameters. """ # --- Solver Options --- - param_name = "{{ ros_opts.package_name }}.ts" + param_name = "solver_options.Tsim" expected_value = {{ solver_options.Tsim }} self.__check_parameter_set(param_name, expected_value) From 1a99e845b788573e616138c0eae5febf9fc5ecdd Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 1 Oct 2025 20:53:23 +0200 Subject: [PATCH 149/164] Disable suddendly failing test, see #1648 (#1649) --- .github/workflows/full_build_windows.yml | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/.github/workflows/full_build_windows.yml b/.github/workflows/full_build_windows.yml index 69176ad40b..551ac6eab6 100644 --- a/.github/workflows/full_build_windows.yml +++ b/.github/workflows/full_build_windows.yml @@ -111,17 +111,17 @@ jobs: cd test simulink_test - - name: Run MATLAB tests with new CasADi - uses: matlab-actions/run-command@v2 - if: always() - with: - command: | - cd ${{runner.workspace}}/acados/.github/windows - setup_mingw - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave - acados_env_variables_windows - cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/p_global_example - main + # - name: Run MATLAB tests with new CasADi + # uses: matlab-actions/run-command@v2 + # if: always() + # with: + # command: | + # cd ${{runner.workspace}}/acados/.github/windows + # setup_mingw + # cd ${{runner.workspace}}/acados/examples/acados_matlab_octave + # acados_env_variables_windows + # cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/p_global_example + # main - name: MATLAB idxs_rev example uses: matlab-actions/run-command@v2 From b04f362c5f016742c7aa17ccb4be0c211daf79b4 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Thu, 2 Oct 2025 13:39:35 +0200 Subject: [PATCH 150/164] Update `detect_cost_type` (#1647) A small follow-up on #1461, allowing for states defined as casadi.MX and adding checks for global parameters in the MATLAB interface. --------- Co-authored-by: sandmaennchen --- interfaces/acados_matlab_octave/detect_cost_type.m | 12 +++--------- .../acados_template/acados_template/acados_ocp.py | 4 ---- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/interfaces/acados_matlab_octave/detect_cost_type.m b/interfaces/acados_matlab_octave/detect_cost_type.m index 897eff58a7..23412916e9 100644 --- a/interfaces/acados_matlab_octave/detect_cost_type.m +++ b/interfaces/acados_matlab_octave/detect_cost_type.m @@ -39,13 +39,6 @@ function detect_cost_type(model, cost, dims, stage_type) z = model.z; p = model.p; - % check type - if isa(x, 'casadi.SX') - isSX = true; - else - error('constraint detection only works for casadi.SX!'); - end - nx = length(x); nu = length(u); nz = length(z); @@ -70,7 +63,8 @@ function detect_cost_type(model, cost, dims, stage_type) if expr_cost.is_quadratic(x) && expr_cost.is_quadratic(u) && expr_cost.is_quadratic(z) ... - && ~any(expr_cost.which_depends(p)) + && ~any(expr_cost.which_depends(p)) && ~any(expr_cost.which_depends(model.p_global)) ... + && ~any(expr_cost.which_depends(model.t)) if expr_cost.is_zero() fprintf('Cost function is zero -> Reformulating as LINEAR_LS cost.\n'); @@ -204,7 +198,7 @@ function detect_cost_type(model, cost, dims, stage_type) % elseif % TODO: can nonLINEAR_LS be detected?! else - fprintf('\n\nCost function is not quadratic -> Using external cost\n\n'); + fprintf('\n\nCost function is not quadratic or depends on parameters -> Using external cost\n\n'); if strcmp(stage_type, 'terminal') cost.cost_type_e = 'EXTERNAL'; elseif strcmp(stage_type, 'path') diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 252b72b9f4..07bbff409f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2261,10 +2261,6 @@ def detect_cost_type(self, model: AcadosModel, cost: AcadosOcpCost, dims: Acados z = model.z p = model.p - # Check type - if not isinstance(x, ca.SX): - raise ValueError("Cost type detection only works for casadi.SX!") - nx = casadi_length(x) nu = casadi_length(u) nz = casadi_length(z) From c0f68482cf5bb21930797b18a4a750d0a81a1f63 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 2 Oct 2025 17:31:50 +0200 Subject: [PATCH 151/164] Fixes related to `idxs_rev` in interfaces (#1650) --- .../multiphase_nonlinear_constraints/main.py | 29 +++++++++++++++---- interfaces/acados_matlab_octave/AcadosOcp.m | 4 +-- .../acados_matlab_octave/ns_from_idxs_rev.m | 11 +++++-- .../acados_template/acados_ocp.py | 4 +-- .../c_templates_tera/acados_multi_solver.in.c | 22 +++++++++----- .../c_templates_tera/acados_solver.in.c | 4 +-- .../acados_template/acados_template/utils.py | 7 ++++- 7 files changed, 58 insertions(+), 23 deletions(-) diff --git a/examples/acados_python/multiphase_nonlinear_constraints/main.py b/examples/acados_python/multiphase_nonlinear_constraints/main.py index 801a53d8a5..76776b6df0 100644 --- a/examples/acados_python/multiphase_nonlinear_constraints/main.py +++ b/examples/acados_python/multiphase_nonlinear_constraints/main.py @@ -58,7 +58,7 @@ def export_double_integrator_model(dim_q, dt) -> AcadosModel: return model -def main(): +def main(soften_h=False, qp_solver='FULL_CONDENSING_QPOASES'): # Horizon definition N = 20 # Number of time intervals Tf = 1.0 # Duration @@ -143,15 +143,33 @@ def main(): ocp.constraints.idxbu = np.arange(nu) # Set nonlinear constraint sizes and bounds - ocp.dims.nh = h_expr.shape[0] ocp.constraints.lh = h_lb ocp.constraints.uh = h_ub + # soften h using idxs_rev + if soften_h: + ocp.constraints.idxs_rev = np.array([-1, 0]) + ocp.cost.zl = np.array([1.0]) + ocp.cost.zu = np.array([1.0]) + ocp.cost.Zl = np.array([1.0]) + ocp.cost.Zu = np.array([1.0]) + + # also at terminal node: + # copy constraint + ocp.model.con_h_expr_e = ocp.model.con_h_expr + ocp.constraints.lh_e = ocp.constraints.lh + ocp.constraints.uh_e = ocp.constraints.uh + # soften + ocp.constraints.idxs_rev_e = np.array([0]) + ocp.cost.zl_e = np.array([1.0]) + ocp.cost.zu_e = np.array([1.0]) + ocp.cost.Zl_e = np.array([1.0]) + ocp.cost.Zu_e = np.array([1.0]) + multiphase_ocp.set_phase(ocp, phase_idx) # Set options - multiphase_ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' - multiphase_ocp.solver_options.qp_solver_cond_N = N + multiphase_ocp.solver_options.qp_solver = qp_solver multiphase_ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' multiphase_ocp.solver_options.nlp_solver_type = 'SQP' multiphase_ocp.solver_options.tf = Tf @@ -163,8 +181,9 @@ def main(): ocp_solver = AcadosOcpSolver(multiphase_ocp, json_file = 'acados_ocp.json') status = ocp_solver.solve() - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") + ocp_solver.print_statistics() assert status == 0, f'acados returned status {status}' if __name__ == '__main__': main() + main(qp_solver="FULL_CONDENSING_HPIPM", soften_h=True) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index aac5659214..cd5dac930b 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -641,7 +641,7 @@ function make_consistent_slacks_initial(self) end if ~strcmp(wrong_field, '') - error(['Inconsistent size for field', wrong_field, ' with dimension ', num2str(dim),... + error(['Inconsistent size for field ', wrong_field, ' with dimension ', num2str(dim),... '. Detected ns_0 = ', num2str(ns_0), ' = nsbu + nsg + nsh_0 + nsphi_0.',... ' With nsg = ', num2str(nsg), ' nsh_0 = ', num2str(nsh_0), ', nsphi_0 = ', num2str(nsphi_0), '.']) end @@ -782,7 +782,7 @@ function make_consistent_slacks_terminal(self) end if ~strcmp(wrong_field, '') - error(['Inconsistent size for field', wrong_field, ' with dimension ', num2str(dim),... + error(['Inconsistent size for field ', wrong_field, ' with dimension ', num2str(dim),... '. Detected ns_e = ', num2str(ns_e), ' = nsbx_e + nsg_e + nsh_e + nsphi_e.',... ' With nsbx_e = ', num2str(nsbx_e), ' nsg_e = ', num2str(nsg_e),... ' nsh_e = ', num2str(nsh_e), ', nsphi_e = ', num2str(nsphi_e), '.']) diff --git a/interfaces/acados_matlab_octave/ns_from_idxs_rev.m b/interfaces/acados_matlab_octave/ns_from_idxs_rev.m index 5d93f5a7d1..9638889247 100644 --- a/interfaces/acados_matlab_octave/ns_from_idxs_rev.m +++ b/interfaces/acados_matlab_octave/ns_from_idxs_rev.m @@ -1,7 +1,12 @@ -function n = ns_from_idxs_rev(idxs_rev) +function ns = ns_from_idxs_rev(idxs_rev) if isempty(idxs_rev) - n = 0; + ns = 0; else - n = max(idxs_rev) + 1; + ns = max(idxs_rev) + 1; + for i=0:ns-1 + if ~ismember(i, idxs_rev) + error(['Detected ns = ', num2str(ns), ', but i = ', num2str(i), ' is not in idxs_rev = [', num2str(idxs_rev), '], the slack with index ', num2str(i), ' is thus not contained in the problem.']); + end + end end end diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 07bbff409f..44435fb80d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -165,7 +165,7 @@ def json_file(self): @json_file.setter def json_file(self, json_file): self.__json_file = json_file - + @property def ros_opts(self) -> Optional[AcadosOcpRosOptions]: """Options to configure ROS 2 nodes and topics.""" @@ -1460,7 +1460,7 @@ def dump_to_json(self) -> None: dir_name = os.path.dirname(self.json_file) if dir_name: os.makedirs(dir_name, exist_ok=True) - + with open(self.json_file, 'w') as f: json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) return 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 5e325f84ce..4ee15634d3 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 @@ -1158,9 +1158,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i idxs_rev_0[{{ i }}] = {{ constraints_0.idxs_rev_0[i] }}; {%- endfor %} - double* lus_0 = calloc(2*NS0, sizeof(double)); + double* lus_0 = calloc(2*{{ dims_0.ns_0 }}, sizeof(double)); double* ls_0 = lus_0; - double* us_0 = lus_0 + NS0; + double* us_0 = lus_0 + {{ dims_0.ns_0 }}; {%- for i in range(end=dims_0.ns_0) %} {%- if constraints_0.ls_0[i] != 0 %} ls_0[{{ i }}] = {{ constraints_0.ls_0[i] }}; @@ -1299,6 +1299,12 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i double* lsphi; double* usphi; + // slacks idxs_rev + int* idxs_rev; + double* lus; + double* ls; + double* us; + {%- for jj in range(end=n_phases) %}{# phases loop !#} /********************* @@ -1767,14 +1773,14 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {% set_global n_idxs_rev = constraints[jj].idxs_rev | length %} {% if n_idxs_rev > 0 %} {% set ni_no_s = phases_dims[jj].nbu + phases_dims[jj].nbx + phases_dims[jj].ng + phases_dims[jj].nh + phases_dims[jj].nphi %} - int* idxs_rev = malloc( {{ ni_no_s }} * sizeof(int)); + idxs_rev = malloc( {{ ni_no_s }} * sizeof(int)); {%- for i in range(end=ni_no_s) %} idxs_rev[{{ i }}] = {{ constraints[jj].idxs_rev[i] }}; {%- endfor %} - double* lus = calloc(2*NS0, sizeof(double)); - double* ls = lus; - double* us = lus + NS0; + lus = calloc(2*{{ phases_dims[jj].ns }}, sizeof(double)); + ls = lus; + us = lus + {{ phases_dims[jj].ns }}; {%- for i in range(end=phases_dims[jj].ns) %} {%- if constraints[jj].ls[i] != 0 %} ls[{{ i }}] = {{ constraints[jj].ls[i] }}; @@ -2122,9 +2128,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i idxs_rev_e[{{ i }}] = {{ constraints_e.idxs_rev_e[i] }}; {%- endfor %} - double* lus_e = calloc(2*NSN, sizeof(double)); + double* lus_e = calloc(2*{{ dims_e.ns_e }}, sizeof(double)); double* ls_e = lus_e; - double* us_e = lus_e + NSN; + double* us_e = lus_e + {{ dims_e.ns_e }}; {%- for i in range(end=dims_e.ns_e) %} {%- if constraints_e.ls_e[i] != 0 %} ls_e[{{ i }}] = {{ constraints_e.ls_e[i] }}; 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 fab3ebfe2f..903c9b84d7 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 @@ -1918,9 +1918,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu idxs_rev[{{ i }}] = {{ constraints.idxs_rev[i] }}; {%- endfor %} - double* lus = calloc(2*NS0, sizeof(double)); + double* lus = calloc(2*NS, sizeof(double)); double* ls = lus; - double* us = lus + NS0; + double* us = lus + NS; {%- for i in range(end=dims.ns) %} {%- if constraints.ls[i] != 0 %} ls[{{ i }}] = {{ constraints.ls[i] }}; diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index ca8a29211f..d5754f5d65 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -432,7 +432,12 @@ def J_to_idx_slack(J): def ns_from_idxs_rev(idxs_rev) -> int: if is_empty(idxs_rev): return 0 - return int(np.max(idxs_rev) + 1) + else: + ns = int(np.max(idxs_rev) + 1) + for i in range(ns): + if i not in idxs_rev: + raise ValueError(f"Detected ns = {ns}, but i is not in idxs_rev = {idxs_rev}, the slack with index {i} is thus not contained in the problem.") + return ns def check_if_nparray_and_flatten(val, name) -> np.ndarray: if not isinstance(val, np.ndarray): From c85bb50baf7d77421b166a5003e27d85f9783f6f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 6 Oct 2025 16:59:56 +0200 Subject: [PATCH 152/164] Improve installation instructions (#1651) - deprecate `make` - add note on common issue of forgotten `..` in cmake - add notes for `MacOS` and `openmp` --- docs/installation/index.md | 79 ++++++++++++++++++++++++++++---------- 1 file changed, 59 insertions(+), 20 deletions(-) diff --git a/docs/installation/index.md b/docs/installation/index.md index 1cc3c511ce..dde55ba444 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -1,9 +1,11 @@ # Installation ## Linux/Mac +Installation on Linux, Mac and Linux within Windows WSL is described here. +Additional notes on [MacOS installation](#notes-for-macos) ### Prerequisites -We assume you have: git, make, cmake installed on your system. +We assume you have: `git`, `make`, `cmake` installed on your system. ### Clone acados Clone acados and its submodules by running: @@ -13,12 +15,7 @@ cd acados git submodule update --recursive --init ``` -### Build and install `acados` -A CMake and a Makefile based build system is available in acados. -Note that only the `CMake` build system is tested using CI and is thus recommended. -Please choose one and proceed with the corresponding paragraph. - -#### **CMake** (recommended) +### Installation via **CMake** Install `acados` as follows: ``` mkdir -p build @@ -27,6 +24,9 @@ cmake -DACADOS_WITH_QPOASES=ON .. # add more optional arguments e.g. -DACADOS_WITH_DAQP=ON, a list of CMake options is provided below make install -j4 ``` +Notes: +- If you get an error: `Specify a source directory [...]`, you probably forgot the `..` part in the `cmake` command +- An alternative [`make` build system has been deprecated](#deprecated-make-build-system) #### CMake options: Below is a list of CMake options available for configuring the `acados` build. @@ -61,24 +61,12 @@ Adjust these options based on your requirements. For more details on specific options, refer to the comments in the `CMakeLists.txt` file. -#### **Make** (not recommended) -NOTE: This build system is not actively tested and might be removed in the future! It is strongly recommended to use the `CMake` build system. -Set the `BLASFEO_TARGET` in `/Makefile.rule`. -Since some of the `C` examples use `qpOASES`, also set `ACADOS_WITH_QPOASES = 1` in `/Makefile.rule`. -For a list of supported targets, we refer to https://github.com/giaf/blasfeo/blob/master/README.md . -Install `acados` as follows: -``` -make shared_library -export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/lib -make examples_c -make run_examples_c -``` -NOTE: On MacOS `DYLD_LIBRARY_PATH` should be used instead of `LD_LIBRARY_PATH`. ### Interfaces installation For the installation of Python/MATLAB/Octave interfaces, please refer to the [Interfaces](../interfaces/index.md) page. + ## Windows 10+ (WSL) ### Prerequisites @@ -197,3 +185,54 @@ cmake --build . -j10 --target INSTALL --config Release ``` - In MATLAB, run `mex -setup C` and select the same `MSVC` version. - Try a MATLAB example (see above). + + +## Notes for MacOS +#### Installing acados with OpenMP on macOS + +The standard `clang` compiler on macOS does not support OpenMP. +Here, we solve this problem by installing `gcc` and make it the default compiler for `acados`. +Using OpenMP allows for parallelization, which can significantly speed up computations, either within one OCP solution or for solving batches of OCPs or simulation problems. + +#### 1. Make `gcc` your default compiler for acados + +First, install `gcc` using Homebrew. If you do not have Homebrew installed, you can find instructions [here](https://brew.sh/). + +```bash +# Install gcc via Homebrew +brew install gcc +``` + +Next we link the `gcc` compiler to `cc` and `c++` so that acados uses `gcc` instead of the default `clang` compiler. + +```bash +brew link gcc +# you might need to force it +# brew link gcc --overwrite --force +``` + +#### 2. Set environment variables + +Set in your shell configuration file `.bashrc`, `zshrc`, etc. the following environment variables to point to the installed `gcc` compiler. +Adjust the version number (`gcc-15` and `g++-15`) if a different version is installed. +You also might need to check whether the path `/opt/homebrew/bin/` is correct for your Homebrew installation: + +```bash +export CC="/opt/homebrew/bin/gcc-15" +export CXX="/opt/homebrew/bin/g++-15" +``` + + +## Deprecated: **Make** build system +NOTE: This build system is not actively tested and might be removed in the future! It is strongly recommended to use the `CMake` build system. + +Set the `BLASFEO_TARGET` in `/Makefile.rule`. +Since some `C` examples use `qpOASES`, also set `ACADOS_WITH_QPOASES = 1` in `/Makefile.rule`. +Install `acados` as follows: +``` +make shared_library +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/lib +make examples_c +make run_examples_c +``` +NOTE: On MacOS `DYLD_LIBRARY_PATH` should be used instead of `LD_LIBRARY_PATH`. From 4b1eb73f1f84ecf4fb19feaf5a672a58a2c3c35a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 7 Oct 2025 14:55:46 +0200 Subject: [PATCH 153/164] Python: modify ROS files to be python 3.8 compatible (#1654) --- .../acados_template/ros2/mapping_node.py | 78 ++++++++++--------- .../acados_template/ros2/utils.py | 4 +- 2 files changed, 42 insertions(+), 40 deletions(-) diff --git a/interfaces/acados_template/acados_template/ros2/mapping_node.py b/interfaces/acados_template/acados_template/ros2/mapping_node.py index 774a4bd18e..ad59977d89 100644 --- a/interfaces/acados_template/acados_template/ros2/mapping_node.py +++ b/interfaces/acados_template/acados_template/ros2/mapping_node.py @@ -31,7 +31,7 @@ import os import re -from typing import Optional, Union, TYPE_CHECKING, Literal +from typing import Optional, Union, TYPE_CHECKING, Literal, List, Tuple from itertools import chain from casadi import SX, MX @@ -47,7 +47,7 @@ -def _parse_msg_type(msg_type: str) -> tuple[str, str]: +def _parse_msg_type(msg_type: str) -> Tuple[str, str]: msg_type = msg_type.strip() if "/" in msg_type: # "pkg/Type" @@ -76,7 +76,7 @@ def __init__( ftype: str, is_array: bool = False, array_size: Optional[int] = None, - children: list['RosField'] | None = None + children: Optional[List['RosField']] = None ): if not isinstance(name, str): raise TypeError("RosField.name must be str") @@ -89,7 +89,7 @@ def __init__( if children is None: children = [] if not isinstance(children, list) or not all(isinstance(ch, RosField) for ch in children): - raise TypeError("RosField.children must be list[RosField]") + raise TypeError("RosField.children must be List[RosField]") self.name = name self.ftype = ftype self.is_array = is_array @@ -109,31 +109,30 @@ def to_dict(self) -> dict: "children": [c.to_dict() for c in self.children], } - def flatten(self, field: Optional['RosField'] = None) -> list['RosField']: + def flatten(self, field: Optional['RosField'] = None) -> List['RosField']: new_name = self.name if field is None else f"{field.name}.{self.name}" field_copy = RosField(new_name, self.ftype, self.is_array, self.array_size, self.children) if not self.children: return [field_copy] - out: list['RosField'] = [] + out: List['RosField'] = [] for c in self.children: out.extend(c.flatten(field_copy)) return out @staticmethod def __get_cpp_type(ftype: str): - match ftype: - case "float64": - return "double" - case "float32": - return "float" - case "int8": - return "int8_t" - case "int16": - return "int16_t" - case "int32": - return "int32_t" + if ftype == "float64": + return "double" + elif ftype == "float32": + return "float" + elif ftype == "int8": + return "int8_t" + elif ftype == "int16": + return "int16_t" + elif ftype == "int32": + return "int32_t" if "/" in ftype or "::" in ftype: return _cpp_msg_type(ftype) @@ -145,8 +144,8 @@ class RosTopicMsg: def __init__(self): self.__topic_name: str = "" self.__msg_type: str = "" - self.__field_tree: list[RosField] = list() - self._flat_field_tree: list[RosField] = list() + self.__field_tree: List[RosField] = list() + self._flat_field_tree: List[RosField] = list() @property def topic_name(self) -> str: @@ -157,7 +156,7 @@ def msg_type(self) -> str: return self.__msg_type @property - def field_tree(self) -> list[RosField]: + def field_tree(self) -> List[RosField]: return self.__field_tree @topic_name.setter @@ -175,7 +174,7 @@ def msg_type(self, value: str): self.__msg_type = value @field_tree.setter - def field_tree(self, value: list[RosField]): + def field_tree(self, value: List[RosField]): if not isinstance(value, list) or not all(isinstance(item, RosField) for item in value): raise TypeError('Invalid field_tree value, expected list of RosField.\n') self.__field_tree = value @@ -195,12 +194,12 @@ def to_dict(self) -> dict: class RosTopicMsgOutput(RosTopicMsg): def __init__(self): super().__init__() - self.__mapping: list[dict] = list() + self.__mapping: List[dict] = list() self.__exec_topic: str = "" self._needs_publish_lock: bool = False @property - def mapping(self) -> list[dict]: + def mapping(self) -> List[dict]: return self.__mapping @property @@ -208,7 +207,7 @@ def exec_topic(self) -> str: return self.__exec_topic @mapping.setter - def mapping(self, value: list[tuple[str, str]]): + def mapping(self, value: List[Tuple[str, str]]): if not isinstance(value, list) or not all(isinstance(item, tuple) and len(item) == 2 and all(isinstance(i, str) for i in item) for item in value): raise TypeError('Invalid mapping value, expected list of tuples (str, str).\n') self.__mapping = [self.__parse_mapping_pair(src, dest) for src, dest in value] @@ -220,10 +219,13 @@ def exec_topic(self, value: str): self.__exec_topic = value def to_dict(self) -> dict: - return super().to_dict() | { + d = super().to_dict() + d.update({ "mapping": self.mapping, "exec_topic": self.exec_topic, - "needs_publish_lock": self._needs_publish_lock} + "needs_publish_lock": self._needs_publish_lock + }) + return d @classmethod def from_msg( @@ -287,19 +289,19 @@ def __init__(self): self.__header_includes: set[str] = set() self.__dependencies: set[str] = set() - self.__in_msgs: list[RosTopicMsg] = [] - self.__out_msgs: list[RosTopicMsgOutput] = [] + self.__in_msgs: List[RosTopicMsg] = [] + self.__out_msgs: List[RosTopicMsgOutput] = [] self.__ocp_json_file: str = "" self.__sim_json_file: str = "" self.__mapper_json_file = "ros_mapper.json" @property - def in_msgs(self) -> list[RosTopicMsg]: + def in_msgs(self) -> List[RosTopicMsg]: return self.__in_msgs @property - def out_msgs(self) -> list[RosTopicMsgOutput]: + def out_msgs(self) -> List[RosTopicMsgOutput]: return self.__out_msgs @property @@ -315,13 +317,13 @@ def mapper_json_file(self) -> str: return self.__mapper_json_file @in_msgs.setter - def in_msgs(self, value: list[RosTopicMsg]): + def in_msgs(self, value: List[RosTopicMsg]): if not isinstance(value, list) or not all(isinstance(item, RosTopicMsg) for item in value): raise TypeError('Invalid in_msg value, expected list of RosTopicMsg.\n') self.__in_msgs = value @out_msgs.setter - def out_msgs(self, value: list[RosTopicMsgOutput]): + def out_msgs(self, value: List[RosTopicMsgOutput]): if not isinstance(value, list ) or not all(isinstance(item, RosTopicMsgOutput) for item in value): raise TypeError('Invalid out_msg value, expected list of RosTopicMsgOutput.\n') self.__out_msgs = value @@ -461,7 +463,7 @@ def render_templates(self): def check_none_values(self): - non_values: list[str] = [] + non_values: List[str] = [] if not self.in_msgs: non_values.append(f"input") @@ -615,7 +617,7 @@ def _size(sym: Union[SX, MX]) -> int: return int(sym.numel()) -def _elem_names(sym: Union[SX, MX]) -> list[str]: +def _elem_names(sym: Union[SX, MX]) -> List[str]: n = _size(sym) return [str(sym[i].name()) for i in range(n)] @@ -623,15 +625,15 @@ def _elem_names(sym: Union[SX, MX]) -> list[str]: def _compute_mapping( src_topic: str, src_name: str, - src_labels: list[str], + src_labels: List[str], dst_name: str, - dst_labels: list[str] -) -> list[tuple[str, str]]: + dst_labels: List[str] +) -> List[Tuple[str, str]]: """Return mapping pairs [(f"{src_topic}.{src_name}[i]","{dst_name}[j]")].""" if src_labels == dst_labels: return [(f"{src_topic}.{src_name}", f"{dst_name}")] - pairs: list[tuple[str, str]] = [] + pairs: List[Tuple[str, str]] = [] # Label-based match for j, lbl in enumerate(dst_labels): diff --git a/interfaces/acados_template/acados_template/ros2/utils.py b/interfaces/acados_template/acados_template/ros2/utils.py index 979b3c4977..bb5089b398 100644 --- a/interfaces/acados_template/acados_template/ros2/utils.py +++ b/interfaces/acados_template/acados_template/ros2/utils.py @@ -1,7 +1,7 @@ import re from enum import Enum -from typing import Union +from typing import Union, Set class ArchType(str, Enum): @@ -12,7 +12,7 @@ class ArchType(str, Enum): class AcadosRosBaseOptions: - _NOT_IMPLEMENTED_ARCHTYPES: set[ArchType] = { + _NOT_IMPLEMENTED_ARCHTYPES: Set[ArchType] = { ArchType.LIFECYCLE_NODE, ArchType.ROS2_CONTROLLER, ArchType.NAV2_CONTROLLER} From 3062d6dd9409e2409069b48b69d02538b63a78f0 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 8 Oct 2025 11:34:06 +0200 Subject: [PATCH 154/164] Clarabel QP support (#1168) We welcome [Clarabel](https://clarabel.org) as the latest member of OCP-QP solvers in acados :tada: :hugs: :cow: ### How to use it 1) compile acados with the CMake option `-DACADOS_WITH_CLARABLE=ON`. 2) set `solver_options.qp_solver = 'PARTIAL_CONDENSING_CLARABEL'` ### Details - Clarabel is a sparse solver, but the blocks of the OCP are inserted as dense blocks in the Clarabel interface. Just as in the OSQP wrapper. - Clarabel is written in Rust and has an official [C++ wrapper,](https://github.com/oxfordcontrol/Clarabel.cpp) which has been forked with minor changes to make the `acados` wrapper work. --------- Co-authored-by: giaf Co-authored-by: Anton Edvinovich Pozharskiy --- .github/workflows/core_build.yml | 7 + .github/workflows/full_build.yml | 22 +- .gitmodules | 3 + CMakeLists.txt | 7 +- acados/CMakeLists.txt | 14 +- acados/ocp_qp/ocp_qp_clarabel.c | 1254 +++++++++++++++++ acados/ocp_qp/ocp_qp_clarabel.h | 115 ++ cmake/.gitignore | 1 + cmake/acadosConfig.cmake.in | 5 + docs/installation/index.md | 1 + .../tests/{test_osqp.py => test_qp_solver.py} | 11 +- .../mass_spring_example.c | 17 +- external/CMakeLists.txt | 10 + external/Clarabel.cpp | 1 + interfaces/acados_c/ocp_qp_interface.c | 14 +- interfaces/acados_c/ocp_qp_interface.h | 6 + interfaces/acados_matlab_octave/ocp_get.c | 4 + .../acados_template/acados_ocp_options.py | 4 +- .../c_templates_tera/CMakeLists.in.txt | 7 +- .../c_templates_tera/Makefile.in | 10 +- .../matlab_templates/make_sfun.in.m | 3 + .../c_templates_tera/multi_Makefile.in | 3 + 22 files changed, 1498 insertions(+), 21 deletions(-) create mode 100644 acados/ocp_qp/ocp_qp_clarabel.c create mode 100644 acados/ocp_qp/ocp_qp_clarabel.h rename examples/acados_python/tests/{test_osqp.py => test_qp_solver.py} (90%) create mode 160000 external/Clarabel.cpp diff --git a/.github/workflows/core_build.yml b/.github/workflows/core_build.yml index f10308c0d6..92f0d7d3c9 100644 --- a/.github/workflows/core_build.yml +++ b/.github/workflows/core_build.yml @@ -14,6 +14,7 @@ env: ACADOS_WITH_QPOASES: ON ACADOS_WITH_DAQP: ON ACADOS_WITH_QPDUNES: ON + ACADOS_WITH_CLARABEL: ON ACADOS_ON_CI: ON @@ -62,9 +63,14 @@ jobs: 'CMakeLists.txt', 'cmake/**/*.cmake', 'acados/**', + '.github/workflows/core_build.yml', 'interfaces/acados_c/**' ) }} + - name: Install Eigen3, for clarabel + if: env.ACADOS_WITH_CLARABEL == 'ON' + run: sudo apt-get update && sudo apt-get install -y libeigen3-dev + - name: Create Build Environment & Configure if: steps.cache-build.outputs.cache-hit != 'true' run: | @@ -75,6 +81,7 @@ jobs: -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP \ -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES \ -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP \ + -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL \ -DACADOS_OCTAVE=OFF \ -DACADOS_WITH_OPENMP=ON \ -DACADOS_NUM_THREADS=1 diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index e48b84f84a..f562a17aa0 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -434,7 +434,15 @@ jobs: shell: bash run: | source ${{ github.workspace }}/acadosenv/bin/activate - python test_osqp.py + python test_qp_solver.py --qp_solver=PARTIAL_CONDENSING_OSQP + + - name: python_CLARABEL_test + if: env.ACADOS_WITH_CLARABEL == 'ON' + working-directory: ${{ github.workspace }}/examples/acados_python/tests + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python test_qp_solver.py --qp_solver=PARTIAL_CONDENSING_CLARABEL python_interface_new_casadi_and_py2octave: @@ -480,7 +488,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=OFF - name: Export Paths for octave working-directory: ${{ github.workspace }} @@ -603,7 +611,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -666,7 +674,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -731,7 +739,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=OFF - name: Configure MATLAB workspace shell: bash @@ -790,7 +798,7 @@ jobs: working-directory: ${{ github.workspace }}/build run: | cmake --version - cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=OFF + cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=OFF - name: Run Simulink closed-loop test uses: matlab-actions/run-command@v2 @@ -867,7 +875,7 @@ jobs: - name: Configure CMake shell: bash working-directory: ${{ github.workspace }}/build - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_OCTAVE=$ACADOS_OCTAVE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_WITH_CLARABEL=$ACADOS_WITH_CLARABEL -DACADOS_OCTAVE=$ACADOS_OCTAVE - name: Run CMake Octave tests (ctest) working-directory: ${{ github.workspace }}/build diff --git a/.gitmodules b/.gitmodules index d3dc11905a..6fcfa5fbaa 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,3 +39,6 @@ [submodule "external/daqp"] path = external/daqp url = https://github.com/darnstrom/daqp.git +[submodule "external/Clarabel.cpp"] + path = external/Clarabel.cpp + url = https://github.com/acados/Clarabel.cpp.git diff --git a/CMakeLists.txt b/CMakeLists.txt index e410d432e1..1e80528e18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,6 +90,7 @@ option(ACADOS_WITH_QORE "QORE solver" OFF) option(ACADOS_WITH_OOQP "OOQP solver" OFF) option(ACADOS_WITH_QPDUNES "qpDUNES solver" OFF) option(ACADOS_WITH_OSQP "OSQP solver" OFF) +option(ACADOS_WITH_CLARABEL "Clarabel solver" OFF) # Interfaces option(ACADOS_OCTAVE "Octave Interface tests" OFF) # Options to use libraries found via find_package @@ -223,6 +224,7 @@ if(CMAKE_BUILD_TYPE MATCHES WithExternalLibs) set(ACADOS_WITH_DAQP ON CACHE BOOL "Add DAQP solver") set(ACADOS_WITH_QPDUNES ON CACHE BOOL "Add qpDUNES solver") set(ACADOS_WITH_OSQP ON CACHE BOOL "Add OSQP solver") + set(ACADOS_WITH_CLARABEL ON CACHE BOOL "Add Clarabel solver") endif() ## External lib checks ( @@ -246,7 +248,6 @@ if(ACADOS_WITH_OOQP MATCHES ON) endif() if(ACADOS_WITH_QORE MATCHES ON) - if(CMAKE_SYSTEM_NAME MATCHES "dSpace" OR CMAKE_CXX_COMPILER_ID MATCHES "MSVC") set(ACADOS_WITH_QORE OFF CACHE BOOL "Add QORE solver" FORCE) message(WARNING "QORE is not compatible with MSVC or dSpace, QORE is has been disabled") @@ -338,6 +339,9 @@ endif() if(${ACADOS_WITH_HPMPC}) set(LINK_FLAG_HPMPC -lhpmpc) endif() +if(${ACADOS_WITH_CLARABEL}) + set(LINK_FLAG_CLARABEL -lclarabel_c) +endif() # if(${ACADOS_WITH_QORE}) # set(LINK_FLAG_QORE -lqore) # endif() @@ -351,6 +355,7 @@ file(APPEND lib/link_libs.json \t\"qpoases\":\ \"${LINK_FLAG_QPOASES}\",\n) file(APPEND lib/link_libs.json \t\"daqp\":\ \"${LINK_FLAG_DAQP}\",\n) file(APPEND lib/link_libs.json \t\"qpdunes\":\ \"${LINK_FLAG_QPDUNES}\",\n) file(APPEND lib/link_libs.json \t\"osqp\":\ \"${LINK_FLAG_OSQP}\",\n) +file(APPEND lib/link_libs.json \t\"clarabel\":\ \"${LINK_FLAG_CLARABEL}\",\n) file(APPEND lib/link_libs.json \t\"hpmpc\":\ \"${LINK_FLAG_HPMPC}\",\n) # file(APPEND lib/link_libs.json \t\"qore\":\ \"${LINK_FLAG_QORE}\",\n) file(APPEND lib/link_libs.json \t\"ooqp\":\ \"${LINK_FLAG_OOQP}\"\n) # no final comma! diff --git a/acados/CMakeLists.txt b/acados/CMakeLists.txt index 3b9bc85e6a..282f0d66ff 100644 --- a/acados/CMakeLists.txt +++ b/acados/CMakeLists.txt @@ -66,6 +66,10 @@ if(NOT ACADOS_WITH_OSQP) list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_osqp.c") endif() +if(NOT ACADOS_WITH_CLARABEL) + list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_clarabel.c") +endif() + # Define acados library add_library(acados ${ACADOS_SRC}) @@ -134,8 +138,13 @@ if(ACADOS_WITH_OSQP) target_link_libraries(acados PUBLIC osqp) target_compile_definitions(acados PUBLIC ACADOS_WITH_OSQP) - # TODO(dimitris): needed or not? - # target_compile_definitions(acados PUBLIC USE_ACADOS_TYPES) +endif() + +if(ACADOS_WITH_CLARABEL) + target_link_libraries(acados PUBLIC libclarabel_c_shared) + # target_link_libraries(acados INTERFACE libclarabel_c) + + target_compile_definitions(acados PUBLIC ACADOS_WITH_CLARABEL) endif() if(ACADOS_WITH_OOQP) @@ -187,6 +196,7 @@ configure_package_config_file(${PROJECT_SOURCE_DIR}/cmake/acadosConfig.cmake.in ACADOS_WITH_DAQP ACADOS_WITH_QPDUNES ACADOS_WITH_OSQP + ACADOS_WITH_CLARABEL ACADOS_WITH_OOQP) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/acadosConfig.cmake diff --git a/acados/ocp_qp/ocp_qp_clarabel.c b/acados/ocp_qp/ocp_qp_clarabel.c new file mode 100644 index 0000000000..8ed1051f85 --- /dev/null +++ b/acados/ocp_qp/ocp_qp_clarabel.c @@ -0,0 +1,1254 @@ +/* + * 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.; + */ + + +#include +#include + +// clarabel +#include "Clarabel.cpp/include/clarabel.h" + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_clarabel.h" +#include "acados/utils/mem.h" +#include "acados/utils/print.h" +#include "acados/utils/timing.h" +#include "acados/utils/types.h" + + +/************************************************ + * helper functions + ************************************************/ + +static void print_csc_as_dns(ClarabelCscMatrix *M) +{ + int i, j = 0; // Predefine row index and column index + int idx; + + // Initialize matrix of zeros + double *A = (double *) calloc(M->m * M->n, sizeof(double)); + for (int ii=0; iim*M->n; ii++) + A[ii] = 1e30; + + // Allocate elements + for (idx = 0; idx < M->colptr[M->n]; idx++) + { + // Get row index i (starting from 1) + i = M->rowval[idx]; + + // Get column index j (increase if necessary) (starting from 1) + while (M->colptr[j + 1] <= idx) j++; + + // Assign values to A + A[j * (M->m) + i] = M->nzval[idx]; + } + + for (i = 0; i < M->m; i++) + { + for (j = 0; j < M->n; j++) + { + if (A[j * (M->m) + i]==1e30) + printf(" * "); + else + printf("%8.4f ", A[j * (M->m) + i]); + } + printf("\n"); + } + + free(A); +} + + +void print_csc_matrix(ClarabelCscMatrix *M, const char *name) +{ + int j, i, row_start, row_stop; + int k = 0; + + // Print name + printf("%s :\n", name); + + for (j = 0; j < M->n; j++) { + row_start = M->colptr[j]; + row_stop = M->colptr[j + 1]; + + if (row_start == row_stop) continue; + else { + for (i = row_start; i < row_stop; i++) { + printf("\t[%3u,%3u] = %.3g\n", (int)M->rowval[i], (int)j, M->nzval[k++]); + } + } + } +} + + +static int acados_clarabel_num_vars(ocp_qp_dims *dims) +{ + int n = 0; + + for (int ii = 0; ii <= dims->N; ii++) + { + n += dims->nx[ii] + dims->nu[ii] + 2*dims->ns[ii]; + } + + return n; +} + + + +static int acados_clarabel_num_constr(ocp_qp_dims *dims) +{ + int m = 0; + + //printf("\ndims N %d\n", dims->N); + + for (int ii = 0; ii <= dims->N; ii++) + { + //printf("dims[%d] nx %d nu %d nb %d ng %d ns %d\n", ii, dims->nx[ii], dims->nu[ii], dims->nb[ii], dims->ng[ii], dims->ns[ii]); + + m += 2 * dims->nb[ii]; + m += 2 * dims->ng[ii]; + m += 2 * dims->ns[ii]; + + // equalities + if (ii < dims->N) + { + m += dims->nx[ii+1]; + } + } + + return m; +} + + + +static int acados_clarabel_nnzmax_P(const ocp_qp_dims *dims) +{ + int nnz = 0; + + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + for (int ii = 0; ii <= dims->N; ii++) + { + nnz += (nu[ii]+nx[ii])*(nu[ii]+nx[ii]+1)/2; // triu(RSQ) + nnz += 2*ns[ii]; // Z + } + + return nnz; +} + + + +static int acados_clarabel_nnzmax_A(const ocp_qp_dims *dims) +{ + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int nnz = 0; + + for (int ii = 0; ii <= N; ii++) + { + // inequality constraints + nnz += 2*nb[ii]; // eye of box constraints + nnz += 2*ng[ii]*nx[ii]; // C + nnz += 2*ng[ii]*nu[ii]; // D + nnz += 2*(nb[ii]+ng[ii])*2*ns[ii]; // soft constraints at worst case, when idxs_rev encoding is used. Typically just 2*ns + nnz += 2*ns[ii]; // eye of slacks nonnegativity constraints + + // dynamics equality constraints + if (ii < dims->N) + { + nnz += nx[ii+1] * nx[ii]; // A + nnz += nx[ii+1] * nu[ii]; // B + nnz += nx[ii+1]; // eye + } + } + + return nnz; +} + + + +static void update_gradient(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int kk, nn = 0; + for (kk = 0; kk <= N; kk++) + { + blasfeo_unpack_dvec(nu[kk]+nx[kk]+2*ns[kk], in->rqz + kk, 0, &mem->q[nn], 1); + nn += nu[kk]+nx[kk]+2*ns[kk]; + } + + // actual number of nonzeros + mem->q_nnz = nn; +} + + + +static void update_hessian_structure(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int ii, jj, kk; + + // CSC format: P_rowval are row indices and P_col_ptr are column pointers + int nn = 0, offset = 0, col = 0; + for (kk = 0; kk <= N; kk++) + { + // write triu(RSQ[kk]) + for (jj = 0; jj < nx[kk] + nu[kk]; jj++) + { + mem->P_col_ptr[col] = nn; + col++; + + for (ii = 0; ii <= jj; ii++) + { + // we write only the upper triangular part + mem->P_rowval[nn] = offset + ii; + nn++; + } + } + offset += nx[kk] + nu[kk]; + + // write Z[kk] + for (jj = 0; jj < 2*ns[kk]; jj++) + { + mem->P_col_ptr[col] = nn; + col++; + + // diagonal + mem->P_rowval[nn] = offset + jj; + nn++; + } + + offset += 2*ns[kk]; + } + + mem->P_col_ptr[col] = nn; + // actual number of nonzeros + mem->P_nnz = nn; +} + + + +static void update_hessian_data(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int ii, kk; + + // Traversing the matrix in column-major order + int nn = 0; + for (kk = 0; kk <= N; kk++) + { + // writing RSQ[kk] + // we write the lower triangular part in row-major order + // that's the same as writing the upper triangular part in + // column-major order + for (ii = 0; ii < nx[kk] + nu[kk]; ii++) + { + blasfeo_unpack_dmat(1, ii+1, in->RSQrq+kk, ii, 0, mem->P_nzval+nn, 1); + nn += ii+1; + } + + // write Z[kk] + blasfeo_unpack_dvec(2*ns[kk], in->Z+kk, 0, mem->P_nzval+nn, 1); + nn += 2*ns[kk]; + } + + // TODO ? check that nn==mem->P_nnz +} + + + +static void update_constraints_matrix_structure(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, jj, kk; + + int row_offset_dyn = 0; + int row_offset_con = 0; + int row_offset_slk = 0; + + int con_start = 0; + int slk_start = 0; + for (kk = 0; kk <= N; kk++) + { + con_start += kk < N ? nx[kk+1] : 0; + slk_start += 2*nb[kk]+2*ng[kk]; + } + + slk_start += con_start; + + // setup cones type + int m = acados_clarabel_num_constr(dims); + int m_eq = con_start; + int m_ineq = m-m_eq; + mem->cones[0] = ClarabelZeroConeT(m_eq); + mem->cones[1] = ClarabelNonnegativeConeT(m_ineq); + + // CSC format: A_i are row indices and A_p are column pointers + int nn = 0, col = 0; + for (kk = 0; kk <= N; kk++) + { + + // control variables + for (jj = 0; jj < nu[kk]; jj++) + { + mem->A_col_ptr[col] = nn; + col++; + + if (kk < dims->N) + { + // write column from B + for (ii = 0; ii < nx[kk+1]; ii++) + { + mem->A_rowval[nn+ii] = row_offset_dyn + ii; + } + nn += nx[kk+1]; + } + + // write bound on u (upper and lower) + for (ii = 0; ii < nb[kk]; ii++) + { + if (in->idxb[kk][ii] == jj) + { + mem->A_rowval[nn] = con_start + row_offset_con + ii; + nn++; + mem->A_rowval[nn] = con_start + row_offset_con + nb[kk] + ng[kk] + ii; + nn++; + break; + } + } + + // write column from D (upper and lower) + for (ii = 0; ii < ng[kk]; ii++) + { + mem->A_rowval[nn+ii] = con_start + row_offset_con + nb[kk] + ii; + mem->A_rowval[nn+ng[kk]+ii] = con_start + row_offset_con + 2*nb[kk] + ng[kk] + ii; + } + nn += 2*ng[kk]; + } + + // state variables + for (jj = 0; jj < nx[kk]; jj++) + { + mem->A_col_ptr[col] = nn; + col++; + + if (kk > 0) + { + // write column from -I + mem->A_rowval[nn] = row_offset_dyn - nx[kk] + jj; + nn++; + } + + if (kk < N) + { + // write column from A + for (ii = 0; ii < nx[kk + 1]; ii++) + { + mem->A_rowval[nn+ii] = row_offset_dyn + ii; + } + nn += nx[kk+1]; + } + + // write bound on x (upper and lower) + for (ii = 0; ii < nb[kk]; ii++) + { + if (in->idxb[kk][ii] == nu[kk] + jj) + { + mem->A_rowval[nn] = con_start + row_offset_con + ii; + nn++; + mem->A_rowval[nn] = con_start + row_offset_con + nb[kk] + ng[kk] + ii; + nn++; + break; + } + } + + // write column from C (upper and lower) + for (ii = 0; ii < ng[kk]; ii++) + { + mem->A_rowval[nn+ii] = con_start + row_offset_con + nb[kk] + ii; + mem->A_rowval[nn+ng[kk]+ii] = con_start + row_offset_con + 2*nb[kk] + ng[kk] + ii; + } + nn += 2*ng[kk]; + } + + // slack variables on lower inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + mem->A_col_ptr[col] = nn; + col++; + + // soft constraint + for (ii=0; iiidxs_rev[kk][ii]==jj) + { + mem->A_rowval[nn] = con_start + row_offset_con + ii; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_rowval[nn] = slk_start + row_offset_slk + jj; + nn++; + } + + // slack variables on upper inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + mem->A_col_ptr[col] = nn; + col++; + + // soft constraint + for (ii=0; iiidxs_rev[kk][ii]==jj) + { + mem->A_rowval[nn] = con_start + row_offset_con + nb[kk] + ng[kk] + ii; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_rowval[nn] = slk_start + row_offset_slk + ns[kk] + jj; + nn++; + } + + row_offset_con += 2*nb[kk]+2*ng[kk]; + row_offset_dyn += kk < N ? nx[kk + 1] : 0; + row_offset_slk += 2*ns[kk]; + } + + // end of matrix + mem->A_col_ptr[col] = nn; + // actual number of nonzeros + mem->A_nnz = nn; +} + + + +// TODO move constant stuff like I to structure routine +static void update_constraints_matrix_data(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, jj, kk; + + + // Traverse matrix in column-major order + int nn = 0; + for (kk = 0; kk <= N; kk++) + { + // control variables + for (jj = 0; jj < nu[kk]; jj++) + { + if (kk < dims->N) + { + // write column from B + blasfeo_unpack_dmat(1, nx[kk+1], in->BAbt+kk, jj, 0, mem->A_nzval+nn, 1); + nn += nx[kk+1]; + } + + // write bound on u + for (ii = 0; ii < dims->nb[kk]; ii++) + { + if (in->idxb[kk][ii] == jj) + { + mem->A_nzval[nn] = -1.0; // lower bound + nn++; + mem->A_nzval[nn] = 1.0; // upper bound + nn++; + break; + } + } + + // write column from D + blasfeo_unpack_dmat(1, ng[kk], in->DCt+kk, jj, 0, mem->A_nzval+nn+ng[kk], 1); + for (ii=0; iiA_nzval[nn+ii] = - mem->A_nzval[nn+ng[kk]+ii]; + } + nn += 2*ng[kk]; + } + + // state variables + for (jj = 0; jj < nx[kk]; jj++) + { + if (kk > 0) + { + // write column from -I + mem->A_nzval[nn] = -1.0; + nn++; + } + + if (kk < N) + { + // write column from A + blasfeo_unpack_dmat(1, nx[kk+1], in->BAbt+kk, nu[kk]+jj, 0, mem->A_nzval+nn, 1); + nn += nx[kk+1]; + } + + // write bound on x + for (ii = 0; ii < nb[kk]; ii++) + { + if (in->idxb[kk][ii] == nu[kk] + jj) + { + mem->A_nzval[nn] = -1.0; // lower bound + nn++; + mem->A_nzval[nn] = 1.0; // upper bound + nn++; + break; + } + } + + // write column from C + blasfeo_unpack_dmat(1, ng[kk], in->DCt+kk, nu[kk]+jj, 0, mem->A_nzval+nn+ng[kk], 1); + for (ii=0; iiA_nzval[nn+ii] = - mem->A_nzval[nn+ng[kk]+ii]; + } + nn += 2*ng[kk]; + + } + + // slack variables on lower inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + // soft constraint + for (ii=0; iiidxs_rev[kk][ii]==jj) + { + //mem->A_nzval[nn] = 1.0; + mem->A_nzval[nn] = -1.0; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_nzval[nn] = -1.0; //1.0; + nn++; + } + + // slack variables on upper inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + // soft constraint + for (ii=0; iiidxs_rev[kk][ii]==jj) + { + //mem->A_nzval[nn] = 1.0; //-1.0; + mem->A_nzval[nn] = -1.0; //-1.0; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_nzval[nn] = -1.0; //1.0; + nn++; + } + + } + + // TODO ? check that nn==mem->A_nnz +} + + + +static void update_bounds(const ocp_qp_in *in, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + //int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, kk, nn = 0; + + // write b to b + for (kk = 0; kk < N; kk++) + { + // unpack b to b + blasfeo_unpack_dvec(nx[kk + 1], in->b + kk, 0, &mem->b[nn], 1); + for (ii = 0; ii < 2*nx[kk+1]; ii++) + { + mem->b[nn+ii] = - mem->b[nn+ii]; + } + nn += nx[kk + 1]; + } + + // write lb lg and ub ug + for (kk = 0; kk <= N; kk++) + { + // unpack lb lg to l and flip sign because in Clarabel lower bounds are casted as upper bounds + blasfeo_unpack_dvec(nb[kk]+ng[kk], in->d+kk, 0, &mem->b[nn], 1); + // unpack ub ug to u and flip signs because in HPIPM the signs are flipped for upper bounds + blasfeo_unpack_dvec(nb[kk]+ng[kk], in->d+kk, nb[kk]+ng[kk], &mem->b[nn+nb[kk]+ng[kk]], 1); + for (ii = 0; ii < 2*nb[kk]+2*ng[kk]; ii++) + { + mem->b[nn+ii] = - mem->b[nn+ii]; + } + nn += 2*nb[kk] + 2*ng[kk]; + } + + // write ls and us + for (kk = 0; kk <= N; kk++) + { + // unpack ls and us to b because in Clarabel lower bounds are casted as upper bounds + blasfeo_unpack_dvec(2*ns[kk], in->d+kk, 2*nb[kk]+2*ng[kk], &mem->b[nn], 1); + for (ii = 0; ii < 2*ns[kk]; ii++) + { + mem->b[nn+ii] = - mem->b[nn+ii]; + } + nn += 2*ns[kk]; + } + + // actual number of nonzeros + mem->b_nnz = nn; +} + + + +static void clarabel_init_data(ocp_qp_clarabel_memory* mem, ocp_qp_in *qp_in) +{ + //update_constraints_matrix_data(qp_in, mem); + //update_hessian_data(qp_in, mem); + + //update_bounds(qp_in, mem); + //update_gradient(qp_in, mem); + + int n = acados_clarabel_num_vars(qp_in->dim); + int m = acados_clarabel_num_constr(qp_in->dim); + + //printf("\nn %d m %d\n", n, m); + + // allocates and initializes a csc matrix + clarabel_CscMatrix_init(&mem->A, m, n, mem->A_col_ptr, mem->A_rowval, mem->A_nzval); + //printf("ocp_qp_clarabel: created A\n"); + //print_csc_matrix(&mem->A, "A_mat"); + //print_csc_as_dns(&mem->A); + //for (int ii=0; ii<=n; ii++) + //{ + // printf("col ptr %d: %d\n", ii, mem->A_col_ptr[ii]); + //} + + // allocates and initializes a csc matrix + clarabel_CscMatrix_init(&mem->P, n, n, mem->P_col_ptr, mem->P_rowval, mem->P_nzval); + //printf("ocp_qp_clarabel: created P\n"); + //print_csc_matrix(&mem->P, "P_mat"); + //print_csc_as_dns(&mem->P); + //for (int ii=0; ii<=n; ii++) + //{ + // printf("col ptr %d: %d\n", ii, mem->P_col_ptr[ii]); + //} + + //printf("\ndone\n"); + //exit(0); + +} + + + +static void ocp_qp_clarabel_update_memory(const ocp_qp_in *in, const ocp_qp_clarabel_opts *opts, + ocp_qp_clarabel_memory *mem) +{ + if (opts->first_run) + { + update_hessian_structure(in, mem); + update_constraints_matrix_structure(in, mem); + } + + update_hessian_data(in, mem); + update_constraints_matrix_data(in, mem); + update_bounds(in, mem); + update_gradient(in, mem); +} + + +/************************************************ + * opts + ************************************************/ + +acados_size_t ocp_qp_clarabel_opts_calculate_size(void *config_, void *dims_) +{ + acados_size_t size = 0; + size += sizeof(ocp_qp_clarabel_opts); + size += sizeof(ClarabelDefaultSettings); + + return size; +} + + + +void *ocp_qp_clarabel_opts_assign(void *config_, void *dims_, void *raw_memory) +{ + ocp_qp_clarabel_opts *opts; + + char *c_ptr = (char *) raw_memory; + + opts = (ocp_qp_clarabel_opts *) c_ptr; + c_ptr += sizeof(ocp_qp_clarabel_opts); + + opts->clarabel_opts = (ClarabelDefaultSettings *) c_ptr; + c_ptr += sizeof(ClarabelDefaultSettings); + + assert((char *) raw_memory + ocp_qp_clarabel_opts_calculate_size(config_, dims_) == c_ptr); + + return (void *) opts; +} + + + +void ocp_qp_clarabel_opts_initialize_default(void *config_, void *dims_, void *opts_) +{ + ocp_qp_clarabel_opts *opts = opts_; + opts->print_level = 0; + + *opts->clarabel_opts = clarabel_DefaultSettings_default(); + opts->clarabel_opts->verbose = false; + opts->clarabel_opts->presolve_enable = false; + + opts->first_run = 1; + + return; +} + + + +void ocp_qp_clarabel_opts_update(void *config_, void *dims_, void *opts_) +{ + // ocp_qp_clarabel_opts *opts = (ocp_qp_clarabel_opts *)opts_; + + return; +} + +void ocp_qp_clarabel_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_clarabel_opts *opts = opts_; + + // Updating options through this function does not work, only before the first call! + if (!opts->first_run) + { +#ifndef ACADOS_SILENT + printf("\nWARNING: ocp_qp_clarabel_opts_set: attempting to set field: %s. However, options cannot be changed after first run, option is NOT updated. \n", field); +#endif + return; + } + + if (!strcmp(field, "iter_max")) + { + int *tmp_ptr = value; + opts->clarabel_opts->max_iter = *tmp_ptr; + } + else if (!strcmp(field, "print_level")) + { + int* print_level = (int *) value; + opts->print_level = *print_level; + } + else if (!strcmp(field, "tol_stat")) + { + double *tol = value; + // printf("in ocp_qp_clarabel_opts_set, tol_stat %e\n", *tol); + opts->clarabel_opts->tol_gap_abs = *tol; + } + else if (!strcmp(field, "tol_eq")) + { + double *tol = value; + opts->clarabel_opts->tol_infeas_abs = *tol; + } + else if (!strcmp(field, "tol_ineq")) + { + double *tol = value; + opts->clarabel_opts->tol_infeas_abs = *tol; + } + else if (!strcmp(field, "tol_comp")) + { + // do nothing + } + else if (!strcmp(field, "warm_start")) + { + // do nothing + } + else + { + printf("\nWARNING: ocp_qp_clarabel_opts_set: field: %s not interfaced yet. Ignoring option and \n", field); + exit(1); + } + + return; +} + +void ocp_qp_clarabel_opts_get(void *config_, void *opts_, const char *field, void *value) +{ + // ocp_qp_clarabel_opts *opts = opts_; + printf("\nerror: ocp_qp_clarabel_opts_get: not implemented for field %s\n", field); + exit(1); +} + + + + +/************************************************ + * memory + ************************************************/ + +acados_size_t ocp_qp_clarabel_memory_calculate_size(void *config_, void *dims_, void *opts_) +{ + ocp_qp_dims *dims = dims_; + + size_t n = acados_clarabel_num_vars(dims); + size_t m = acados_clarabel_num_constr(dims); + + size_t A_nnzmax = acados_clarabel_nnzmax_A(dims); + size_t P_nnzmax = acados_clarabel_nnzmax_P(dims); + + acados_size_t size = 0; + size += sizeof(ocp_qp_clarabel_memory); + + size += A_nnzmax * sizeof(ClarabelFloat); // A_nzval + size += A_nnzmax * sizeof(uintptr_t); // A_rowval + size += (n + 1) * sizeof(uintptr_t); // A_col_ptr + + size += P_nnzmax * sizeof(ClarabelFloat); // A_nzval + size += P_nnzmax * sizeof(uintptr_t); // P_rowval + size += (n + 1) * sizeof(uintptr_t); // P_col_ptr + + size += n * sizeof(ClarabelFloat); // q + size += m * sizeof(ClarabelFloat); // b + + size += 1 * 8; + + return size; +} + + + + +void *ocp_qp_clarabel_memory_assign(void *config_, void *dims_, void *opts_, void *raw_memory) +{ + ocp_qp_dims *dims = dims_; + ocp_qp_clarabel_memory *mem; + + int n = acados_clarabel_num_vars(dims); + int m = acados_clarabel_num_constr(dims); + int P_nnzmax = acados_clarabel_nnzmax_P(dims); + int A_nnzmax = acados_clarabel_nnzmax_A(dims); + + // char pointer + char *c_ptr = (char *) raw_memory; + + mem = (ocp_qp_clarabel_memory *) c_ptr; + c_ptr += sizeof(ocp_qp_clarabel_memory); + + mem->P_nnzmax = P_nnzmax; + mem->A_nnzmax = A_nnzmax; + + mem->solver = NULL; + + align_char_to(8, &c_ptr); + + // doubles + mem->q = (ClarabelFloat *) c_ptr; + c_ptr += n * sizeof(ClarabelFloat); + + mem->b = (ClarabelFloat *) c_ptr; + c_ptr += m * sizeof(ClarabelFloat); + + mem->P_nzval = (ClarabelFloat *) c_ptr; + c_ptr += (mem->P_nnzmax) * sizeof(ClarabelFloat); + + mem->A_nzval = (ClarabelFloat *) c_ptr; + c_ptr += (mem->A_nnzmax) * sizeof(ClarabelFloat); + + // ints + mem->P_rowval = (uintptr_t *) c_ptr; + c_ptr += (mem->P_nnzmax) * sizeof(uintptr_t); + + mem->P_col_ptr = (uintptr_t *) c_ptr; + c_ptr += (n + 1) * sizeof(uintptr_t); + + mem->A_rowval = (uintptr_t *) c_ptr; + c_ptr += (mem->A_nnzmax) * sizeof(uintptr_t); + + mem->A_col_ptr = (uintptr_t *) c_ptr; + c_ptr += (n + 1) * sizeof(uintptr_t); + + assert((char *) raw_memory + ocp_qp_clarabel_memory_calculate_size(config_, dims, opts_) >= c_ptr); + + return mem; +} + + + +void ocp_qp_clarabel_memory_get(void *config_, void *mem_, const char *field, void* value) +{ + // qp_solver_config *config = config_; + ocp_qp_clarabel_memory *mem = mem_; + + if (!strcmp(field, "time_qp_solver_call")) + { + double *tmp_ptr = value; + *tmp_ptr = mem->time_qp_solver_call; + } + else if (!strcmp(field, "iter")) + { + int *tmp_ptr = value; + *tmp_ptr = mem->iter; + } + else if (!strcmp(field, "status")) + { + int *tmp_ptr = value; + *tmp_ptr = mem->status; + } + else + { + printf("\nerror: ocp_qp_clarabel_memory_get: field %s not available\n", field); + exit(1); + } + + return; + +} + + +void ocp_qp_clarabel_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_) +{ + // ocp_qp_in *qp_in = qp_in_; + // reset memory + printf("acados: reset clarabel_mem not implemented.\n"); + exit(1); +} + + + +/************************************************ + * workspace + ************************************************/ + +acados_size_t ocp_qp_clarabel_workspace_calculate_size(void *config_, void *dims_, void *opts_) +{ + return 0; +} + + + +/************************************************ + * functions + ************************************************/ + +static void fill_in_qp_out(const ocp_qp_in *in, ocp_qp_out *out, ocp_qp_clarabel_memory *mem) +{ + ocp_qp_dims *dims = in->dim; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int kk, nn; + + //int con_start = 0; + //int slk_start = 0; + //for (kk = 0; kk <= N; kk++) + //{ + // con_start += kk < N ? nx[kk + 1] : 0; + // slk_start += 2*nb[kk] + 2*ng[kk]; + //} + + //slk_start += con_start; + + ClarabelDefaultSolution *sol = &mem->solution; + + // primal variables + nn = 0; + for (kk = 0; kk <= N; kk++) + { + blasfeo_pack_dvec(nx[kk]+nu[kk]+2*ns[kk], &sol->x[nn], 1, out->ux + kk, 0); + nn += nx[kk] + nu[kk] + 2*ns[kk]; + } + + // dual variables + nn = 0; + for (kk = 0; kk < N; kk++) + { + blasfeo_pack_dvec(nx[kk + 1], &sol->z[nn], 1, out->pi + kk, 0); + nn += nx[kk + 1]; + } + + //nn = 0; + for (kk = 0; kk <= N; kk++) + { + blasfeo_pack_dvec(2*nb[kk]+2*ng[kk], &sol->z[nn], 1, out->lam+kk, 0); + nn += 2*nb[kk]+2*ng[kk]; + } + + //nn = 0; + for (kk = 0; kk <= N; kk++) + { + blasfeo_pack_dvec(2*ns[kk], &sol->z[nn], 1, out->lam+kk, 2*nb[kk]+2*ng[kk]); + nn += 2*ns[kk]; + } +} + + + +// clarabel f64 printing stuff +static void print_array_double(double *array, size_t n) +{ + printf("["); + for (size_t i = 0; i < n; i++) + { + printf("%.10f", array[i]); + if (i < n - 1) + { + printf(", "); + } + } + printf("]\n"); +} + +static void print_solution(ClarabelDefaultSolution_f64 *solution) +{ + printf("Solution (x)\t = "); + print_array_double(solution->x, solution->x_length); + printf("Multipliers (z)\t = "); + print_array_double(solution->z, solution->z_length); + printf("Slacks (s)\t = "); + print_array_double(solution->s, solution->s_length); +} + +int ocp_qp_clarabel(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_) +{ + ocp_qp_in *qp_in = qp_in_; + ocp_qp_out *qp_out = qp_out_; + + //int N = qp_in->dim->N; + //int *ns = qp_in->dim->ns; + + //print_ocp_qp_dims(qp_in->dim); + + //for (int ii = 0; ii <= N; ii++) + //{ + // if (ns[ii] > 0) + // { + // printf("\nClarabel interface can not handle ns>0 yet.\n"); + // exit(1); + // } + //} + + // print_ocp_qp_in(qp_in); + + qp_info *info = (qp_info *) qp_out->misc; + acados_timer tot_timer, qp_timer, interface_timer, solver_call_timer; + acados_tic(&tot_timer); + + // cast data structures + ocp_qp_clarabel_opts *opts = (ocp_qp_clarabel_opts *) opts_; + ocp_qp_clarabel_memory *mem = (ocp_qp_clarabel_memory *) mem_; + + acados_tic(&interface_timer); + ocp_qp_clarabel_update_memory(qp_in, opts, mem); + info->interface_time = acados_toc(&interface_timer); + + acados_tic(&qp_timer); + + if (!opts->first_run) + { + // ClarabelDefaultInfo tmp_info; + clarabel_DefaultSolver_update_P(mem->solver, mem->P_nzval, mem->P_nnz); + clarabel_DefaultSolver_update_A(mem->solver, mem->A_nzval, mem->A_nnz); + clarabel_DefaultSolver_update_q(mem->solver, mem->q, mem->q_nnz); + clarabel_DefaultSolver_update_b(mem->solver, mem->b, mem->b_nnz); + } + else + { + //printf("\nbefore build solver\n"); + // TODO for now, need to free previous solver (and csc matrices) ... + if (mem->solver!=NULL) + { + //printf("\nfreeing solver before new\n"); + clarabel_DefaultSolver_free(mem->solver); + } + // init new csc matrices + clarabel_init_data(mem, qp_in); + // Build solver + mem->solver = clarabel_DefaultSolver_new(&mem->P, mem->q, &mem->A, mem->b, 2, mem->cones, opts->clarabel_opts); + opts->first_run = 0; + // opts->first_run = 1; + // uncomment above to force first_run, and investigate solver updates. + } + + // solve Clarabel + acados_tic(&solver_call_timer); + // Solve + //clarabel_DefaultSolver_print_to_file(mem->solver, "clarabel_data.txt"); + //clarabel_DefaultSolver_save_to_file(mem->solver, "clarabel_data.txt"); + clarabel_DefaultSolver_solve(mem->solver); + mem->time_qp_solver_call = acados_toc(&solver_call_timer); + + + // Get solution + mem->solution = clarabel_DefaultSolver_solution(mem->solver); + if (opts->print_level > 1) + { + print_solution(&mem->solution); + } + //clarabel_DefaultSolver_free(mem->solver); + //print_solution(&mem->solution); + + /* fill qp_out */ + fill_in_qp_out(qp_in, qp_out, mem); + ocp_qp_compute_t(qp_in, qp_out); + + //d_ocp_qp_sol_print(qp_in->dim, qp_out); + + // info + ClarabelDefaultInfo clarabel_info = clarabel_DefaultSolver_info(mem->solver); + info->solve_QP_time = acados_toc(&qp_timer); + //info->solve_QP_time = clarabel_info.solve_time; + info->total_time = acados_toc(&tot_timer); + info->num_iter = clarabel_info.iterations; + mem->iter = clarabel_info.iterations; + + // status + int acados_status = ACADOS_QP_FAILURE; // generic QP failure + ClarabelSolverStatus clarabel_status = mem->solution.status; + if (clarabel_status==ClarabelSolved) + acados_status = ACADOS_SUCCESS; + else if (clarabel_status==ClarabelMaxIterations) + acados_status = ACADOS_MAXITER; + + return acados_status; +} + + + +void ocp_qp_clarabel_eval_adj_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +{ + printf("\nerror: ocp_qp_clarabel_eval_adj_sens: not implemented yet\n"); + exit(1); +} + +void ocp_qp_clarabel_eval_forw_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +{ + printf("\nerror: ocp_qp_clarabel_eval_forw_sens: not implemented yet\n"); + exit(1); +} + +void ocp_qp_clarabel_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2) +{ + printf("\nerror: ocp_qp_clarabel_solver_get: not implemented yet\n"); + exit(1); +} + + +void ocp_qp_clarabel_terminate(void *config_, void *mem_, void *work_) +{ + ocp_qp_clarabel_memory *mem = (ocp_qp_clarabel_memory *) mem_; + // Free the matrices and the solver + clarabel_DefaultSolver_free(mem->solver); +} + + + + +void ocp_qp_clarabel_config_initialize_default(void *config_) +{ + qp_solver_config *config = config_; + + config->opts_calculate_size = &ocp_qp_clarabel_opts_calculate_size; + config->opts_assign = &ocp_qp_clarabel_opts_assign; + config->opts_initialize_default = &ocp_qp_clarabel_opts_initialize_default; + config->opts_update = &ocp_qp_clarabel_opts_update; + config->opts_set = &ocp_qp_clarabel_opts_set; + config->opts_get = &ocp_qp_clarabel_opts_get; + config->memory_calculate_size = &ocp_qp_clarabel_memory_calculate_size; + config->memory_assign = &ocp_qp_clarabel_memory_assign; + config->memory_get = &ocp_qp_clarabel_memory_get; + config->workspace_calculate_size = &ocp_qp_clarabel_workspace_calculate_size; + config->evaluate = &ocp_qp_clarabel; + config->terminate = &ocp_qp_clarabel_terminate; + config->eval_forw_sens = &ocp_qp_clarabel_eval_forw_sens; + config->eval_adj_sens = &ocp_qp_clarabel_eval_adj_sens; + config->memory_reset = &ocp_qp_clarabel_memory_reset; + config->solver_get = &ocp_qp_clarabel_solver_get; + + return; +} diff --git a/acados/ocp_qp/ocp_qp_clarabel.h b/acados/ocp_qp/ocp_qp_clarabel.h new file mode 100644 index 0000000000..9ecc350178 --- /dev/null +++ b/acados/ocp_qp/ocp_qp_clarabel.h @@ -0,0 +1,115 @@ +/* + * 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.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_CLARABEL_H_ +#define ACADOS_OCP_QP_OCP_QP_CLARABEL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// clarabel +// #include "clarabel/Clarabel.h" +#include "Clarabel.cpp/include/clarabel.h" + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef struct ocp_qp_clarabel_opts_ +{ + // settings *clarabel_opts; + ClarabelDefaultSettings *clarabel_opts; + int print_level; + int first_run; + +} ocp_qp_clarabel_opts; + + +typedef struct ocp_qp_clarabel_memory_ +{ + ClarabelCscMatrix P; // just upper triangular is enough + uintptr_t P_nnzmax; + uintptr_t *P_col_ptr; + uintptr_t *P_rowval; + ClarabelFloat *P_nzval; + uintptr_t P_nnz; + + ClarabelCscMatrix A; + uintptr_t A_nnzmax; + uintptr_t *A_col_ptr; + uintptr_t *A_rowval; + ClarabelFloat *A_nzval; + uintptr_t A_nnz; + + ClarabelFloat *q; + uintptr_t q_nnz; + ClarabelFloat *b; + uintptr_t b_nnz; + + ClarabelSupportedConeT cones[2]; + + ClarabelDefaultSolver *solver; + ClarabelDefaultSolution solution; + + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_clarabel_memory; + +acados_size_t ocp_qp_clarabel_opts_calculate_size(void *config, void *dims); +// +void *ocp_qp_clarabel_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_qp_clarabel_opts_initialize_default(void *config, void *dims, void *opts_); +// +void ocp_qp_clarabel_opts_update(void *config, void *dims, void *opts_); +// +acados_size_t ocp_qp_clarabel_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *ocp_qp_clarabel_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_clarabel_workspace_calculate_size(void *config, void *dims, void *opts_); +// +int ocp_qp_clarabel(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_clarabel_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); +// +void ocp_qp_clarabel_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); +// +void ocp_qp_clarabel_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_CLARABEL_H_ diff --git a/cmake/.gitignore b/cmake/.gitignore index bb8a490c37..b83afb1930 100644 --- a/cmake/.gitignore +++ b/cmake/.gitignore @@ -3,3 +3,4 @@ *Config*.cmake *Targets*.cmake daqp*.cmake +Clarabel.cmake diff --git a/cmake/acadosConfig.cmake.in b/cmake/acadosConfig.cmake.in index a6668d5b21..83108e52f6 100644 --- a/cmake/acadosConfig.cmake.in +++ b/cmake/acadosConfig.cmake.in @@ -37,6 +37,7 @@ set(ACADOS_WITH_QPDUNES @ACADOS_WITH_QPDUNES@) set(ACADOS_WITH_OSQP @ACADOS_WITH_OSQP@) set(ACADOS_WITH_OOQP @ACADOS_WITH_OOQP@) set(ACADOS_WITH_DAQP @ACADOS_WITH_DAQP@) +set(ACADOS_WITH_CLARABEL @ACADOS_WITH_CLARABEL@) # Add acados CMake folder to CMake prefix and module path set(CMAKE_MODULE_PATH_save "${CMAKE_MODULE_PATH}") @@ -89,6 +90,10 @@ if(ACADOS_WITH_DAQP) find_package(daqp) endif() +if(ACADOS_WITH_CLARABEL) + find_package(clarabel_c) +endif() + if(ACADOS_WITH_OSQP) find_package(osqp) endif() diff --git a/docs/installation/index.md b/docs/installation/index.md index dde55ba444..da08db67d3 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -40,6 +40,7 @@ Adjust these options based on your requirements. | `ACADOS_WITH_QPDUNES` | Compile acados with optional QP solver qpDUNES | `OFF` | | `ACADOS_WITH_OSQP` | Compile acados with optional QP solver OSQP | `OFF` | | `ACADOS_WITH_HPMPC` | Compile acados with optional QP solver HPMPC | `OFF` | +| `ACADOS_WITH_CLARABEL` | Compile acados with optional QP solver Clarabel | `OFF` | | `ACADOS_WITH_QORE` | Compile acados with optional QP solver QORE (experimental) | `OFF` | | `ACADOS_WITH_OOQP` | Compile acados with optional QP solver OOQP (experimental) | `OFF` | | `BLASFEO_TARGET` | BLASFEO Target architecture, see BLASFEO repository for more information. Possible values include: `X64_AUTOMATIC`, `GENERIC`, `X64_INTEL_SKYLAKE_X`, `X64_INTEL_HASWELL`, `X64_INTEL_SANDY_BRIDGE`, `X64_INTEL_CORE`, `X64_AMD_BULLDOZER`, `ARMV8A_APPLE_M1`, `ARMV8A_ARM_CORTEX_A76`, `ARMV8A_ARM_CORTEX_A73`, `ARMV8A_ARM_CORTEX_A57`, `ARMV8A_ARM_CORTEX_A55`, `ARMV8A_ARM_CORTEX_A53`, `ARMV7A_ARM_CORTEX_A15`, `ARMV7A_ARM_CORTEX_A9`, `ARMV7A_ARM_CORTEX_A7` | `X64_AUTOMATIC` | diff --git a/examples/acados_python/tests/test_osqp.py b/examples/acados_python/tests/test_qp_solver.py similarity index 90% rename from examples/acados_python/tests/test_osqp.py rename to examples/acados_python/tests/test_qp_solver.py index abf5f50788..20414868c3 100644 --- a/examples/acados_python/tests/test_osqp.py +++ b/examples/acados_python/tests/test_qp_solver.py @@ -34,10 +34,12 @@ from acados_template import AcadosOcp, AcadosOcpSolver from pendulum_model import export_pendulum_ode_model import numpy as np +import argparse import scipy.linalg -def main(): +def main(qp_solver="PARTIAL_CONDENSING_OSQP"): + print(f"Solving with {qp_solver}") ocp = AcadosOcp() # set model @@ -85,7 +87,7 @@ def main(): ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) # set options - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_OSQP' + ocp.solver_options.qp_solver = qp_solver ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' @@ -122,4 +124,7 @@ def main(): # plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) if __name__ == "__main__": - main() + parser = argparse.ArgumentParser() + parser.add_argument('--qp_solver', type=str, default='PARTIAL_CONDENSING_OSQP', help='QP solver to use') + args = parser.parse_args() + main(qp_solver=args.qp_solver) diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index e14857e37b..5b57ca1a89 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -96,7 +96,7 @@ int main() { int acados_return = 0; - int ii_max = 9; + int ii_max = 10; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -117,6 +117,9 @@ int main() { #ifndef ACADOS_WITH_OSQP ii_max--; #endif + #ifndef ACADOS_WITH_CLARABEL + ii_max--; + #endif // choose ocp qp solvers ocp_qp_solver_t ocp_qp_solvers[] = @@ -149,6 +152,10 @@ int main() { #ifdef ACADOS_WITH_OSQP PARTIAL_CONDENSING_OSQP, #endif + + #ifdef ACADOS_WITH_CLARABEL + PARTIAL_CONDENSING_CLARABEL, + #endif }; @@ -296,7 +303,13 @@ int main() { N2 = N2_values[jj]; config->opts_set(config, opts, "cond_N", &N2); break; - +#endif +#ifdef ACADOS_WITH_CLARABEL + case PARTIAL_CONDENSING_CLARABEL: + printf("\nPartial condensing + Clarabel (N2 = %d):\n\n", N2); + N2 = N2_values[jj]; + config->opts_set(config, opts, "cond_N", &N2); + break; #endif case INVALID_QP_SOLVER: printf("\nInvalid QP solver\n\n"); diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt index f522351b70..20460c2b04 100644 --- a/external/CMakeLists.txt +++ b/external/CMakeLists.txt @@ -92,6 +92,16 @@ if(ACADOS_WITH_OSQP) add_subdirectory(osqp) endif() + +if(ACADOS_WITH_CLARABEL) + set(CLARABEL_BUILD_EXAMPLES OFF CACHE BOOL "set Clarabel examples" FORCE) + set(CLARABEL_C_OUTPUT_DIR "${CMAKE_INSTALL_PREFIX}/build/external/Clarabel.cpp" CACHE STRING "Set Clarabel.cpp output dir" FORCE) + set(PROFILING OFF CACHE BOOL "Turn off profiling in CLARABEL") + set(CTRLC OFF CACHE BOOL "Turn off CTRLC in CLARABEL") + add_subdirectory(Clarabel.cpp) + add_dependencies(acados libclarabel_c_shared) +endif() + if (ACADOS_WITH_OOQP) include(external/ooqp) endif() diff --git a/external/Clarabel.cpp b/external/Clarabel.cpp new file mode 160000 index 0000000000..c73eb64352 --- /dev/null +++ b/external/Clarabel.cpp @@ -0,0 +1 @@ +Subproject commit c73eb64352f17fc18bc10fe3c082fcbf9a3c0487 diff --git a/interfaces/acados_c/ocp_qp_interface.c b/interfaces/acados_c/ocp_qp_interface.c index 128f31bc69..b8e81e88d2 100644 --- a/interfaces/acados_c/ocp_qp_interface.c +++ b/interfaces/acados_c/ocp_qp_interface.c @@ -74,6 +74,11 @@ #include "acados/ocp_qp/ocp_qp_osqp.h" #endif +#ifdef ACADOS_WITH_CLARABEL +#include "acados/ocp_qp/ocp_qp_clarabel.h" +#endif + + // TODO: no "plan" is entering, rename?! void ocp_qp_xcond_solver_config_initialize_from_plan( @@ -108,6 +113,13 @@ void ocp_qp_xcond_solver_config_initialize_from_plan( ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #endif +#ifdef ACADOS_WITH_CLARABEL + case PARTIAL_CONDENSING_CLARABEL: + ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_clarabel_config_initialize_default(solver_config->qp_solver); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + break; +#endif #ifdef ACADOS_WITH_QPDUNES case PARTIAL_CONDENSING_QPDUNES: ocp_qp_xcond_solver_config_initialize_default(solver_config); @@ -154,7 +166,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan( exit(1); break; default: - printf("\nerror: ocp_qp_xcond_solver_config_initialize_from_plan: unsupported plan->qp_solver\n"); + printf("\nerror: ocp_qp_xcond_solver_config_initialize_from_plan: unsupported plan->qp_solver %d\n", solver_name); printf("This might happen, if acados was not compiled with the specified QP solver.\n"); exit(1); } diff --git a/interfaces/acados_c/ocp_qp_interface.h b/interfaces/acados_c/ocp_qp_interface.h index 3dc3f1a532..d8261ee228 100644 --- a/interfaces/acados_c/ocp_qp_interface.h +++ b/interfaces/acados_c/ocp_qp_interface.h @@ -47,6 +47,7 @@ extern "C" { /// PARTIAL_CONDENSING_HPMPC /// PARTIAL_CONDENSING_OOQP /// PARTIAL_CONDENSING_OSQP +/// PARTIAL_CONDENSING_CLARABEL /// PARTIAL_CONDENSING_QPDUNES /// FULL_CONDENSING_HPIPM /// FULL_CONDENSING_QPOASES @@ -73,6 +74,11 @@ typedef enum { #else PARTIAL_CONDENSING_OSQP_NOT_AVAILABLE, #endif +#ifdef ACADOS_WITH_CLARABEL + PARTIAL_CONDENSING_CLARABEL, +#else + PARTIAL_CONDENSING_CLARABEL_NOT_AVAILABLE, +#endif #ifdef ACADOS_WITH_QPDUNES PARTIAL_CONDENSING_QPDUNES, #else diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index 9a0666d88f..da65e8833a 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -624,6 +624,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) #if defined(ACADOS_WITH_DAQP) if (plan->ocp_qp_solver_plan.qp_solver==FULL_CONDENSING_DAQP) solver_type=2; +#endif +#if defined(ACADOS_WITH_CLARABEL) + if (plan->ocp_qp_solver_plan.qp_solver==PARTIAL_CONDENSING_CLARABEL) + solver_type=1; #endif // ocp solver (not dense) if (solver_type==1) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 6a72dc3460..cd063ba5c3 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -166,7 +166,7 @@ def __init__(self): @property def qp_solver(self): """QP solver to be used in the NLP solver. - String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'). + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'PARTIAL_CONDENSING_CLARABEL', FULL_CONDENSING_DAQP'). Default: 'PARTIAL_CONDENSING_HPIPM'. """ return self.__qp_solver @@ -1430,7 +1430,7 @@ def with_batch_functionality(self): def qp_solver(self, qp_solver): qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ - 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'PARTIAL_CONDENSING_CLARABEL', \ 'FULL_CONDENSING_DAQP') if qp_solver in qp_solvers: self.__qp_solver = qp_solver diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index ee6be77e7f..32b7a499b9 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -64,7 +64,7 @@ {%- endif %} {%- if acados_link_libs and os and os == "pc" %}{# acados linking libraries and flags #} - {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp -%} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp ~ " " ~ acados_link_libs.clarabel -%} {%- set openmp_flag = acados_link_libs.openmp %} {%- else %} {%- set openmp_flag = " " %} @@ -72,6 +72,8 @@ {%- set link_libs = "-lqpOASES_e" %} {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} {%- set link_libs = "-ldaqp" %} + {%- elif qp_solver == "PARTIAL_CONDENSING_CLARABEL" %} + {%- set link_libs = "-lclarabel_c" %} {%- else %} {%- set link_libs = "" %} {%- endif %} @@ -196,6 +198,9 @@ set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_co {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%} -DACADOS_WITH_OSQP {%- endif -%} +{%- if qp_solver == "PARTIAL_CONDENSING_CLARABEL" -%} + -DACADOS_WITH_CLARABEL +{%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%} -DACADOS_WITH_QPDUNES {%- endif -%} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index 1b03e3dba0..3dc508b0ea 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -64,8 +64,9 @@ {%- set control = ";" %} {%- endif %} -{%- if acados_link_libs and os and os == "pc" %}{# acados linking libraries and flags #} - {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp -%} +{# acados linking libraries and flags #} +{%- if acados_link_libs and os and os == "pc" %} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp ~ " " ~ acados_link_libs.clarabel -%} {%- set openmp_flag = acados_link_libs.openmp %} {%- else %} {%- set openmp_flag = " " %} @@ -73,6 +74,8 @@ {%- set link_libs = "-lqpOASES_e" %} {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} {%- set link_libs = "-ldaqp" %} + {%- elif qp_solver == "PARTIAL_CONDENSING_CLARABEL" %} + {%- set link_libs = "-lclarabel_c " %} {%- else %} {%- set link_libs = "" %} {%- endif %} @@ -161,6 +164,9 @@ CPPFLAGS += -DACADOS_WITH_DAQP {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %} CPPFLAGS += -DACADOS_WITH_OSQP {%- endif %} +{%- if qp_solver == "PARTIAL_CONDENSING_CLARABEL" %} +CPPFLAGS += -DACADOS_WITH_CLARABEL +{%- endif %} {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %} CPPFLAGS += -DACADOS_WITH_QPDUNES {%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m index d48238d093..54a88661c8 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m @@ -154,6 +154,9 @@ {%- elif solver_options.qp_solver is containing("OSQP") %} CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ]; COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ]; +{%- elif solver_options.qp_solver is containing("CLARABEL") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_CLARABEL ' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_CLARABEL ' ]; {%- elif solver_options.qp_solver is containing("QPDUNES") %} CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; diff --git a/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in index 0656d547be..4cdae05328 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/multi_Makefile.in @@ -284,6 +284,9 @@ CPPFLAGS += -DACADOS_WITH_DAQP {%- if solver_options.qp_solver == "PARTIAL_CONDENSING_OSQP" %} CPPFLAGS += -DACADOS_WITH_OSQP {%- endif %} +{%- if solver_options.qp_solver == "PARTIAL_CONDENSING_CLARABEL" %} +CPPFLAGS += -DACADOS_WITH_CLARABEL +{%- endif %} {%- if solver_options.qp_solver == "PARTIAL_CONDENSING_QPDUNES" %} CPPFLAGS += -DACADOS_WITH_QPDUNES {%- endif %} From c3f374ba70c4efd0ea9adc664189228738d0b3f2 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Thu, 9 Oct 2025 11:38:22 +0200 Subject: [PATCH 155/164] Python: sparsify in `translate_to_external_cost` (#1658) --- .../acados_template/acados_template/acados_ocp.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 44435fb80d..311f5ba0b6 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1912,26 +1912,26 @@ def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, def __translate_ls_cost_to_external_cost(x, u, z, Vx, Vu, Vz, yref, W): res = 0 if not is_empty(Vx): - res += Vx @ x + res += ca.sparsify(Vx) @ x if not is_empty(Vu): - res += Vu @ u + res += ca.sparsify(Vu) @ u if not is_empty(Vz): - res += Vz @ z + res += ca.sparsify(Vz) @ z res -= yref - return 0.5 * (res.T @ W @ res) + return 0.5 * (res.T @ ca.sparsify(W) @ res) @staticmethod def __translate_nls_cost_to_external_cost(y_expr, yref, W): res = y_expr - yref - return 0.5 * (res.T @ W @ res) + return 0.5 * (res.T @ ca.sparsify(W) @ res) @staticmethod def __get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, x, u, z): res = y_expr - yref ux = ca.vertcat(u, x) inner_jac = ca.jacobian(res, ux) - gn_hess = inner_jac.T @ W @ inner_jac + gn_hess = inner_jac.T @ ca.sparsify(W) @ inner_jac return gn_hess @staticmethod From c1c53ac3a90c47b04e645a09c3d52d3e5857aa39 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Mon, 13 Oct 2025 17:07:08 +0200 Subject: [PATCH 156/164] Improve QP status reporting in Acados (#1659) - Polish QP staus in 6 files, i.e. `acados/dense_qp/dense_qp_daqp.c`, `acados/dense_qp/dense_qp_hpipm.c`, `acados/dense_qp/dense_qp_qpoases.c`, `acados/ocp_qp/ocp_qp_hpipm.c`, `acados/ocp_qp/ocp_qp_osqp.c`, `acados/ocp_qp/ocp_qp_qpdunes.c`. - A look-up table in `interfaces/acados_template/acados_template/acados_ocp_options.py` - Add `ACADOS_UNKNOWN = -1` in `acados/utils/types.h` --------- Co-authored-by: Jonathan Frey --- acados/dense_qp/dense_qp_daqp.c | 9 +++- acados/dense_qp/dense_qp_hpipm.c | 13 +++--- acados/dense_qp/dense_qp_qpoases.c | 5 ++- acados/ocp_qp/ocp_qp_hpipm.c | 12 +++--- acados/ocp_qp/ocp_qp_osqp.c | 11 ++++- acados/ocp_qp/ocp_qp_qpdunes.c | 6 ++- acados/utils/types.h | 2 + .../acados_template/acados_ocp_options.py | 42 +++++++++++++++++++ 8 files changed, 85 insertions(+), 15 deletions(-) diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c index 3ac52f2b4f..9532435a76 100644 --- a/acados/dense_qp/dense_qp_daqp.c +++ b/acados/dense_qp/dense_qp_daqp.c @@ -781,9 +781,14 @@ int dense_qp_daqp(void* config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void acados_status = ACADOS_SUCCESS; else if (daqp_status == EXIT_ITERLIMIT) acados_status = ACADOS_MAXITER; + else if (daqp_status == EXIT_INFEASIBLE) + acados_status = ACADOS_INFEASIBLE; + else if (daqp_status == EXIT_UNBOUNDED) + acados_status = ACADOS_UNBOUNDED; + else + acados_status = ACADOS_UNKNOWN; // NOTE: There are also: - // EXIT_INFEASIBLE, EXIT_CYCLE, EXIT_UNBOUNDED, EXIT_NONCONVEX, EXIT_OVERDETERMINED_INITIAL - + // EXIT_CYCLE, EXIT_UNBOUNDED, EXIT_NONCONVEX, EXIT_OVERDETERMINED_INITIAL return acados_status; } diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index 2e5b7d1027..077459ac9f 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -37,6 +37,7 @@ #include "hpipm/include/hpipm_d_dense_qp.h" #include "hpipm/include/hpipm_d_dense_qp_ipm.h" #include "hpipm/include/hpipm_d_dense_qp_sol.h" +#include "hpipm/include/hpipm_common.h" // acados #include "acados/dense_qp/dense_qp_common.h" #include "acados/dense_qp/dense_qp_hpipm.h" @@ -311,11 +312,13 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void #endif // check exit conditions - int acados_status = hpipm_status; - if (hpipm_status == 0) acados_status = ACADOS_SUCCESS; - if (hpipm_status == 1) acados_status = ACADOS_MAXITER; - if (hpipm_status == 2) acados_status = ACADOS_MINSTEP; - return acados_status; + if (hpipm_status == SUCCESS) return ACADOS_SUCCESS; + if (hpipm_status == MAX_ITER) return ACADOS_MAXITER; + if (hpipm_status == MIN_STEP) return ACADOS_MINSTEP; + if (hpipm_status == NAN_SOL) return ACADOS_NAN_DETECTED; + if (hpipm_status == INCONS_EQ) return ACADOS_INFEASIBLE; + + return ACADOS_UNKNOWN; } diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 4e0d5bd4a4..d16f6be59c 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -783,7 +783,10 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo int acados_status = qpoases_status; if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS; - if (qpoases_status == RET_MAX_NWSR_REACHED) acados_status = ACADOS_MAXITER; + else if (qpoases_status == RET_MAX_NWSR_REACHED) acados_status = ACADOS_MAXITER; + else if( qpoases_status == RET_INIT_FAILED_INFEASIBILITY) acados_status = ACADOS_INFEASIBLE; + else if( qpoases_status == RET_INIT_FAILED_UNBOUNDEDNESS) acados_status = ACADOS_UNBOUNDED; + else acados_status = ACADOS_UNKNOWN; return acados_status; } diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index 72667e929f..310b66b999 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -37,6 +37,7 @@ #include "hpipm/include/hpipm_d_ocp_qp.h" #include "hpipm/include/hpipm_d_ocp_qp_ipm.h" #include "hpipm/include/hpipm_d_ocp_qp_sol.h" +#include "hpipm/include/hpipm_common.h" // uncomment to codegen QP // #include "hpipm/include/hpipm_d_ocp_qp_utils.h" @@ -368,12 +369,13 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void * // d_ocp_qp_ipm_arg_print(qp_in->dim, opts->hpipm_opts); // check exit conditions - int acados_status = mem->status; - if (mem->status == 0) acados_status = ACADOS_SUCCESS; - if (mem->status == 1) acados_status = ACADOS_MAXITER; - if (mem->status == 2) acados_status = ACADOS_MINSTEP; + if (mem->status == SUCCESS) return ACADOS_SUCCESS; + if (mem->status == MAX_ITER) return ACADOS_MAXITER; + if (mem->status == MIN_STEP) return ACADOS_MINSTEP; + if (mem->status == NAN_SOL) return ACADOS_NAN_DETECTED; + if (mem->status == INCONS_EQ) return ACADOS_INFEASIBLE; - return acados_status; + return ACADOS_UNKNOWN; } diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index b8d9d669dc..6f3cb05a3c 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -1777,11 +1777,20 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m //printf("\nOSQP solved\n"); acados_status = ACADOS_SUCCESS; } - if (osqp_status == OSQP_MAX_ITER_REACHED) + else if (osqp_status == OSQP_MAX_ITER_REACHED) { //printf("\nOSQP max iter reached\n"); acados_status = ACADOS_MAXITER; } + else if (osqp_status == OSQP_PRIMAL_INFEASIBLE) + { + //printf("\nOSQP primal infeasible\n"); + acados_status = ACADOS_INFEASIBLE; + } + else + { + acados_status = ACADOS_UNKNOWN; + } mem->status = acados_status; return acados_status; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.c b/acados/ocp_qp/ocp_qp_qpdunes.c index a36c729963..eb0503ecc1 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.c +++ b/acados/ocp_qp/ocp_qp_qpdunes.c @@ -913,10 +913,14 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *in, ocp_qp_out *out, void *opts_, v printf("qpDUNES seems to have really strong requirements with this regard...\n"); acados_status = ACADOS_QP_FAILURE; } + else if (qpdunes_status == QPDUNES_ERR_STAGE_QP_INFEASIBLE) + { + acados_status = ACADOS_INFEASIBLE; + } else { printf("\nqpDUNES: returned error not handled by acados.\n"); - acados_status = ACADOS_QP_FAILURE; + acados_status = ACADOS_UNKNOWN; } mem->status = acados_status; diff --git a/acados/utils/types.h b/acados/utils/types.h index 60f933c74c..bce6587f7e 100644 --- a/acados/utils/types.h +++ b/acados/utils/types.h @@ -73,6 +73,7 @@ typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, doub // enum of return values enum return_values { + ACADOS_UNKNOWN = -1, ACADOS_SUCCESS = 0, ACADOS_NAN_DETECTED = 1, ACADOS_MAXITER = 2, @@ -82,6 +83,7 @@ enum return_values ACADOS_UNBOUNDED = 6, ACADOS_TIMEOUT = 7, ACADOS_QPSCALING_BOUNDS_NOT_SATISFIED = 8, + ACADOS_INFEASIBLE = 9, }; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index cd063ba5c3..b7dfa6a167 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -168,6 +168,48 @@ def qp_solver(self): """QP solver to be used in the NLP solver. String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'PARTIAL_CONDENSING_CLARABEL', FULL_CONDENSING_DAQP'). Default: 'PARTIAL_CONDENSING_HPIPM'. + + QP solver statuses are mapped to the acados status definitions. + + For HPIPM the status values are mapped as below + | HPIPM status | acados status | + |----------------------------------| + | SUCCESS | ACADOS_SUCCESS 0 | + | MAXIT | ACADOS_MAXITER 2 | + | MINSTEP | ACADOS_MINSTEP 3 | + | NAN | ACADOS_NAN 1 | + | INCONS_EQ | ACADOS_INFEASIBLE 9 | + | ELSE | ACADOS_UNKNOWN -1 | + + For qpOASES the status values are mapped as below + | qpOASES status | acados status | + |-----------------------------------------------------| + | SUCCESSFUL_RETURN | ACADOS_SUCCESS 0 | + | RET_MAX_NWSR_REACHED | ACADOS_MAXITER 2 | + | RET_INIT_FAILED_UNBOUNDEDNESS | ACADOS_UNBOUNDED 6 | + | RET_INIT_FAILED_INFEASIBILITY | ACADOS_INFEASIBLE 9 | + | ELSE | ACADOS_UNKNOWN -1 | + + For DAQP the status values are mapped as below + | DAQP status | acados status | + |------------------------------------------------| + | EXIT_OPTIMAL | ACADOS_SUCCESS 0 | + | EXIT_SOFT_OPTIMAL | ACADOS_MAXITER 0 | + | EXIT_EXIT_ITERLIMIT | ACADOS_MAXITER 2 | + | EXIT_UNBOUNDED | ACADOS_UNBOUNDED 6 | + | EXIT_INFEASIBLE | ACADOS_INFEASIBLE 9 | + | ELSE | ACADOS_UNKNOWN -1 | + + For QPDUNES the status values are mapped as below + | QPDUNES status | acados status | + |---------------------------------------------------------------------| + | QPDUNES_OK | ACADOS_SUCCESS 0 | + | QPDUNES_SUCC_OPTIMAL_SOLUTION_FOUND | ACADOS_MAXITER 0 | + | QPDUNES_ERR_ITERATION_LIMIT_REACHED | ACADOS_MAXITER 2 | + | QPDUNES_ERR_DIVISION_BY_ZERO | ACADOS_QP_FAILURE 4 | + | QPDUNES_ERR_STAGE_QP_INFEASIBLE | ACADOS_INFEASIBLE 9 | + | ELSE | ACADOS_UNKNOWN -1 | + """ return self.__qp_solver From acc8199c18df86e66619f9d3cf5d9e58ecbb99e4 Mon Sep 17 00:00:00 2001 From: emstok Date: Mon, 13 Oct 2025 21:33:30 +0200 Subject: [PATCH 157/164] Update detect_constraint_structure.m to remove redundant assignments causing errors (#1663) Removed some redundant and (presumed) incorrect code which would throw errors. This errors were only encountered when constraints in con_h_expr and were being automatically rewritten as general linear constraints. The removed lines attempted to assign variables C and D into invalid locations. Based on the naming of the invalid locations I presume these were left over from before acados v0.4.0. The C, D, lg, and, ug are already assigned within an identical if() case some some lines previous (lines 215-221), making the removed lines redundant anyway even if they were correct. --- .../acados_matlab_octave/detect_constraint_structure.m | 7 ------- 1 file changed, 7 deletions(-) diff --git a/interfaces/acados_matlab_octave/detect_constraint_structure.m b/interfaces/acados_matlab_octave/detect_constraint_structure.m index 791df2ee6d..83316d9ff4 100644 --- a/interfaces/acados_matlab_octave/detect_constraint_structure.m +++ b/interfaces/acados_matlab_octave/detect_constraint_structure.m @@ -231,13 +231,6 @@ constraints.lbu = lbu; constraints.ubu = ubu; end - % g - if ~isempty(lg) - model.constr_C = C; - model.constr_D = D; - constraints.lg = lg; - constraints.ug = ug; - end end end From fa6723e9d350ad0631e98c73a7bc72f247ec8dc3 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 14 Oct 2025 14:22:12 +0200 Subject: [PATCH 158/164] Python: refactor `translate_to_external` and add GGN Hessian (#1661) --- .../acados_template/acados_ocp.py | 250 +++++++----------- .../casadi_function_generation.py | 1 + 2 files changed, 94 insertions(+), 157 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 311f5ba0b6..ef45ea8c2c 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1718,194 +1718,117 @@ def translate_cost_to_external_cost(self, def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + self._translate_cost_term_to_external(stage_type='initial', yref=yref_0, W=W_0, cost_hessian=cost_hessian) - if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: - raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") - - if cost_hessian == 'GAUSS_NEWTON': - if self.cost.cost_type_0 not in ['LINEAR_LS', 'NONLINEAR_LS', None]: - raise Exception(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_0 = {self.cost.cost_type_0}.") - - casadi_symbolics_type = type(self.model.x) - - if yref_0 is None: - yref_0 = self.cost.yref_0 - else: - if yref_0.shape[0] != self.cost.yref_0.shape[0]: - raise ValueError(f"yref_0 has wrong shape, got {yref_0.shape}, expected {self.cost.yref_0.shape}.") - - if not isinstance(yref_0, casadi_symbolics_type): - raise TypeError(f"yref_0 has wrong type, got {type(yref_0)}, expected {casadi_symbolics_type}.") - - if W_0 is None: - W_0 = self.cost.W_0 - else: - if W_0.shape != self.cost.W_0.shape: - raise ValueError(f"W_0 has wrong shape, got {W_0.shape}, expected {self.cost.W_0.shape}.") - - if not isinstance(W_0, casadi_symbolics_type): - raise TypeError(f"W_0 has wrong type, got {type(W_0)}, expected {casadi_symbolics_type}.") - - if self.cost.cost_type_0 == "LINEAR_LS": - self.model.cost_expr_ext_cost_0 = \ - self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, - self.cost.Vx_0, self.cost.Vu_0, self.cost.Vz_0, - yref_0, W_0) - self.cost.Vx_0 = np.zeros((0,0)) - self.cost.Vu_0 = np.zeros((0,0)) - self.cost.Vz_0 = np.zeros((0,0)) - self.cost.W_0 = np.zeros((0,0)) - self.model.cost_y_expr_0 = [] - self.cost.yref_0 = np.zeros((0,)) - - elif self.cost.cost_type_0 == "NONLINEAR_LS": - self.model.cost_expr_ext_cost_0 = \ - self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_0, yref_0, W_0) - - if cost_hessian == 'GAUSS_NEWTON': - self.model.cost_expr_ext_cost_custom_hess_0 = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_0, yref_0, W_0, self.model.x, self.model.u, self.model.z) - - self.cost.W_0 = np.zeros((0,0)) - self.model.cost_y_expr_0 = [] - self.cost.yref_0 = np.zeros((0,)) - elif self.cost.cost_type_0 == "CONVEX_OVER_NONLINEAR": - self.model.cost_expr_ext_cost_0 = \ - self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0, - self.model.cost_y_expr_0, yref_0) - self.model.cost_r_in_psi_expr_0 = [] - self.model.cost_psi_expr_0 = [] - self.model.cost_y_expr_0 = [] - self.cost.yref_0 = np.zeros((0,)) + def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.SX, ca.MX]] = None, W: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + self._translate_cost_term_to_external(stage_type='path', yref=yref, W=W, cost_hessian=cost_hessian) - if self.cost.cost_type_0 is not None: - self.cost.cost_type_0 = 'EXTERNAL' + def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, ca.MX]] = None, W_e: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + self._translate_cost_term_to_external(stage_type='terminal', yref=yref_e, W=W_e, cost_hessian=cost_hessian) - def translate_intermediate_cost_term_to_external(self, yref: Optional[Union[ca.SX, ca.MX]] = None, W: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + def _translate_cost_term_to_external(self, stage_type: str, yref: Optional[Union[ca.SX, ca.MX]], W: Optional[Union[ca.SX, ca.MX]], cost_hessian: str): + """Generic helper to translate a cost term (initial/path/terminal) to EXTERNAL. + stage_type: one of 'initial', 'path', 'terminal' + """ if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") + suffix = {'initial': '_0', 'path': '', 'terminal': '_e'}[stage_type] + # cost_type attribute + cost_type = getattr(self.cost, f'cost_type{suffix}') + if cost_hessian == 'GAUSS_NEWTON': - if self.cost.cost_type not in ['LINEAR_LS', 'NONLINEAR_LS']: - raise ValueError(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type = {self.cost.cost_type}.") + allowed = ['LINEAR_LS', 'NONLINEAR_LS'] + if stage_type == 'initial': + allowed.append(None) + if cost_type not in allowed: + raise ValueError(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type{suffix} = {cost_type}.") casadi_symbolics_type = type(self.model.x) + # yref and W default to current values + yref_attr = getattr(self.cost, f'yref{suffix}') + W_attr = getattr(self.cost, f'W{suffix}') + if yref is None: - yref = self.cost.yref + yref = yref_attr else: - if yref.shape[0] != self.cost.yref.shape[0]: - raise ValueError(f"yref has wrong shape, got {yref.shape}, expected {self.cost.yref.shape}.") - + if yref.shape[0] != yref_attr.shape[0]: + raise ValueError(f"yref{suffix} has wrong shape, got {yref.shape}, expected {yref_attr.shape}.") if not isinstance(yref, casadi_symbolics_type): - raise TypeError(f"yref has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") + raise TypeError(f"yref{suffix} has wrong type, got {type(yref)}, expected {casadi_symbolics_type}.") if W is None: - W = self.cost.W + W = W_attr else: - if W.shape != self.cost.W.shape: - raise ValueError(f"W has wrong shape, got {W.shape}, expected {self.cost.W.shape}.") - + if W.shape != W_attr.shape: + raise ValueError(f"W{suffix} has wrong shape, got {W.shape}, expected {W_attr.shape}.") if not isinstance(W, casadi_symbolics_type): - raise TypeError(f"W has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") + raise TypeError(f"W{suffix} has wrong type, got {type(W)}, expected {casadi_symbolics_type}.") - if self.cost.cost_type == "LINEAR_LS": - self.model.cost_expr_ext_cost = \ - self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, - self.cost.Vx, self.cost.Vu, self.cost.Vz, - yref, W) - self.cost.Vx = np.zeros((0,0)) - self.cost.Vu = np.zeros((0,0)) - self.cost.Vz = np.zeros((0,0)) - self.cost.W = np.zeros((0,0)) - self.model.cost_y_expr = [] - self.cost.yref = np.zeros((0,)) + # perform translation + if cost_type == 'LINEAR_LS': + Vx = getattr(self.cost, f'Vx{suffix}') - elif self.cost.cost_type == "NONLINEAR_LS": - self.model.cost_expr_ext_cost = \ - self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, W) - if cost_hessian == 'GAUSS_NEWTON': - self.model.cost_expr_ext_cost_custom_hess = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr, yref, W, self.model.x, self.model.u, self.model.z) + # pass default as Vu, Vz are not present at terminal state + Vu = getattr(self.cost, f'Vu{suffix}', None) + Vz = getattr(self.cost, f'Vz{suffix}', None) - self.cost.W = np.zeros((0,0)) - self.model.cost_y_expr = [] - self.cost.yref = np.zeros((0,)) + translated = self.__translate_ls_cost_to_external_cost( + self.model.x, self.model.u, self.model.z, Vx, Vu, Vz, yref, W) + setattr(self.model, f'cost_expr_ext_cost{suffix}', translated) - elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR": - self.model.cost_expr_ext_cost = \ - self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr, - self.model.cost_y_expr, yref) + # clear original cost matrices/refs if they exist + setattr(self.cost, f'Vx{suffix}', np.zeros((0,0))) + setattr(self.cost, f'yref{suffix}', np.zeros((0,))) + setattr(self.cost, f'W{suffix}', np.zeros((0,0))) - self.model.cost_r_in_psi_expr = [] - self.model.cost_psi_expr = [] - self.model.cost_y_expr = [] - self.cost.yref = np.zeros((0,)) + if stage_type != 'terminal': + setattr(self.cost, f'Vz{suffix}', np.zeros((0,0))) + setattr(self.cost, f'Vu{suffix}', np.zeros((0,0))) - self.cost.cost_type = 'EXTERNAL' + setattr(self.model, f'cost_y_expr{suffix}', []) + elif cost_type == 'NONLINEAR_LS': + y_expr = getattr(self.model, f'cost_y_expr{suffix}') + translated = self.__translate_nls_cost_to_external_cost(y_expr, yref, W) + setattr(self.model, f'cost_expr_ext_cost{suffix}', translated) - def translate_terminal_cost_term_to_external(self, yref_e: Optional[Union[ca.SX, ca.MX]] = None, W_e: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): - if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: - raise ValueError(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") - - if cost_hessian == 'GAUSS_NEWTON': - if self.cost.cost_type_e not in ['LINEAR_LS', 'NONLINEAR_LS']: - raise ValueError(f"cost_hessian 'GAUSS_NEWTON' is only supported for LINEAR_LS, NONLINEAR_LS cost types, got cost_type_e = {self.cost.cost_type_e}.") - - casadi_symbolics_type = type(self.model.x) - - if yref_e is None: - yref_e = self.cost.yref_e - else: - if yref_e.shape[0] != self.cost.yref_e.shape[0]: - raise ValueError(f"yref_e has wrong shape, got {yref_e.shape}, expected {self.cost.yref_e.shape}.") - - if not isinstance(yref_e, casadi_symbolics_type): - raise TypeError(f"yref_e has wrong type, got {type(yref_e)}, expected {casadi_symbolics_type}.") - - if W_e is None: - W_e = self.cost.W_e - else: - if W_e.shape != self.cost.W_e.shape: - raise ValueError(f"W_e has wrong shape, got {W_e.shape}, expected {self.cost.W_e.shape}.") - - if not isinstance(W_e, casadi_symbolics_type): - raise TypeError(f"W_e has wrong type, got {type(W_e)}, expected {casadi_symbolics_type}.") - - if self.cost.cost_type_e == "LINEAR_LS": - self.model.cost_expr_ext_cost_e = \ - self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z, - self.cost.Vx_e, None, None, - yref_e, W_e) - - self.cost.Vx_e = np.zeros((0,0)) - self.cost.W_e = np.zeros((0,0)) - self.model.cost_y_expr_e = [] - self.cost.yref_e = np.zeros((0,)) - - elif self.cost.cost_type_e == "NONLINEAR_LS": - self.model.cost_expr_ext_cost_e = \ - self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_e, yref_e, W_e) if cost_hessian == 'GAUSS_NEWTON': - self.model.cost_expr_ext_cost_custom_hess_e = self.__get_gn_hessian_expression_from_nls_cost(self.model.cost_y_expr_e, yref_e, W_e, self.model.x, [], self.model.z) - - self.cost.W_e = np.zeros((0,0)) - self.model.cost_y_expr_e = [] - self.cost.yref_e = np.zeros((0,)) + u_ = [] if stage_type == 'terminal' else self.model.u + hess = self.__get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, self.model.x, u_) + # TODO: atm only the hessian in ux can be customized, add z? + setattr(self.model, f'cost_expr_ext_cost_custom_hess{suffix}', hess) + + setattr(self.cost, f'W{suffix}', np.zeros((0,0))) + setattr(self.cost, f'yref{suffix}', np.zeros((0,))) + setattr(self.model, f'cost_y_expr{suffix}', []) + + elif cost_type == 'CONVEX_OVER_NONLINEAR': + r = getattr(self.model, f'cost_r_in_psi_expr{suffix}') + psi = getattr(self.model, f'cost_psi_expr{suffix}') + y_expr = getattr(self.model, f'cost_y_expr{suffix}') + translated = self.__translate_conl_cost_to_external_cost(r, psi, y_expr, yref) + setattr(self.model, f'cost_expr_ext_cost{suffix}', translated) - elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR": - self.model.cost_expr_ext_cost_e = \ - self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e, - self.model.cost_y_expr_e, yref_e) + if cost_hessian == 'GAUSS_NEWTON': + u_ = [] if stage_type == 'terminal' else self.model.u + custom_outer_hess = getattr(self.model, f'cost_conl_custom_outer_hess{suffix}') + hess = self.__get_ggn_hessian_expression_from_conl_cost(r, psi, y_expr, yref, self.model.x, u_, custom_outer_hess) + # TODO: atm only the hessian in ux can be customized + setattr(self.model, f'cost_expr_ext_cost_custom_hess{suffix}', hess) - self.model.cost_r_in_psi_expr_e = [] - self.model.cost_psi_expr_e = [] - self.model.cost_y_expr_e = [] - self.cost.yref_e = np.zeros((0,)) + setattr(self.model, f'cost_r_in_psi_expr{suffix}', []) + setattr(self.model, f'cost_psi_expr{suffix}', []) + setattr(self.model, f'cost_y_expr{suffix}', []) + setattr(self.cost, f'yref{suffix}', np.zeros((0,))) - self.cost.cost_type_e = 'EXTERNAL' + # set cost type to EXTERNAL if any cost was present + if cost_type is not None: + setattr(self.cost, f'cost_type{suffix}', 'EXTERNAL') @staticmethod @@ -1927,7 +1850,7 @@ def __translate_nls_cost_to_external_cost(y_expr, yref, W): return 0.5 * (res.T @ ca.sparsify(W) @ res) @staticmethod - def __get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, x, u, z): + def __get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, x, u): res = y_expr - yref ux = ca.vertcat(u, x) inner_jac = ca.jacobian(res, ux) @@ -1938,6 +1861,19 @@ def __get_gn_hessian_expression_from_nls_cost(y_expr, yref, W, x, u, z): def __translate_conl_cost_to_external_cost(r, psi, y_expr, yref): return ca.substitute(psi, r, y_expr - yref) + @staticmethod + def __get_ggn_hessian_expression_from_conl_cost(r, psi, y_expr, yref, x, u, custom_outer_hess = None): + res = y_expr - yref + ux = ca.vertcat(u, x) + inner_jac = ca.jacobian(res, ux) + + if is_empty(custom_outer_hess): + outer_hess = ca.substitute(ca.hessian(psi, r)[0], r, res) + else: + outer_hess = custom_outer_hess + return inner_jac.T @ outer_hess @ inner_jac + + def formulate_constraint_as_L2_penalty( self, constr_expr: ca.SX, diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 3267c0edcf..4642a74789 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -651,6 +651,7 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag outer_hess_expr = outer_hess_fun(inner_expr, t, p, p_global) outer_hess_is_diag = outer_hess_expr.sparsity().is_diag() + # if residual dimension <= 4, do not exploit diagonal structure if casadi_length(res_expr) <= 4: outer_hess_is_diag = 0 From 18c4d439f5f03fc1e91a1580fd66fbc80cdf08c9 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 14 Oct 2025 14:32:07 +0200 Subject: [PATCH 159/164] Python: refactors deprecation warnings (#1665) This PR refactors all deprecation warnings in the Python interface to use the `@deprecated` decorator from the `deprecated.sphinx` module instead of print statements. In addition, this removes the deprecated function `eval_param_sens` and updates `setup.py` to align version of `acados_template` with acados version --- .../acados_template/acados_dims.py | 5 +- .../acados_template/acados_model.py | 3 +- .../acados_template/acados_ocp_options.py | 29 +++++------ .../acados_template/acados_ocp_solver.py | 50 +------------------ .../acados_template/acados_sim.py | 5 +- .../acados_template/acados_template/utils.py | 4 +- interfaces/acados_template/setup.py | 13 ++--- 7 files changed, 34 insertions(+), 75 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_dims.py b/interfaces/acados_template/acados_template/acados_dims.py index 9b6056d2a8..7dc476b17a 100644 --- a/interfaces/acados_template/acados_template/acados_dims.py +++ b/interfaces/acados_template/acados_template/acados_dims.py @@ -29,6 +29,8 @@ # POSSIBILITY OF SUCH DAMAGE.; # +from deprecated.sphinx import deprecated + def check_int_value(name, value, *, positive=False, nonnegative=False): if not isinstance(value, int): raise TypeError(f"Invalid {name} value: expected an integer, got {type(value).__name__}.") @@ -387,11 +389,10 @@ def n_global_data(self): return self.__n_global_data @property + @deprecated(version="0.4.0", reason="Use `ocp.solver_options.N` instead.") def N(self): """ :math:`N` - Number of shooting intervals. - DEPRECATED: use ocp.solver_options.N instead. - Type: int; default: None""" return self.__N diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 5da0814efc..afaab66810 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -33,6 +33,7 @@ import casadi as ca import numpy as np +from deprecated.sphinx import deprecated from casadi import MX, SX @@ -911,8 +912,8 @@ def substitute(self, var: Union[ca.SX, ca.MX], expr_new: Union[ca.SX, ca.MX]) -> return + @deprecated(version="0.4.0", reason="Use `reformulate_with_polynomial_control()` instead.") def augment_model_with_polynomial_control(self, degree: int) -> None: - print("Deprecation warning: augment_model_with_polynomial_control() is deprecated and has been renamed to reformulate_with_polynomial_control().") self.reformulate_with_polynomial_control(degree=degree) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index b7dfa6a167..cb9d22cfb4 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -31,6 +31,7 @@ import os +from deprecated.sphinx import deprecated from .utils import check_if_nparray_and_flatten INTEGRATOR_TYPES = ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK') @@ -536,11 +537,11 @@ def nlp_qp_tol_min_comp(self): return self.__nlp_qp_tol_min_comp @property + @deprecated(version="0.4.0", reason="Use globalization_fixed_step_length instead.") def nlp_solver_step_length(self): """ This option is deprecated and has new name: globalization_fixed_step_length """ - print("The option nlp_solver_step_length is deprecated and has new name: globalization_fixed_step_length") return self.__globalization_fixed_step_length @property @@ -982,11 +983,11 @@ def globalization_alpha_min(self): return self.__globalization_alpha_min @property + @deprecated(version="0.4.0", reason="Use globalization_alpha_min instead.") def alpha_min(self): """ The option alpha_min is deprecated and has new name: globalization_alpha_min """ - print("The option alpha_min is deprecated and has new name: globalization_alpha_min") return self.globalization_alpha_min @property @@ -1040,11 +1041,11 @@ def globalization_alpha_reduction(self): return self.__globalization_alpha_reduction @property + @deprecated(version="0.4.0", reason="Use globalization_alpha_reduction instead.") def alpha_reduction(self): """ The option alpha_reduction is deprecated and has new name: globalization_alpha_reduction """ - print("The option alpha_reduction is deprecated and has new name: globalization_alpha_reduction") return self.globalization_alpha_reduction @property @@ -1057,11 +1058,11 @@ def globalization_line_search_use_sufficient_descent(self): return self.__globalization_line_search_use_sufficient_descent @property + @deprecated(version="0.4.0", reason="Use globalization_line_search_use_sufficient_descent instead.") def line_search_use_sufficient_descent(self): """ The option line_search_use_sufficient_descent is deprecated and has new name: globalization_line_search_use_sufficient_descent """ - print("The option line_search_use_sufficient_descent is deprecated and has new name: globalization_line_search_use_sufficient_descent") return self.globalization_line_search_use_sufficient_descent @property @@ -1079,11 +1080,11 @@ def globalization_eps_sufficient_descent(self): return self.__globalization_eps_sufficient_descent @property + @deprecated(version="0.4.0", reason="Use globalization_line_search_use_sufficient_descent instead.") def eps_sufficient_descent(self): """ The option eps_sufficient_descent is deprecated and has new name: globalization_line_search_use_sufficient_descent """ - print("The option eps_sufficient_descent is deprecated and has new name: globalization_line_search_use_sufficient_descent") return self.globalization_line_search_use_sufficient_descent @property @@ -1108,11 +1109,11 @@ def globalization_full_step_dual(self): return self.__globalization_full_step_dual @property + @deprecated(version="0.4.0", reason="Use globalization_full_step_dual instead.") def full_step_dual(self): """ The option full_step_dual is deprecated and has new name: globalization_full_step_dual """ - print("The option full_step_dual is deprecated and has new name: globalization_full_step_dual") return self.globalization_full_step_dual @property @@ -1449,9 +1450,9 @@ def with_value_sens_wrt_params(self): return self.__with_value_sens_wrt_params @property + @deprecated(version="0.4.0", reason="Use with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") def num_threads_in_batch_solve(self): """ - DEPRECATED, use the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver. Integer indicating how many threads should be used within the batch solve. If more than one thread should be used, the solver is compiled with openmp. Default: 1. @@ -1671,8 +1672,8 @@ def globalization_alpha_min(self, globalization_alpha_min): self.__globalization_alpha_min = globalization_alpha_min @alpha_min.setter + @deprecated(version="0.4.0", reason="Use globalization_alpha_min instead.") def alpha_min(self, alpha_min): - print("This option is deprecated and has new name: globalization_alpha_min") self.globalization_alpha_min = alpha_min @globalization_alpha_reduction.setter @@ -1680,8 +1681,8 @@ def globalization_alpha_reduction(self, globalization_alpha_reduction): self.__globalization_alpha_reduction = globalization_alpha_reduction @alpha_reduction.setter + @deprecated(version="0.4.0", reason="Use globalization_alpha_reduction instead.") def alpha_reduction(self, globalization_alpha_reduction): - print("This option is deprecated and has new name: globalization_alpha_reduction") self.globalization_alpha_reduction = globalization_alpha_reduction @globalization_line_search_use_sufficient_descent.setter @@ -1692,8 +1693,8 @@ def globalization_line_search_use_sufficient_descent(self, globalization_line_se raise ValueError(f'Invalid value for globalization_line_search_use_sufficient_descent. Possible values are 0, 1, got {globalization_line_search_use_sufficient_descent}') @line_search_use_sufficient_descent.setter + @deprecated(version="0.4.0", reason="Use globalization_line_search_use_sufficient_descent instead.") def line_search_use_sufficient_descent(self, globalization_line_search_use_sufficient_descent): - print("This option is deprecated and has new name: globalization_line_search_use_sufficient_descent") if globalization_line_search_use_sufficient_descent in [0, 1]: self.__globalization_line_search_use_sufficient_descent = globalization_line_search_use_sufficient_descent else: @@ -1714,8 +1715,8 @@ def globalization_full_step_dual(self, globalization_full_step_dual): raise ValueError(f'Invalid value for globalization_full_step_dual. Possible values are 0, 1, got {globalization_full_step_dual}') @full_step_dual.setter + @deprecated(version="0.4.0", reason="Use globalization_full_step_dual instead.") def full_step_dual(self, globalization_full_step_dual): - print("This option is deprecated and has new name: globalization_full_step_dual") self.globalization_full_step_dual = globalization_full_step_dual @@ -1818,8 +1819,8 @@ def globalization_eps_sufficient_descent(self, globalization_eps_sufficient_desc raise ValueError('Invalid globalization_eps_sufficient_descent value. globalization_eps_sufficient_descent must be a positive float.') @eps_sufficient_descent.setter + @deprecated(version="0.4.0", reason="Use globalization_eps_sufficient_descent instead.") def eps_sufficient_descent(self, globalization_eps_sufficient_descent): - print("This option is deprecated and has new name: globalization_eps_sufficient_descent") self.globalization_eps_sufficient_descent = globalization_eps_sufficient_descent @sim_method_num_stages.setter @@ -1933,8 +1934,8 @@ def nlp_qp_tol_safety_factor(self, nlp_qp_tol_safety_factor): self.__nlp_qp_tol_safety_factor = nlp_qp_tol_safety_factor @nlp_solver_step_length.setter + @deprecated(version="0.4.0", reason="Use globalization_fixed_step_length instead.") def nlp_solver_step_length(self, nlp_solver_step_length): - print("The option nlp_solver_step_length is deprecated and has new name: globalization_fixed_step_length") self.globalization_fixed_step_length = nlp_solver_step_length @nlp_solver_warm_start_first_qp.setter @@ -2294,8 +2295,8 @@ def ext_cost_num_hess(self, ext_cost_num_hess): raise ValueError('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.') @num_threads_in_batch_solve.setter + @deprecated(version="0.4.0", reason="Set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") def num_threads_in_batch_solve(self, num_threads_in_batch_solve): - print("Warning: num_threads_in_batch_solve is deprecated, set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index b305f4a309..8d01165e98 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -49,6 +49,7 @@ import numpy as np import scipy.linalg +from deprecated.sphinx import deprecated from .builders import CMakeBuilder from .acados_ocp import AcadosOcp from .acados_multiphase_ocp import AcadosMultiphaseOcp @@ -673,8 +674,8 @@ def eval_and_get_optimal_value_gradient(self, with_respect_to: str = "initial_st return grad + @deprecated(version="0.4.0", reason="Use eval_and_get_optimal_value_gradient() instead.") def get_optimal_value_gradient(self, with_respect_to: str = "initial_state") -> np.ndarray: - print("Deprecation warning: get_optimal_value_gradient() is deprecated and has been renamed to eval_and_get_optimal_value_gradient().") return self.eval_and_get_optimal_value_gradient(with_respect_to) @@ -936,53 +937,6 @@ def eval_adjoint_solution_sensitivity(self, raise NotImplementedError(f"with_respect_to {with_respect_to} not implemented.") - - def eval_param_sens(self, index: int, stage: int=0, field="ex"): - """ - Calculate the sensitivity of the current solution with respect to the initial state component of index. - - NOTE: Correct computation of sensitivities requires - - (1) HPIPM as QP solver, - - (2) the usage of an exact Hessian, - - (3) positive definiteness of the full-space Hessian if the square-root version of the Riccati recursion is used - OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), - (4) the solution of at least one QP in advance to evaluation of the sensitivities as the factorization is reused. - - :param index: integer corresponding to initial state index in range(nx) - """ - - print("WARNING: eval_param_sens() is deprecated. Please use eval_solution_sensitivity() instead!") - - self._ensure_solution_sensitivities_available(False) - - field = field.encode('utf-8') - - if not isinstance(index, int): - raise TypeError('AcadosOcpSolver.eval_param_sens(): index must be Integer.') - - if field == "ex": - if not stage == 0: - raise NotImplementedError('AcadosOcpSolver.eval_param_sens(): only stage == 0 is supported.') - nx = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, "x".encode('utf-8')) - - if index < 0 or index > nx: - raise ValueError(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.') - - elif field == "p_global": - nparam = self.__acados_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "p".encode('utf-8')) - - if index < 0 or index > nparam: - raise IndexError(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nparam-1], got: {index}.') - - # actual eval_param - self.__acados_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out) - - return - - def get(self, stage_: int, field_: str): """ Get the last solution of the solver. diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index 4a2850c014..a49681ed62 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -33,6 +33,7 @@ import numpy as np from typing import Optional from copy import deepcopy +from deprecated.sphinx import deprecated from .acados_model import AcadosModel from .acados_dims import AcadosSimDims from .builders import CMakeBuilder @@ -171,9 +172,9 @@ def ext_fun_expand_dyn(self): @property + @deprecated(version="0.4.0", reason="Set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") def num_threads_in_batch_solve(self): """ - DEPRECATED, use the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver. Integer indicating how many threads should be used within the batch solve. If more than one thread should be used, the sim solver is compiled with openmp. Default: 1. @@ -296,8 +297,8 @@ def sim_method_jac_reuse(self, sim_method_jac_reuse): raise ValueError('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.') @num_threads_in_batch_solve.setter + @deprecated(version="0.4.0", reason="Set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") def num_threads_in_batch_solve(self, num_threads_in_batch_solve): - print("Warning: num_threads_in_batch_solve is deprecated, set the flag with_batch_functionality instead and pass the number of threads directly to the BatchSolver.") if isinstance(num_threads_in_batch_solve, int) and num_threads_in_batch_solve > 0: self.__num_threads_in_batch_solve = num_threads_in_batch_solve else: diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index d5754f5d65..a687abf334 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -36,6 +36,7 @@ import sys import platform import urllib.request +from deprecated.sphinx import deprecated from subprocess import DEVNULL, STDOUT, call if os.name == 'nt': from ctypes import wintypes @@ -386,9 +387,8 @@ def format_class_dict(d): out[k.replace(k, out_key)] = v return out +@deprecated(version="0.4.0", reason="Use get_simulink_default_opts() instead.") def get_default_simulink_opts() -> dict: - print("get_default_simulink_opts is deprecated, use get_simulink_default_opts instead." - + " This function will be removed in a future release.") return get_simulink_default_opts() diff --git a/interfaces/acados_template/setup.py b/interfaces/acados_template/setup.py index 33c277a260..9e27add338 100644 --- a/interfaces/acados_template/setup.py +++ b/interfaces/acados_template/setup.py @@ -33,16 +33,16 @@ import sys print(sys.version_info) -if sys.version_info < (3,5): - sys.exit('Python version 3.5 or later required. Exiting.') +if sys.version_info < (3,8): + sys.exit('Python version 3.8 or later required. Exiting.') setup(name='acados_template', - version='0.1', - python_requires='>=3.5', + version='0.5.1', + python_requires='>=3.8', description='A templating framework for acados', url='http://github.com/acados/acados', - author='Andrea Zanelli', - maintainer="Jonathan Frey", + author='The acados authors.', + maintainer="The acados authors.", license='BSD 2-Clause', packages = find_packages(), include_package_data = True, @@ -59,6 +59,7 @@ 'matplotlib', 'future-fstrings', 'cython', + 'Deprecated', ], package_data={'': [ 'simulink_default_opts.json', From 8f3956530206a9eb6fbf810ba5bb56c4ee53c02f Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <84231306+Pandatheon@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:36:42 +0200 Subject: [PATCH 160/164] `AcadosCasadiOcp`: Fix dimension mismatch when using MX (#1660) - Fix dimension mismatch when using MX - Fix OCP without terminal cost - Split `AcadosCasdiOcp` and `AcadosCasdiOcpSolver` for simplification. - Add `anderson_scqp_experiment.py` into test --- .github/workflows/full_build.yml | 7 + .../acados_template/acados_casadi_ocp.py | 740 ++++++++++++++++++ .../acados_casadi_ocp_solver.py | 683 +--------------- 3 files changed, 749 insertions(+), 681 deletions(-) create mode 100644 interfaces/acados_template/acados_template/acados_casadi_ocp.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index f562a17aa0..06ed2a9e8a 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -159,6 +159,13 @@ jobs: python test_casadi_LICQ_violation.py python test_casadi_closed_loop.py + - name: Anderson_acceleration + working-directory: ${{ github.workspace }}/examples/acados_python/anderson_acceleration + shell: bash + run: | + source ${{ github.workspace }}/acadosenv/bin/activate + python anderson_scqp_experiment.py + py_non_ocp: needs: call_core_build runs-on: ubuntu-22.04 diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp.py b/interfaces/acados_template/acados_template/acados_casadi_ocp.py new file mode 100644 index 0000000000..41fe1758fc --- /dev/null +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp.py @@ -0,0 +1,740 @@ +# -*- coding: future_fstrings -*- +# +# 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.; +# +import casadi as ca + +import numpy as np + +from .utils import casadi_length, is_casadi_SX, is_empty +from .acados_ocp import AcadosOcp +from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpFlattenedIterate + +class AcadosCasadiOcp: + + def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): + """ + Creates an equivalent CasADi NLP formulation of the OCP. + Experimental, not fully implemented yet. + + :return: nlp_dict, bounds_dict, w0 (initial guess) + """ + ocp.make_consistent() + + # create index map for variables + self._index_map = { + # indices of variables within w + 'x_in_w': [], + 'u_in_w': [], + # indices of slack variables within w + 'sl_in_w': [], + 'su_in_w': [], + # indices of parameters within p_nlp + 'p_in_p_nlp': [], + 'p_global_in_p_nlp': [], + 'yref_in_p_nlp': [], + # indices of state bounds and control bounds within lam_x(lam_w) in casadi formulation + 'lam_bx_in_lam_w':[], + 'lam_bu_in_lam_w': [], + # indices of dynamic constraints within g in casadi formulation + 'pi_in_lam_g': [], + # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation + 'lam_gnl_in_lam_g': [], + # indices of slack variables within lam_g in casadi formulation + 'lam_sl_in_lam_g': [], + 'lam_su_in_lam_g': [], + } + self.offset_w = 0 # offset for the indices in index_map + self.offset_gnl = 0 + self.offset_lam = 0 + self.offset_p = 0 + + # unpack + model = ocp.model + dims = ocp.dims + constraints = ocp.constraints + cost = ocp.cost + solver_options = ocp.solver_options + N_horizon = solver_options.N_horizon + + # check what is not supported yet + if any([dims.nsbx, dims.nsbx_e, dims.nsbu]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for variables (x and u) yet.") + if any([dims.nsg, dims.nsg_e, dims.nsphi, dims.nsphi_e]): + raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for general linear and convex-over-nonlinear constraints (g, phi).") + if dims.nz > 0: + raise NotImplementedError("AcadosCasadiOcpSolver does not support algebraic variables (z) yet.") + if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: + raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") + + # create primal variables and slack variables + ca_symbol = model.get_casadi_symbol() + xtraj_nodes = [] + utraj_nodes = [] + sl_nodes = [] + su_nodes = [] + if multiple_shooting: + for i in range(N_horizon+1): + self._append_node_for_variables('x', ca_symbol, xtraj_nodes, i, dims) + self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) + self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) + self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) + else: # single_shooting + self._x_traj_fun = [] + for i in range(N_horizon): + self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) + self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) + self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) + + # setup state and control bounds + lb_xtraj_nodes = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] + ub_xtraj_nodes = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] + lb_utraj_nodes = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + ub_utraj_nodes = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + # setup slack variables + # TODO: speicify different bounds for lsbu, lsbx, lsg, lsh ,lsphi + lb_slack_nodes = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ + ([0* ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ + ([0 * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) + ub_slack_nodes = ([np.inf * ca.DM.ones((dims.ns, 1))] if dims.ns_0 else []) + \ + ([np.inf * ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ + ([np.inf * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) + if multiple_shooting: + for i in range(0, N_horizon+1): + self._set_bounds_indices('x', i, lb_xtraj_nodes, ub_xtraj_nodes, constraints, dims) + self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) + else: # single_shooting + for i in range(0, N_horizon): + self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) + + ### Concatenate primal variables and bounds + # w = [x0, u0, sl0, su0, x1, u1, ...] + w_sym_list = [] + lbw_list = [] + ubw_list = [] + w0_list = [] + x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) + if multiple_shooting: + for i in range(N_horizon+1): + self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_nodes, lb_xtraj_nodes, ub_xtraj_nodes, i, dims, x_guess) + if i < N_horizon: + self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) + else: # single_shooting + xtraj_nodes.append(x_guess) + self._x_traj_fun.append(x_guess) + for i in range(N_horizon+1): + if i < N_horizon: + self._append_variables_and_bounds('u', w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) + self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) + + nw = self.offset_w # number of primal variables + + # setup parameter nodes and value + ptraj_nodes = [] + p_list = [] + for i in range(N_horizon+1): + self._append_node_and_value_for_params(ca_symbol, p_list, ptraj_nodes, i, ocp) + p_list.append(ocp.p_global_values) + self._index_map['p_global_in_p_nlp'].append(list(range(self.offset_p, self.offset_p+dims.np_global))) + self.offset_p += dims.np_global + + # vectorize + w = ca.vertcat(*w_sym_list) + lbw = ca.vertcat(*lbw_list) + ubw = ca.vertcat(*ubw_list) + p_nlp = ca.vertcat(*ptraj_nodes, model.p_global) + + ### Create Constraints + g = [] + lbg = [] + ubg = [] + if with_hessian: + lam_g = [] + hess_l = ca.DM.zeros((nw, nw)) + # dynamics constraints + if solver_options.integrator_type == "DISCRETE": + f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) + elif solver_options.integrator_type == "ERK": + param = ca.vertcat(model.u, model.p, model.p_global) + ca_expl_ode = ca.Function('ca_expl_ode', [model.x, param], [model.f_expl_expr]) + f_discr_fun = ca.simpleRK(ca_expl_ode, solver_options.sim_method_num_steps[0], solver_options.sim_method_num_stages[0]) + else: + raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") + + for i in range(N_horizon+1): + # add dynamics constraints + if multiple_shooting: + if i < N_horizon: + utraj_node = utraj_nodes[i] if dims.nu > 0 else ca_symbol('dummy_u', 0, 1) + ptraj_node = ptraj_nodes[i][:dims.np] if dims.np > 0 else ca_symbol('dummy_p', 0, 1) + if solver_options.integrator_type == "DISCRETE": + dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], utraj_node, ptraj_node, model.p_global) + elif solver_options.integrator_type == "ERK": + param = ca.vertcat(utraj_node, ptraj_node, model.p_global) + dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], param, solver_options.time_steps[i]) + self._append_constraints(i, 'dyn', g, lbg, ubg, + g_expr = dyn_equality, + lbg_expr = np.zeros((dims.nx, 1)), + ubg_expr = np.zeros((dims.nx, 1)), + cons_dim=dims.nx) + if with_hessian: + # add hessian of dynamics constraints + lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) + lam_g.append(lam_g_dyn) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_dyn: + adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + else: # single_shooting + if i < N_horizon: + utraj_node = utraj_nodes[i] if dims.nu > 0 else ca_symbol('dummy_u', 0, 1) + ptraj_node = ptraj_nodes[i][:dims.np] if dims.np > 0 else ca_symbol('dummy_p', 0, 1) + x_current = xtraj_nodes[i] + if solver_options.integrator_type == "DISCRETE": + x_next = f_discr_fun(x_current, utraj_node, ptraj_node, model.p_global) + elif solver_options.integrator_type == "ERK": + param = ca.vertcat(utraj_node, ptraj_node, model.p_global) + x_next = f_discr_fun(x_current, param, solver_options.time_steps[i]) + xtraj_nodes.append(x_next) + self._x_traj_fun.append(f_discr_fun) + + # Nonlinear Constraints + constraint_dict = self._get_constraint_node(i, N_horizon, xtraj_nodes, utraj_nodes, ptraj_nodes, model, constraints, dims) + + # add linear constraints + if constraint_dict['ng'] > 0: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = constraint_dict['linear_constr_expr'], + lbg_expr = constraint_dict['lg'], + ubg_expr = constraint_dict['ug'], + cons_dim=constraint_dict['ng']) + + # add nonlinear constraints using constraint_dict directly (no locals) + if constraint_dict['nh'] > 0: + if constraint_dict['nsh'] > 0: + # h_fun with slack variables + soft_h_indices = constraint_dict['idxsh'] + hard_h_indices = np.array([h for h in range(len(constraint_dict['lh'])) if h not in constraint_dict['idxsh']]) + for index_in_nh in range(constraint_dict['nh']): + if index_in_nh in soft_h_indices: + index_in_soft = soft_h_indices.tolist().index(index_in_nh) + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = constraint_dict['h_i_nlp_expr'][index_in_nh] + sl_nodes[i][index_in_soft], + lbg_expr = constraint_dict['lh'][index_in_nh], + ubg_expr = np.inf * ca.DM.ones((1, 1)), + cons_dim=1, + sl=True) + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = constraint_dict['h_i_nlp_expr'][index_in_nh] - su_nodes[i][index_in_soft], + lbg_expr = -np.inf * ca.DM.ones((1, 1)), + ubg_expr = constraint_dict['uh'][index_in_nh], + cons_dim=1, + su=True) + elif index_in_nh in hard_h_indices: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = constraint_dict['h_i_nlp_expr'][index_in_nh], + lbg_expr = constraint_dict['lh'][index_in_nh], + ubg_expr = constraint_dict['uh'][index_in_nh], + cons_dim=1) + else: + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = constraint_dict['h_i_nlp_expr'], + lbg_expr = constraint_dict['lh'], + ubg_expr = constraint_dict['uh'], + cons_dim=constraint_dict['nh']) + if with_hessian: + # add hessian contribution + lam_h = ca_symbol(f'lam_h_{i}', constraint_dict['nh'], 1) + lam_g.append(lam_h) + if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: + adj = ca.jtimes(constraint_dict['h_i_nlp_expr'], w, lam_h, True) + hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) + + # add convex-over-nonlinear constraints + if constraint_dict['nphi'] > 0: + conl_constr_fun = constraint_dict['conl_constr_fun'] + utraj_node = utraj_nodes[i] if dims.nu > 0 else ca_symbol('dummy_u', 0, 1) + ptraj_node = ptraj_nodes[i][:dims.np] if dims.np > 0 else ca_symbol('dummy_p', 0, 1) + self._append_constraints(i, 'gnl', g, lbg, ubg, + g_expr = conl_constr_fun(xtraj_nodes[i], utraj_node, ptraj_node, model.p_global), + lbg_expr = constraint_dict['lphi'], + ubg_expr = constraint_dict['uphi'], + cons_dim=constraint_dict['nphi']) + if with_hessian: + lam_phi = ca_symbol(f'lam_phi_{i}', constraint_dict['nphi'], 1) + lam_g.append(lam_phi) + # always use CONL Hessian approximation here, disregarding inner second derivative + outer_hess_r = ca.vertcat(*[ca.hessian(constraint_dict['con_phi_expr'][j], constraint_dict['con_r_in_phi'])[0] for j in range(constraint_dict['nphi'])]) + outer_hess_r = ca.substitute(outer_hess_r, constraint_dict['con_r_in_phi'], constraint_dict['con_r_expr']) + r_in_nlp = ca.substitute(constraint_dict['con_r_expr'], model.x, xtraj_nodes[-1]) + dr_dw = ca.jacobian(r_in_nlp, w) + hess_l += dr_dw.T @ outer_hess_r @ dr_dw + + ### Cost and residual + nlp_cost = 0 + residual_list = [] + for i in range(N_horizon+1): + cost_dict = self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, p_nlp, sl_nodes, su_nodes, ocp) + + cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, cost_dict['p_for_model'], model.p_global], [cost_dict['cost_expr']]) + cost_i = cost_fun_i(cost_dict['xtraj_node'], cost_dict['utraj_node'], cost_dict['ptraj_node'], model.p_global) + nlp_cost += solver_options.cost_scaling[i] * cost_i + + if cost_dict['residual_expr'] is not None: + residual_fun_i = ca.Function(f'residual_fun_{i}', [model.x, model.u, cost_dict['p_for_model'], model.p_global], [cost_dict['residual_expr']]) + residual_i = ca.sqrt(solver_options.cost_scaling[i]) * ca.sqrt(cost_dict['W_mat']) @ residual_fun_i(cost_dict['xtraj_node'], cost_dict['utraj_node'], cost_dict['ptraj_node'], model.p_global) + residual_list.append(residual_i) + if cost_dict['ns'] > 0: + penalty_expr_i = 0.5 * ca.mtimes(cost_dict['sl_node'].T, ca.mtimes(np.diag(cost_dict['Zl']), cost_dict['sl_node'])) + \ + ca.mtimes(cost_dict['zl'].reshape(-1, 1).T, cost_dict['sl_node']) + \ + 0.5 * ca.mtimes(cost_dict['su_node'].T, ca.mtimes(np.diag(cost_dict['Zu']), cost_dict['su_node'])) + \ + ca.mtimes(cost_dict['zu'].reshape(-1, 1).T, cost_dict['su_node']) + nlp_cost += solver_options.cost_scaling[i] * penalty_expr_i + + if with_hessian: + lam_f = ca_symbol('lam_f', 1, 1) + if (ocp.cost.cost_type == "LINEAR_LS" and ocp.cost.cost_type_0 == "LINEAR_LS" and ocp.cost.cost_type_e == "LINEAR_LS"): + hess_l += lam_f * ca.hessian(nlp_cost, w)[0] + elif (ocp.cost.cost_type == "NONLINEAR_LS" and ocp.cost.cost_type_0 == "NONLINEAR_LS" and ocp.cost.cost_type_e == "NONLINEAR_LS"): + if ocp.solver_options.hessian_approx == 'EXACT': + hess_l += lam_f * ca.hessian(nlp_cost, w)[0] + elif ocp.solver_options.hessian_approx == 'GAUSS_NEWTON': + Gauss_Newton = 0 + for i in range(len(residual_list)): + dr_dw = ca.jacobian(residual_list[i], w) + Gauss_Newton += dr_dw.T @ dr_dw + hess_l += lam_f * Gauss_Newton + else: + raise NotImplementedError("Hessian approximation not implemented for this cost type.") + lam_g_vec = ca.vertcat(*lam_g) + nlp_hess_l_custom = ca.Function('nlp_hess_l', [w, p_nlp, lam_f, lam_g_vec], [ca.triu(hess_l)]) + assert casadi_length(lam_g_vec) == casadi_length(ca.vertcat(*g)), f"Number of nonlinear constraints does not match the expected number, got {casadi_length(lam_g_vec)} != {casadi_length(ca.vertcat(*g))}." + else: + nlp_hess_l_custom = None + hess_l = None + + # create NLP + nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} + bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} + w0 = np.concatenate(w0_list) + p = np.concatenate(p_list) + + self.__nlp = nlp + self.__bounds = bounds + self.__w0 = w0 + self.__p = p + self.__index_map = self._index_map + self.__nlp_hess_l_custom = nlp_hess_l_custom + self.__hess_approx_expr = hess_l + + def _append_node_for_variables(self, _field, ca_symbol, node:list, i, dims): + """ + Helper function to append a node to the NLP formulation. + """ + if i == 0: + ns = dims.ns_0 + nx = dims.nx + nu = dims.nu + elif i < dims.N: + ns = dims.ns + nx = dims.nx + nu = dims.nu + else: + ns = dims.ns_e + nx = dims.nx + nu = 0 + + if _field == 'x': + node.append(ca_symbol(f'x{i}', nx, 1)) + elif _field == 'u': + node.append(ca_symbol(f'u{i}', nu, 1)) + elif _field == 'sl': + if ns > 0: + node.append(ca_symbol(f'sl{i}', ns, 1)) + else: + node.append([]) + elif _field == 'su': + if ns > 0: + node.append(ca_symbol(f'su{i}', ns, 1)) + else: + node.append([]) + + def _append_node_and_value_for_params(self, ca_symbol, p_list, node, stage, ocp:AcadosOcp): + """ + Helper function to append parameter values to the NLP formulation. + """ + if stage == 0: + ny = ocp.dims.ny_0 + yref = ocp.cost.yref_0 + elif stage < ocp.solver_options.N_horizon: + ny = ocp.dims.ny + yref = ocp.cost.yref + else: + ny = ocp.dims.ny_e + yref = ocp.cost.yref_e + + node.append(ca.vertcat(ca_symbol(f'p{stage}', ocp.dims.np, 1), ca_symbol(f'yref{stage}', ny, 1))) + p_list.append(ocp.parameter_values) + self._index_map['p_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ocp.dims.np))) + self.offset_p += ocp.dims.np + p_list.append(yref) + self._index_map['yref_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ny))) + self.offset_p += ny + + def _set_bounds_indices(self, _field, i, lb_node, ub_node, constraints, dims): + """ + Helper function to set bounds and indices for the primal variables. + """ + if i == 0: + idxbx = constraints.idxbx_0 + idxbu = constraints.idxbu + lbx = constraints.lbx_0 + ubx = constraints.ubx_0 + lbu = constraints.lbu + ubu = constraints.ubu + elif i < dims.N: + idxbx = constraints.idxbx + idxbu = constraints.idxbu + lbx = constraints.lbx + ubx = constraints.ubx + lbu = constraints.lbu + ubu = constraints.ubu + elif i == dims.N: + idxbx = constraints.idxbx_e + lbx = constraints.lbx_e + ubx = constraints.ubx_e + if i < dims.N: + if _field == 'x': + lb_node[i][idxbx] = lbx + ub_node[i][idxbx] = ubx + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) + self.offset_lam += dims.nx + elif _field == 'u': + lb_node[i][idxbu] = lbu + ub_node[i][idxbu] = ubu + self._index_map['lam_bu_in_lam_w'].append(list(self.offset_lam + idxbu)) + self.offset_lam += dims.nu + elif i == dims.N: + if _field == 'x': + lb_node[i][idxbx] = lbx + ub_node[i][idxbx] = ubx + self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) + self.offset_lam += dims.nx + + def _append_variables_and_bounds(self, _field, w_sym_list, lbw_list, ubw_list, w0_list, + node_list, lb_node_list, ub_node_list, i, dims, x_guess): + """ + Unified helper function to add a primal or slack variable to the NLP formulation. + """ + if _field == "x": + # Add state variable + w_sym_list.append(node_list[i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(x_guess) + self._index_map['x_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nx))) + self.offset_w += dims.nx + + elif _field == "u": + # Add control variable + w_sym_list.append(node_list[i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((dims.nu,))) + self._index_map['u_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nu))) + self.offset_w += dims.nu + + elif _field == "slack": + # Add slack variables (sl and su) + if i == 0 and dims.ns_0: + ns = dims.ns_0 + elif i < dims.N and dims.ns: + ns = dims.ns + elif i == dims.N and dims.ns_e: + ns = dims.ns_e + else: + self._index_map['sl_in_w'].append([]) + self._index_map['su_in_w'].append([]) + return + + # Add sl + w_sym_list.append(node_list[0][i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((ns,))) + # Add su + w_sym_list.append(node_list[1][i]) + lbw_list.append(lb_node_list[i]) + ubw_list.append(ub_node_list[i]) + w0_list.append(np.zeros((ns,))) + + self._index_map['sl_in_w'].append(list(range(self.offset_w, self.offset_w + ns))) + self._index_map['su_in_w'].append(list(range(self.offset_w + ns, self.offset_w + 2 * ns))) + self.offset_w += 2 * ns + + else: + raise ValueError(f"Unsupported for: {_field}") + + def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr, cons_dim, sl=False, su=False): + """ + Helper function to append constraints to the NLP formulation. + """ + g.append(g_expr) + lbg.append(lbg_expr) + ubg.append(ubg_expr) + if _field == 'dyn': + self._index_map['pi_in_lam_g'].append(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) + self.offset_gnl += cons_dim + elif _field == 'gnl': + if not sl and not su: + self._index_map['lam_gnl_in_lam_g'][i].extend(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) + self.offset_gnl += cons_dim + elif sl: + self._index_map['lam_sl_in_lam_g'][i].append(self.offset_gnl) + self.offset_gnl += 1 + elif su: + self._index_map['lam_su_in_lam_g'][i].append(self.offset_gnl) + self.offset_gnl += 1 + + def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, p_nlp, sl_node, su_node, ocp: AcadosOcp): + """ + Helper function to get the cost node for a given stage. + """ + cost_dict = {} + dims = ocp.dims + model = ocp.model + cost = ocp.cost + p_index = self._index_map['p_in_p_nlp'][i] + yref_index = self._index_map['yref_in_p_nlp'][i] + yref = p_nlp[yref_index] + if i == 0: + if cost.cost_type_0 == "NONLINEAR_LS" and not is_empty(ocp.model.cost_y_expr_0): + y = ocp.model.cost_y_expr_0 + residual_expr = y - yref + elif cost.cost_type_0 == "LINEAR_LS" and not is_empty(cost.Vx_0) and not is_empty(cost.Vu_0): + y = cost.Vx_0 @ model.x + cost.Vu_0 @ model.u + residual_expr = y - yref + else: + residual_expr = None + cost_dict['xtraj_node'] = xtraj_node[0] + cost_dict['utraj_node'] = utraj_node[0] + cost_dict['ptraj_node'] = p_nlp[p_index + yref_index] + cost_dict['sl_node'] = sl_node[0] + cost_dict['su_node'] = su_node[0] + cost_dict['cost_expr'] = ocp.get_initial_cost_expression(yref) + cost_dict['residual_expr'] = residual_expr + cost_dict['p_for_model'] = ca.vertcat(ocp.model.p, yref) + cost_dict['ns'] = dims.ns_0 + cost_dict['W_mat'] = cost.W_0 + cost_dict['zl'] = cost.zl_0 + cost_dict['Zl'] = cost.Zl_0 + cost_dict['zu'] = cost.zu_0 + cost_dict['Zu'] = cost.Zu_0 + elif i < N_horizon: + if cost.cost_type == "NONLINEAR_LS" and not is_empty(ocp.model.cost_y_expr): + y = ocp.model.cost_y_expr + residual_expr = y - yref + elif cost.cost_type == "LINEAR_LS" and not is_empty(cost.Vx) and not is_empty(cost.Vu): + y = cost.Vx @ model.x + cost.Vu @ model.u + residual_expr = y - yref + else: + residual_expr = None + cost_dict['xtraj_node'] = xtraj_node[i] + cost_dict['utraj_node'] = utraj_node[i] + cost_dict['ptraj_node'] = p_nlp[p_index + yref_index] + cost_dict['sl_node'] = sl_node[i] + cost_dict['su_node'] = su_node[i] + cost_dict['cost_expr'] = ocp.get_path_cost_expression(yref) + cost_dict['residual_expr'] = residual_expr + cost_dict['p_for_model'] = ca.vertcat(ocp.model.p, yref) + cost_dict['ns'] = dims.ns + cost_dict['W_mat'] = cost.W + cost_dict['zl'] = cost.zl + cost_dict['Zl'] = cost.Zl + cost_dict['zu'] = cost.zu + cost_dict['Zu'] = cost.Zu + else: + if cost.cost_type_e == "NONLINEAR_LS" and not is_empty(ocp.model.cost_y_expr_e): + y = ocp.model.cost_y_expr_e + residual_expr = y - yref + elif cost.cost_type_e == "LINEAR_LS" and not is_empty(cost.Vx_e): + y = cost.Vx_e @ model.x + residual_expr = y - yref + else: + residual_expr = None + cost_dict['xtraj_node'] = xtraj_node[-1] + cost_dict['utraj_node'] = utraj_node[-1] + cost_dict['ptraj_node'] = p_nlp[p_index + yref_index] + cost_dict['sl_node'] = sl_node[-1] + cost_dict['su_node'] = su_node[-1] + cost_dict['cost_expr'] = ocp.get_terminal_cost_expression(yref) + cost_dict['residual_expr'] = residual_expr + cost_dict['p_for_model'] = ca.vertcat(ocp.model.p, yref) + cost_dict['ns'] = dims.ns_e + cost_dict['W_mat'] = cost.W_e + cost_dict['zl'] = cost.zl_e + cost_dict['Zl'] = cost.Zl_e + cost_dict['zu'] = cost.zu_e + cost_dict['Zu'] = cost.Zu_e + + return cost_dict + + def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims): + """ + Helper function to get the constraint node for a given stage. + """ + # dict to store constraint function for each node + cons_dict = {} + if i == 0 and N_horizon > 0: + cons_dict['lg'], cons_dict['ug'] = constraints.lg, constraints.ug + cons_dict['lh'], cons_dict['uh'] = constraints.lh_0, constraints.uh_0 + cons_dict['lphi'], cons_dict['uphi'] = constraints.lphi_0, constraints.uphi_0 + cons_dict['ng'], cons_dict['nh'], cons_dict['nphi'] = dims.ng, dims.nh_0, dims.nphi_0 + cons_dict['nsg'], cons_dict['nsh'], cons_dict['nsphi'], cons_dict['idxsh'] = dims.nsg, dims.nsh_0, dims.nsphi_0, constraints.idxsh_0 + + # linear function + if dims.ng > 0: + cons_dict['C'] = constraints.C + cons_dict['D'] = constraints.D + cons_dict['linear_constr_expr'] = ca.mtimes(cons_dict['C'], xtraj_node[i]) + ca.mtimes(cons_dict['D'], utraj_node[i]) + # nonlinear function + cons_dict['h_fun'] = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) + cons_dict['h_i_nlp_expr'] = cons_dict['h_fun'](xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) + # compound nonlinear constraint + cons_dict['conl_constr_fun'] = None + if dims.nphi_0 > 0: + cons_dict['con_phi_expr'], cons_dict['con_r_in_phi'], cons_dict['con_r_expr'] = model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0 + conl_expr = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) + cons_dict['conl_constr_fun'] = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) + + elif i < N_horizon: + # populate cons_dict for intermediate stage (mirror initial-stage style) + cons_dict['lg'], cons_dict['ug'] = constraints.lg, constraints.ug + cons_dict['lh'], cons_dict['uh'] = constraints.lh, constraints.uh + cons_dict['lphi'], cons_dict['uphi'] = constraints.lphi, constraints.uphi + cons_dict['ng'], cons_dict['nh'], cons_dict['nphi'] = dims.ng, dims.nh, dims.nphi + cons_dict['nsg'], cons_dict['nsh'], cons_dict['nsphi'], cons_dict['idxsh'] = dims.nsg, dims.nsh, dims.nsphi, constraints.idxsh + + # linear function + if dims.ng > 0: + cons_dict['C'] = constraints.C + cons_dict['D'] = constraints.D + cons_dict['linear_constr_expr'] = ca.mtimes(cons_dict['C'], xtraj_node[i]) + ca.mtimes(cons_dict['D'], utraj_node[i]) + # nonlinear function + cons_dict['h_fun'] = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) + cons_dict['h_i_nlp_expr'] = cons_dict['h_fun'](xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) + # compound nonlinear constraint + cons_dict['conl_constr_fun'] = None + if dims.nphi > 0: + cons_dict['con_phi_expr'], cons_dict['con_r_in_phi'], cons_dict['con_r_expr'] = model.con_phi_expr, model.con_r_in_phi, model.con_r_expr + conl_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) + cons_dict['conl_constr_fun'] = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) + + else: + # populate cons_dict for terminal stage + cons_dict['lg'], cons_dict['ug'] = constraints.lg_e, constraints.ug_e + cons_dict['lh'], cons_dict['uh'] = constraints.lh_e, constraints.uh_e + cons_dict['lphi'], cons_dict['uphi'] = constraints.lphi_e, constraints.uphi_e + cons_dict['ng'], cons_dict['nh'], cons_dict['nphi'] = dims.ng_e, dims.nh_e, dims.nphi_e + cons_dict['nsg'], cons_dict['nsh'], cons_dict['nsphi'], cons_dict['idxsh'] = dims.nsg_e, dims.nsh_e, dims.nsphi_e, constraints.idxsh_e + + # linear function + if dims.ng_e > 0: + cons_dict['C'] = constraints.C_e + cons_dict['linear_constr_expr'] = ca.mtimes(cons_dict['C'], xtraj_node[i]) + # nonlinear function + cons_dict['h_fun'] = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) + cons_dict['h_i_nlp_expr'] = cons_dict['h_fun'](xtraj_node[i], ptraj_node[i][:dims.np], model.p_global) + # compound nonlinear constraint + cons_dict['conl_constr_fun'] = None + if dims.nphi_e > 0: + cons_dict['con_phi_expr'], cons_dict['con_r_in_phi'], cons_dict['con_r_expr'] = model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e + conl_expr = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) + cons_dict['conl_constr_fun'] = ca.Function('conl_constr_e_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) + + self._index_map['lam_gnl_in_lam_g'].append([]) + self._index_map['lam_sl_in_lam_g'].append([]) + self._index_map['lam_su_in_lam_g'].append([]) + + return cons_dict + + @property + def nlp(self): + """ + Dict containing all symbolics needed to create a `casadi.nlpsol` solver, namely entries 'x', 'p', 'g', 'f'. + """ + return self.__nlp + + @property + def w0(self): + """ + Default initial guess for primal variable vector w for given NLP. + """ + return self.__w0 + + @property + def p_nlp_values(self): + """ + Default parameter vector p_nlp in the form of [p_0,..., p_N, p_global] for given NLP. + """ + return self.__p + + @property + def bounds(self): + """ + Dict containing all bounds needed to call a `casadi.nlpsol` solver. + """ + return self.__bounds + + @property + def index_map(self): + """ + Dict containing indices corresponding to stage-wise values of the original OCP, specifically: + - 'x_in_w': indices of x variables within primal variable vector w + - 'u_in_w': indices of u variables within primal variable vector w + - 'pi_in_lam_g': indices of dynamic constraints within g in casadi formulation + - 'lam_gnl_in_lam_g' indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation + """ + return self.__index_map + + @property + def nlp_hess_l_custom(self): + """ + CasADi Function that computes the Hessian approximation of the Lagrangian in the format required by `casadi.nlpsol`, i.e. as upper triangular matrix. + The Hessian is set up to match the Hessian that would be used in acados and depends on the solver options. + """ + return self.__nlp_hess_l_custom + + @property + def hess_approx_expr(self): + """ + CasADi expression corresponding to the Hessian approximation of the Lagrangian. + Expression corresponding to what is output by the `nlp_hess_l_custom` function. + """ + return self.__hess_approx_expr \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 441deb3e40..0a4ce58236 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -35,689 +35,10 @@ import numpy as np -from .utils import casadi_length, is_casadi_SX, is_empty +from .utils import casadi_length from .acados_ocp import AcadosOcp from .acados_ocp_iterate import AcadosOcpIterate, AcadosOcpFlattenedIterate - -class AcadosCasadiOcp: - - def __init__(self, ocp: AcadosOcp, with_hessian=False, multiple_shooting=True): - """ - Creates an equivalent CasADi NLP formulation of the OCP. - Experimental, not fully implemented yet. - - :return: nlp_dict, bounds_dict, w0 (initial guess) - """ - ocp.make_consistent() - - # create index map for variables - self._index_map = { - # indices of variables within w - 'x_in_w': [], - 'u_in_w': [], - # indices of slack variables within w - 'sl_in_w': [], - 'su_in_w': [], - # indices of parameters within p_nlp - 'p_in_p_nlp': [], - 'p_global_in_p_nlp': [], - 'yref_in_p_nlp': [], - # indices of state bounds and control bounds within lam_x(lam_w) in casadi formulation - 'lam_bx_in_lam_w':[], - 'lam_bu_in_lam_w': [], - # indices of dynamic constraints within g in casadi formulation - 'pi_in_lam_g': [], - # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation - 'lam_gnl_in_lam_g': [], - # indices of slack variables within lam_g in casadi formulation - 'lam_sl_in_lam_g': [], - 'lam_su_in_lam_g': [], - } - self.offset_w = 0 # offset for the indices in index_map - self.offset_gnl = 0 - self.offset_lam = 0 - self.offset_p = 0 - - # unpack - model = ocp.model - dims = ocp.dims - constraints = ocp.constraints - cost = ocp.cost - solver_options = ocp.solver_options - N_horizon = solver_options.N_horizon - - # check what is not supported yet - if any([dims.nsbx, dims.nsbx_e, dims.nsbu]): - raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for variables (x and u) yet.") - if any([dims.nsg, dims.nsg_e, dims.nsphi, dims.nsphi_e]): - raise NotImplementedError("AcadosCasadiOcpSolver does not support slack variables (s) for general linear and convex-over-nonlinear constraints (g, phi).") - if dims.nz > 0: - raise NotImplementedError("AcadosCasadiOcpSolver does not support algebraic variables (z) yet.") - if ocp.solver_options.integrator_type not in ["DISCRETE", "ERK"]: - raise NotImplementedError(f"AcadosCasadiOcpSolver does not support integrator type {ocp.solver_options.integrator_type} yet.") - - # create primal variables and slack variables - ca_symbol = model.get_casadi_symbol() - xtraj_nodes = [] - utraj_nodes = [] - sl_nodes = [] - su_nodes = [] - if multiple_shooting: - for i in range(N_horizon+1): - self._append_node_for_variables('x', ca_symbol, xtraj_nodes, i, dims) - self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) - self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) - self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) - else: # single_shooting - self._x_traj_fun = [] - for i in range(N_horizon): - self._append_node_for_variables('u', ca_symbol, utraj_nodes, i, dims) - self._append_node_for_variables('sl', ca_symbol, sl_nodes, i, dims) - self._append_node_for_variables('su', ca_symbol, su_nodes, i, dims) - - # setup state and control bounds - lb_xtraj_nodes = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - ub_xtraj_nodes = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - lb_utraj_nodes = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - ub_utraj_nodes = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] - # setup slack variables - # TODO: speicify different bounds for lsbu, lsbx, lsg, lsh ,lsphi - lb_slack_nodes = ([0 * ca.DM.ones((dims.ns_0, 1))] if dims.ns_0 else []) + \ - ([0* ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ - ([0 * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) - ub_slack_nodes = ([np.inf * ca.DM.ones((dims.ns, 1))] if dims.ns_0 else []) + \ - ([np.inf * ca.DM.ones((dims.ns, 1)) for _ in range(N_horizon-1)] if dims.ns else []) + \ - ([np.inf * ca.DM.ones((dims.ns_e, 1))] if dims.ns_e else []) - if multiple_shooting: - for i in range(0, N_horizon+1): - self._set_bounds_indices('x', i, lb_xtraj_nodes, ub_xtraj_nodes, constraints, dims) - self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) - else: # single_shooting - for i in range(0, N_horizon): - self._set_bounds_indices('u', i, lb_utraj_nodes, ub_utraj_nodes, constraints, dims) - - ### Concatenate primal variables and bounds - # w = [x0, u0, sl0, su0, x1, u1, ...] - w_sym_list = [] - lbw_list = [] - ubw_list = [] - w0_list = [] - x_guess = ocp.constraints.x0 if ocp.constraints.has_x0 else np.zeros((dims.nx,)) - if multiple_shooting: - for i in range(N_horizon+1): - self._append_variables_and_bounds('x', w_sym_list, lbw_list, ubw_list, w0_list, xtraj_nodes, lb_xtraj_nodes, ub_xtraj_nodes, i, dims, x_guess) - if i < N_horizon: - self._append_variables_and_bounds('u',w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) - self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) - else: # single_shooting - xtraj_nodes.append(x_guess) - self._x_traj_fun.append(x_guess) - for i in range(N_horizon+1): - if i < N_horizon: - self._append_variables_and_bounds('u', w_sym_list, lbw_list, ubw_list, w0_list, utraj_nodes, lb_utraj_nodes, ub_utraj_nodes, i, dims, x_guess) - self._append_variables_and_bounds('slack', w_sym_list, lbw_list, ubw_list, w0_list, [sl_nodes, su_nodes], lb_slack_nodes, ub_slack_nodes, i, dims, x_guess) - - nw = self.offset_w # number of primal variables - - # setup parameter nodes and value - ptraj_nodes = [] - p_list = [] - for i in range(N_horizon+1): - self._append_node_and_value_for_params(ca_symbol, p_list, ptraj_nodes, i, ocp) - p_list.append(ocp.p_global_values) - self._index_map['p_global_in_p_nlp'].append(list(range(self.offset_p, self.offset_p+dims.np_global))) - self.offset_p += dims.np_global - - # vectorize - w = ca.vertcat(*w_sym_list) - lbw = ca.vertcat(*lbw_list) - ubw = ca.vertcat(*ubw_list) - p_nlp = ca.vertcat(*ptraj_nodes, model.p_global) - - ### Create Constraints - g = [] - lbg = [] - ubg = [] - if with_hessian: - lam_g = [] - hess_l = ca.DM.zeros((nw, nw)) - # dynamics constraints - if solver_options.integrator_type == "DISCRETE": - f_discr_fun = ca.Function('f_discr_fun', [model.x, model.u, model.p, model.p_global], [model.disc_dyn_expr]) - elif solver_options.integrator_type == "ERK": - param = ca.vertcat(model.u, model.p, model.p_global) - ca_expl_ode = ca.Function('ca_expl_ode', [model.x, param], [model.f_expl_expr]) - f_discr_fun = ca.simpleRK(ca_expl_ode, solver_options.sim_method_num_steps[0], solver_options.sim_method_num_stages[0]) - else: - raise NotImplementedError(f"Integrator type {solver_options.integrator_type} not supported.") - - for i in range(N_horizon+1): - # add dynamics constraints - if multiple_shooting: - if i < N_horizon: - if solver_options.integrator_type == "DISCRETE": - dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) - elif solver_options.integrator_type == "ERK": - param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) - dyn_equality = xtraj_nodes[i+1] - f_discr_fun(xtraj_nodes[i], param, solver_options.time_steps[i]) - self._append_constraints(i, 'dyn', g, lbg, ubg, - g_expr = dyn_equality, - lbg_expr = np.zeros((dims.nx, 1)), - ubg_expr = np.zeros((dims.nx, 1)), - cons_dim=dims.nx) - if with_hessian: - # add hessian of dynamics constraints - lam_g_dyn = ca_symbol(f'lam_g_dyn{i}', dims.nx, 1) - lam_g.append(lam_g_dyn) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_dyn: - adj = ca.jtimes(dyn_equality, w, lam_g_dyn, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - else: # single_shooting - if i < N_horizon: - x_current = xtraj_nodes[i] - if solver_options.integrator_type == "DISCRETE": - x_next = f_discr_fun(x_current, utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) - elif solver_options.integrator_type == "ERK": - param = ca.vertcat(utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global) - x_next = f_discr_fun(x_current, param, solver_options.time_steps[i]) - xtraj_nodes.append(x_next) - self._x_traj_fun.append(f_discr_fun) - # Nonlinear Constraints - # initial stage - lg, ug, lh, uh, lphi, uphi, ng, nh, nphi, nsg, nsh, nsphi, idxsh, linear_constr_expr, h_i_nlp_expr, conl_constr_fun =\ - self._get_constraint_node(i, N_horizon, xtraj_nodes, utraj_nodes, ptraj_nodes, model, constraints, dims) - - # add linear constraints - if ng > 0: - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = linear_constr_expr, - lbg_expr = lg, - ubg_expr = ug, - cons_dim=ng) - - # add nonlinear constraints - if nh > 0: - if nsh > 0: - # h_fun with slack variables - soft_h_indices = idxsh - hard_h_indices = np.array([h for h in range(len(lh)) if h not in idxsh]) - for index_in_nh in range(nh): - if index_in_nh in soft_h_indices: - index_in_soft = soft_h_indices.tolist().index(index_in_nh) - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr[index_in_nh] + sl_nodes[i][index_in_soft], - lbg_expr = lh[index_in_nh], - ubg_expr = np.inf * ca.DM.ones((1, 1)), - cons_dim=1, - sl=True) - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr[index_in_nh] - su_nodes[i][index_in_soft], - lbg_expr = -np.inf * ca.DM.ones((1, 1)), - ubg_expr = uh[index_in_nh], - cons_dim=1, - su=True) - elif index_in_nh in hard_h_indices: - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr[index_in_nh], - lbg_expr = lh[index_in_nh], - ubg_expr = uh[index_in_nh], - cons_dim=1) - else: - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = h_i_nlp_expr, - lbg_expr = lh, - ubg_expr = uh, - cons_dim=nh) - if with_hessian: - # add hessian contribution - lam_h = ca_symbol(f'lam_h_{i}', dims.nh, 1) - lam_g.append(lam_h) - if ocp.solver_options.hessian_approx == 'EXACT' and ocp.solver_options.exact_hess_constr: - adj = ca.jtimes(h_i_nlp_expr, w, lam_h, True) - hess_l += ca.jacobian(adj, w, {"symmetric": is_casadi_SX(model.x)}) - - # add convex-over-nonlinear constraints - if nphi > 0: - self._append_constraints(i, 'gnl', g, lbg, ubg, - g_expr = conl_constr_fun(xtraj_nodes[i], utraj_nodes[i], ptraj_nodes[i][:dims.np], model.p_global), - lbg_expr = lphi, - ubg_expr = uphi, - cons_dim=nphi) - if with_hessian: - lam_phi = ca_symbol(f'lam_phi', nphi, 1) - lam_g.append(lam_phi) - # always use CONL Hessian approximation here, disregarding inner second derivative - outer_hess_r = ca.vertcat(*[ca.hessian(model.con_phi_expr[i], model.con_r_in_phi)[0] for i in range(dims.nphi)]) - outer_hess_r = ca.substitute(outer_hess_r, model.con_r_in_phi, model.con_r_expr) - r_in_nlp = ca.substitute(model.con_r_expr, model.x, xtraj_nodes[-1]) - dr_dw = ca.jacobian(r_in_nlp, w) - hess_l += dr_dw.T @ outer_hess_r @ dr_dw - - ### Cost and residual - nlp_cost = 0 - residual_list = [] - for i in range(N_horizon+1): - xtraj_node_i, utraj_node_i, ptraj_node_i, sl_node_i, su_node_i, cost_expr_i, residual_expr_i, p_for_model, ns, W_mat, zl, Zl, zu, Zu = \ - self._get_cost_node(i, N_horizon, xtraj_nodes, utraj_nodes, p_nlp, sl_nodes, su_nodes, ocp) - - cost_fun_i = ca.Function(f'cost_fun_{i}', [model.x, model.u, p_for_model, model.p_global], [cost_expr_i]) - cost_i = cost_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) - nlp_cost += solver_options.cost_scaling[i] * cost_i - - if residual_expr_i is not None: - residual_fun_i = ca.Function(f'residual_fun_{i}', [model.x, model.u, p_for_model, model.p_global], [residual_expr_i]) - residual_i = ca.sqrt(solver_options.cost_scaling[i]) * ca.sqrt(W_mat) @ residual_fun_i(xtraj_node_i, utraj_node_i, ptraj_node_i, model.p_global) - residual_list.append(residual_i) - if ns: - penalty_expr_i = 0.5 * ca.mtimes(sl_node_i.T, ca.mtimes(np.diag(Zl), sl_node_i)) + \ - ca.mtimes(zl.reshape(-1, 1).T, sl_node_i) + \ - 0.5 * ca.mtimes(su_node_i.T, ca.mtimes(np.diag(Zu), su_node_i)) + \ - ca.mtimes(zu.reshape(-1, 1).T, su_node_i) - nlp_cost += solver_options.cost_scaling[i] * penalty_expr_i - - if with_hessian: - lam_f = ca_symbol('lam_f', 1, 1) - if (ocp.cost.cost_type == "LINEAR_LS" and ocp.cost.cost_type_0 == "LINEAR_LS" and ocp.cost.cost_type_e == "LINEAR_LS"): - hess_l += lam_f * ca.hessian(nlp_cost, w)[0] - elif (ocp.cost.cost_type == "NONLINEAR_LS" and ocp.cost.cost_type_0 == "NONLINEAR_LS" and ocp.cost.cost_type_e == "NONLINEAR_LS"): - if ocp.solver_options.hessian_approx == 'EXACT': - hess_l += lam_f * ca.hessian(nlp_cost, w)[0] - elif ocp.solver_options.hessian_approx == 'GAUSS_NEWTON': - Gauss_Newton = 0 - for i in range(len(residual_list)): - dr_dw = ca.jacobian(residual_list[i], w) - Gauss_Newton += dr_dw.T @ dr_dw - hess_l += lam_f * Gauss_Newton - else: - raise NotImplementedError("Hessian approximation not implemented for this cost type.") - lam_g_vec = ca.vertcat(*lam_g) - nlp_hess_l_custom = ca.Function('nlp_hess_l', [w, p_nlp, lam_f, lam_g_vec], [ca.triu(hess_l)]) - assert casadi_length(lam_g_vec) == casadi_length(ca.vertcat(*g)), f"Number of nonlinear constraints does not match the expected number, got {casadi_length(lam_g_vec)} != {casadi_length(ca.vertcat(*g))}." - else: - nlp_hess_l_custom = None - hess_l = None - - # create NLP - nlp = {"x": w, "p": p_nlp, "g": ca.vertcat(*g), "f": nlp_cost} - bounds = {"lbx": lbw, "ubx": ubw, "lbg": ca.vertcat(*lbg), "ubg": ca.vertcat(*ubg)} - w0 = np.concatenate(w0_list) - p = np.concatenate(p_list) - - self.__nlp = nlp - self.__bounds = bounds - self.__w0 = w0 - self.__p = p - self.__index_map = self._index_map - self.__nlp_hess_l_custom = nlp_hess_l_custom - self.__hess_approx_expr = hess_l - - def _append_node_for_variables(self, _field, ca_symbol, node:list, i, dims): - """ - Helper function to append a node to the NLP formulation. - """ - if i == 0: - ns = dims.ns_0 - nx = dims.nx - nu = dims.nu - elif i < dims.N: - ns = dims.ns - nx = dims.nx - nu = dims.nu - else: - ns = dims.ns_e - nx = dims.nx - nu = 0 - - if _field == 'x': - node.append(ca_symbol(f'x{i}', nx, 1)) - elif _field == 'u': - node.append(ca_symbol(f'u{i}', nu, 1)) - elif _field == 'sl': - if ns > 0: - node.append(ca_symbol(f'sl{i}', ns, 1)) - else: - node.append([]) - elif _field == 'su': - if ns > 0: - node.append(ca_symbol(f'su{i}', ns, 1)) - else: - node.append([]) - - def _append_node_and_value_for_params(self, ca_symbol, p_list, node, stage, ocp:AcadosOcp): - """ - Helper function to append parameter values to the NLP formulation. - """ - if stage == 0: - ny = ocp.dims.ny_0 - yref = ocp.cost.yref_0 - elif stage < ocp.solver_options.N_horizon: - ny = ocp.dims.ny - yref = ocp.cost.yref - else: - ny = ocp.dims.ny_e - yref = ocp.cost.yref_e - - node.append(ca.vertcat(ca_symbol(f'p{stage}', ocp.dims.np, 1), ca_symbol(f'yref{stage}', ny, 1))) - p_list.append(ocp.parameter_values) - self._index_map['p_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ocp.dims.np))) - self.offset_p += ocp.dims.np - p_list.append(yref) - self._index_map['yref_in_p_nlp'].append(list(range(self.offset_p, self.offset_p + ny))) - self.offset_p += ny - - def _set_bounds_indices(self, _field, i, lb_node, ub_node, constraints, dims): - """ - Helper function to set bounds and indices for the primal variables. - """ - if i == 0: - idxbx = constraints.idxbx_0 - idxbu = constraints.idxbu - lbx = constraints.lbx_0 - ubx = constraints.ubx_0 - lbu = constraints.lbu - ubu = constraints.ubu - elif i < dims.N: - idxbx = constraints.idxbx - idxbu = constraints.idxbu - lbx = constraints.lbx - ubx = constraints.ubx - lbu = constraints.lbu - ubu = constraints.ubu - elif i == dims.N: - idxbx = constraints.idxbx_e - lbx = constraints.lbx_e - ubx = constraints.ubx_e - if i < dims.N: - if _field == 'x': - lb_node[i][idxbx] = lbx - ub_node[i][idxbx] = ubx - self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) - self.offset_lam += dims.nx - elif _field == 'u': - lb_node[i][idxbu] = lbu - ub_node[i][idxbu] = ubu - self._index_map['lam_bu_in_lam_w'].append(list(self.offset_lam + idxbu)) - self.offset_lam += dims.nu - elif i == dims.N: - if _field == 'x': - lb_node[i][idxbx] = lbx - ub_node[i][idxbx] = ubx - self._index_map['lam_bx_in_lam_w'].append(list(self.offset_lam + idxbx)) - self.offset_lam += dims.nx - - def _append_variables_and_bounds(self, _field, w_sym_list, lbw_list, ubw_list, w0_list, - node_list, lb_node_list, ub_node_list, i, dims, x_guess): - """ - Unified helper function to add a primal or slack variable to the NLP formulation. - """ - if _field == "x": - # Add state variable - w_sym_list.append(node_list[i]) - lbw_list.append(lb_node_list[i]) - ubw_list.append(ub_node_list[i]) - w0_list.append(x_guess) - self._index_map['x_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nx))) - self.offset_w += dims.nx - - elif _field == "u": - # Add control variable - w_sym_list.append(node_list[i]) - lbw_list.append(lb_node_list[i]) - ubw_list.append(ub_node_list[i]) - w0_list.append(np.zeros((dims.nu,))) - self._index_map['u_in_w'].append(list(range(self.offset_w, self.offset_w + dims.nu))) - self.offset_w += dims.nu - - elif _field == "slack": - # Add slack variables (sl and su) - if i == 0 and dims.ns_0: - ns = dims.ns_0 - elif i < dims.N and dims.ns: - ns = dims.ns - elif i == dims.N and dims.ns_e: - ns = dims.ns_e - else: - self._index_map['sl_in_w'].append([]) - self._index_map['su_in_w'].append([]) - return - - # Add sl - w_sym_list.append(node_list[0][i]) - lbw_list.append(lb_node_list[i]) - ubw_list.append(ub_node_list[i]) - w0_list.append(np.zeros((ns,))) - # Add su - w_sym_list.append(node_list[1][i]) - lbw_list.append(lb_node_list[i]) - ubw_list.append(ub_node_list[i]) - w0_list.append(np.zeros((ns,))) - - self._index_map['sl_in_w'].append(list(range(self.offset_w, self.offset_w + ns))) - self._index_map['su_in_w'].append(list(range(self.offset_w + ns, self.offset_w + 2 * ns))) - self.offset_w += 2 * ns - - else: - raise ValueError(f"Unsupported for: {_field}") - - def _append_constraints(self, i, _field, g, lbg, ubg, g_expr, lbg_expr, ubg_expr, cons_dim, sl=False, su=False): - """ - Helper function to append constraints to the NLP formulation. - """ - g.append(g_expr) - lbg.append(lbg_expr) - ubg.append(ubg_expr) - if _field == 'dyn': - self._index_map['pi_in_lam_g'].append(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) - self.offset_gnl += cons_dim - elif _field == 'gnl': - if not sl and not su: - self._index_map['lam_gnl_in_lam_g'][i].extend(list(range(self.offset_gnl, self.offset_gnl + cons_dim))) - self.offset_gnl += cons_dim - elif sl: - self._index_map['lam_sl_in_lam_g'][i].append(self.offset_gnl) - self.offset_gnl += 1 - elif su: - self._index_map['lam_su_in_lam_g'][i].append(self.offset_gnl) - self.offset_gnl += 1 - - def _get_cost_node(self, i, N_horizon, xtraj_node, utraj_node, p_nlp, sl_node, su_node, ocp: AcadosOcp): - """ - Helper function to get the cost node for a given stage. - """ - dims = ocp.dims - model = ocp.model - cost = ocp.cost - p_index = self._index_map['p_in_p_nlp'][i] - yref_index = self._index_map['yref_in_p_nlp'][i] - yref = p_nlp[yref_index] - if i == 0: - if cost.cost_type_0 == "NONLINEAR_LS": - y = ocp.model.cost_y_expr_0 - residual_expr = y - yref - elif cost.cost_type_0 == "LINEAR_LS": - y = cost.Vx_0 @ model.x + cost.Vu_0 @ model.u - residual_expr = y - yref - else: - residual_expr = None - return (xtraj_node[0], - utraj_node[0], - p_nlp[p_index + yref_index], - sl_node[0], - su_node[0], - ocp.get_initial_cost_expression(yref), - residual_expr, - ca.vertcat(ocp.model.p, yref), - dims.ns_0, cost.W_0, cost.zl_0, cost.Zl_0, cost.zu_0, cost.Zu_0) - elif i < N_horizon: - if cost.cost_type_0 == "NONLINEAR_LS": - y = ocp.model.cost_y_expr - residual_expr = y - yref - elif cost.cost_type_0 == "LINEAR_LS": - y = cost.Vx @ model.x + cost.Vu @ model.u - residual_expr = y - yref - else: - residual_expr = None - return (xtraj_node[i], - utraj_node[i], - p_nlp[p_index + yref_index], - sl_node[i], - su_node[i], - ocp.get_path_cost_expression(yref), - residual_expr, - ca.vertcat(ocp.model.p, yref), - dims.ns, cost.W, cost.zl, cost.Zl, cost.zu, cost.Zu) - else: - if cost.cost_type_0 == "NONLINEAR_LS": - y = ocp.model.cost_y_expr_e - residual_expr = y - yref - elif cost.cost_type_0 == "LINEAR_LS": - y = cost.Vx_e @ model.x - residual_expr = y - yref - else: - residual_expr = None - return (xtraj_node[-1], - [], - p_nlp[p_index + yref_index], - sl_node[-1], - su_node[-1], - ocp.get_terminal_cost_expression(yref), - residual_expr, - ca.vertcat(ocp.model.p, yref), - dims.ns_e, cost.W_e, cost.zl_e, cost.Zl_e, cost.zu_e, cost.Zu_e) - - def _get_constraint_node(self, i, N_horizon, xtraj_node, utraj_node, ptraj_node, model, constraints, dims): - """ - Helper function to get the constraint node for a given stage. - """ - if i == 0 and N_horizon > 0: - lg, ug = constraints.lg, constraints.ug - lh, uh = constraints.lh_0, constraints.uh_0 - lphi, uphi = constraints.lphi_0, constraints.uphi_0 - ng, nh, nphi = dims.ng, dims.nh_0, dims.nphi_0 - nsg, nsh, nsphi, idxsh = dims.nsg, dims.nsh_0, dims.nsphi_0, constraints.idxsh_0 - - # linear function - linear_constr_expr = None - if dims.ng > 0: - C = constraints.C - D = constraints.D - linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) - # nonlinear function - h_fun = ca.Function('h_0_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr_0]) - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) - # compound nonlinear constraint - conl_constr_fun = None - if dims.nphi_0 > 0: - conl_expr = ca.substitute(model.con_phi_expr_0, model.con_r_in_phi_0, model.con_r_expr_0) - conl_constr_fun = ca.Function('conl_constr_0_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) - - elif i < N_horizon: - lg, ug = constraints.lg, constraints.ug - lh, uh = constraints.lh, constraints.uh - lphi, uphi = constraints.lphi, constraints.uphi - ng, nh, nphi = dims.ng, dims.nh, dims.nphi - nsg, nsh, nsphi, idxsh = dims.nsg, dims.nsh, dims.nsphi, constraints.idxsh - - linear_constr_expr = None - if dims.ng > 0: - C = constraints.C - D = constraints.D - linear_constr_expr = ca.mtimes(C, xtraj_node[i]) + ca.mtimes(D, utraj_node[i]) - h_fun = ca.Function('h_fun', [model.x, model.u, model.p, model.p_global], [model.con_h_expr]) - h_i_nlp_expr = h_fun(xtraj_node[i], utraj_node[i], ptraj_node[i][:dims.np], model.p_global) - conl_constr_fun = None - if dims.nphi > 0: - conl_expr = ca.substitute(model.con_phi_expr, model.con_r_in_phi, model.con_r_expr) - conl_constr_fun = ca.Function('conl_constr_fun', [model.x, model.u, model.p, model.p_global], [conl_expr]) - - else: - lg, ug = constraints.lg_e, constraints.ug_e - lh, uh = constraints.lh_e, constraints.uh_e - lphi, uphi = constraints.lphi_e, constraints.uphi_e - ng, nh, nphi = dims.ng_e, dims.nh_e, dims.nphi_e - nsg, nsh, nsphi, idxsh = dims.nsg_e, dims.nsh_e, dims.nsphi_e, constraints.idxsh_e - - linear_constr_expr = None - if dims.ng_e > 0: - C = constraints.C_e - linear_constr_expr = ca.mtimes(C, xtraj_node[i]) - h_fun = ca.Function('h_e_fun', [model.x, model.p, model.p_global], [model.con_h_expr_e]) - h_i_nlp_expr = h_fun(xtraj_node[i], ptraj_node[i][:dims.np], model.p_global) - conl_constr_fun = None - if dims.nphi_e > 0: - conl_expr = ca.substitute(model.con_phi_expr_e, model.con_r_in_phi_e, model.con_r_expr_e) - conl_constr_fun = ca.Function('conl_constr_e_fun', [model.x, model.p, model.p_global], [conl_expr]) - - self._index_map['lam_gnl_in_lam_g'].append([]) - self._index_map['lam_sl_in_lam_g'].append([]) - self._index_map['lam_su_in_lam_g'].append([]) - - return ( - lg, ug, - lh, uh, - lphi, uphi, - ng, nh, nphi, - nsg, nsh, nsphi, - idxsh, - linear_constr_expr, - h_i_nlp_expr, - conl_constr_fun - ) - - @property - def nlp(self): - """ - Dict containing all symbolics needed to create a `casadi.nlpsol` solver, namely entries 'x', 'p', 'g', 'f'. - """ - return self.__nlp - - @property - def w0(self): - """ - Default initial guess for primal variable vector w for given NLP. - """ - return self.__w0 - - @property - def p_nlp_values(self): - """ - Default parameter vector p_nlp in the form of [p_0,..., p_N, p_global] for given NLP. - """ - return self.__p - - @property - def bounds(self): - """ - Dict containing all bounds needed to call a `casadi.nlpsol` solver. - """ - return self.__bounds - - @property - def index_map(self): - """ - Dict containing indices corresponding to stage-wise values of the original OCP, specifically: - - 'x_in_w': indices of x variables within primal variable vector w - - 'u_in_w': indices of u variables within primal variable vector w - - 'pi_in_lam_g': indices of dynamic constraints within g in casadi formulation - - 'lam_gnl_in_lam_g' indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation - """ - return self.__index_map - - @property - def nlp_hess_l_custom(self): - """ - CasADi Function that computes the Hessian approximation of the Lagrangian in the format required by `casadi.nlpsol`, i.e. as upper triangular matrix. - The Hessian is set up to match the Hessian that would be used in acados and depends on the solver options. - """ - return self.__nlp_hess_l_custom - - @property - def hess_approx_expr(self): - """ - CasADi expression corresponding to the Hessian approximation of the Lagrangian. - Expression corresponding to what is output by the `nlp_hess_l_custom` function. - """ - return self.__hess_approx_expr +from .acados_casadi_ocp import AcadosCasadiOcp class AcadosCasadiOcpSolver: From ec59717be3c7d470ddecf6178838286b3829ac15 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 14 Oct 2025 15:30:40 +0200 Subject: [PATCH 161/164] Python: cleanup classmethod/staticmethod (#1652) --- .../acados_template/acados_template/acados_ocp.py | 8 ++++---- .../acados_template/acados_ocp_solver.py | 12 ++++++------ .../acados_template/acados_sim_solver.py | 12 ++++++------ 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index ef45ea8c2c..59459ce1b8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1401,8 +1401,8 @@ def __get_template_list(self, cmake_builder=None) -> list: return template_list - @classmethod - def _get_matlab_simulink_template_list(cls, name: str) -> list: + @staticmethod + def _get_matlab_simulink_template_list(name: str) -> list: template_list = [] template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') template_list.append((template_file, f'acados_solver_sfunction_{name}.c')) @@ -1424,8 +1424,8 @@ def _get_matlab_simulink_template_list(cls, name: str) -> list: return template_list # dont render sim sfunctions for MOCP - @classmethod - def _get_integrator_simulink_template_list(cls, name: str) -> list: + @staticmethod + def _get_integrator_simulink_template_list(name: str) -> list: template_list = [] template_file = os.path.join('matlab_templates', 'acados_sim_solver_sfun.in.c') template_list.append((template_file, f'acados_sim_solver_sfunction_{name}.c')) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 8d01165e98..9fb40efb19 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -91,8 +91,8 @@ def shared_lib(self,): """`shared_lib` - solver shared library""" return self.__shared_lib - @classmethod - def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], + @staticmethod + def generate(acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts: Optional[dict]=None, cmake_builder: Optional[CMakeBuilder] = None, verbose=True): @@ -145,8 +145,8 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], shutil.copyfile(acados_ocp.solver_options.custom_update_header_filename, target_location) - @classmethod - def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): + @staticmethod + def build(code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): """ Builds the code for an acados OCP solver, that has been generated in code_export_dir. @@ -176,8 +176,8 @@ def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = verbose_system_call([make_cmd, 'ocp_shared_lib'], verbose) - @classmethod - def create_cython_solver(cls, json_file): + @staticmethod + def create_cython_solver(json_file): """ Returns an `AcadosOcpSolverCython` object. diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index 4baa65a231..c049b9cf33 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -84,8 +84,8 @@ def T(self,): """`T` - Simulation time.""" return self.__T - @classmethod - def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None): + @staticmethod + def generate(acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None): """ Generates the code for an acados sim solver, given the description in acados_sim """ @@ -109,8 +109,8 @@ def generate(self, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_bui acados_sim.render_templates(cmake_builder) - @classmethod - def build(self, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): + @staticmethod + def build(code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): code_export_dir = os.path.abspath(code_export_dir) with set_directory(code_export_dir): @@ -124,8 +124,8 @@ def build(self, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder verbose_system_call(['make', 'sim_shared_lib'], verbose) - @classmethod - def create_cython_solver(self, json_file): + @staticmethod + def create_cython_solver(json_file): """ """ with open(json_file, 'r') as f: From a525ff47a392c7a1a1e2a0a134ab8bbe36886a1e Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 14 Oct 2025 15:56:10 +0200 Subject: [PATCH 162/164] Python: Refactor warning prints to use warnings package (#1668) --- interfaces/acados_matlab_octave/GenerateContext.m | 2 +- .../acados_template/acados_template/acados_model.py | 2 +- interfaces/acados_template/acados_template/acados_ocp.py | 3 ++- .../acados_template/acados_ocp_batch_solver.py | 7 +++---- .../acados_template/acados_ocp_options.py | 1 + .../acados_template/acados_template/acados_ocp_solver.py | 2 +- interfaces/acados_template/acados_template/acados_sim.py | 1 + .../acados_template/acados_sim_batch_solver.py | 9 ++++----- .../acados_template/acados_template/acados_sim_solver.py | 5 ++--- interfaces/acados_template/acados_template/utils.py | 7 ++++--- 10 files changed, 20 insertions(+), 19 deletions(-) diff --git a/interfaces/acados_matlab_octave/GenerateContext.m b/interfaces/acados_matlab_octave/GenerateContext.m index 6b0380107c..44afc15bce 100644 --- a/interfaces/acados_matlab_octave/GenerateContext.m +++ b/interfaces/acados_matlab_octave/GenerateContext.m @@ -288,6 +288,6 @@ function check_casadi_version_supports_p_global() cse(dummy); % Check if cse exists blazing_spline('blazing_spline', {[1, 2, 3], [1, 2, 3]}); catch - error('CasADi version does not support extract_parametric or cse functions, thus it is not compatible with p_global in acados. Please install nightly-main release or later, see: https://github.com/casadi/casadi/releases/tag/nightly-main'); + error('CasADi version does not support extract_parametric or cse functions, thus it is not compatible with p_global in acados. Please use CasADi >= 3.7.2'); end end \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index afaab66810..4d26d7add8 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -221,7 +221,7 @@ def p_global(self): This feature can be used to precompute expensive terms which only depend on these parameters, e.g. spline coefficients, when p_global are underlying data points. Only supported for OCP solvers. Updating these parameters can be done using :py:attr:`acados_template.acados_ocp_solver.AcadosOcpSolver.set_p_global_and_precompute_dependencies(values)`. - NOTE: this is only supported with CasADi beta release https://github.com/casadi/casadi/releases/tag/nightly-se + NOTE: this is only supported with CasADi >= 3.7.2 Default: :code:`[]` """ return self.__p_global diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 59459ce1b8..0f03e578ea 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -38,6 +38,7 @@ import casadi as ca import os, shutil import json +import warnings from .acados_model import AcadosModel from .acados_ocp_cost import AcadosOcpCost @@ -1150,7 +1151,7 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None raise NotImplementedError(f"with_solution_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") if opts.qp_solver_cond_N != opts.N_horizon or opts.qp_solver.startswith("FULL_CONDENSING"): if opts.qp_solver_cond_ric_alg != 0: - print("Warning: Parametric sensitivities with condensing should be used with qp_solver_cond_ric_alg=0, as otherwise the full space Hessian needs to be factorized and the algorithm cannot handle indefinite ones.") + warnings.warn("Parametric sensitivities with condensing should be used with qp_solver_cond_ric_alg=0, as otherwise the full space Hessian needs to be factorized and the algorithm cannot handle indefinite ones.") if opts.with_value_sens_wrt_params: if dims.np_global == 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 066c542dda..b69f7c322f 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -36,6 +36,7 @@ from ctypes import (POINTER, c_int, c_void_p, cast, c_double, c_char_p) import numpy as np import time +import warnings class AcadosOcpBatchSolver(): """ @@ -61,13 +62,11 @@ def __init__(self, ocp: AcadosOcp, N_batch_max: int, raise ValueError("AcadosOcpBatchSolver: argument N_batch_max should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = ocp.solver_options.num_threads_in_batch_solve - print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead.") - print("In the future, it should be passed explicitly in the AcadosOcpBatchSolver constructor.") + warnings.warn(f"num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead. In the future, it should be passed explicitly in the AcadosOcpBatchSolver constructor.") if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: raise ValueError("AcadosOcpBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") if not ocp.solver_options.with_batch_functionality: - print("Warning: Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False.") - print("Attempting to compile with openmp nonetheless.") + warnings.warn("Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False. Attempting to compile with openmp nonetheless.") ocp.solver_options.with_batch_functionality = True self.__num_threads_in_batch_solve = num_threads_in_batch_solve diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index cb9d22cfb4..d11f6c1b3d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -30,6 +30,7 @@ # import os +import warnings from deprecated.sphinx import deprecated from .utils import check_if_nparray_and_flatten diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 9fb40efb19..6f3ee8e6dc 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -460,7 +460,7 @@ def solve_for_x0(self, x0_bar, fail_on_nonzero_status=True, print_stats_on_failu if fail_on_nonzero_status: raise RuntimeError(f'acados acados_ocp_solver returned status {status}') elif print_stats_on_failure: - print(f'Warning: acados acados_ocp_solver returned status {status}') + warnings.warn(f'acados acados_ocp_solver returned status {status}') u0 = self.get(0, "u") return u0 diff --git a/interfaces/acados_template/acados_template/acados_sim.py b/interfaces/acados_template/acados_template/acados_sim.py index a49681ed62..f9425c16a2 100644 --- a/interfaces/acados_template/acados_template/acados_sim.py +++ b/interfaces/acados_template/acados_template/acados_sim.py @@ -31,6 +31,7 @@ import os, json import numpy as np +import warnings from typing import Optional from copy import deepcopy from deprecated.sphinx import deprecated diff --git a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py index 0893f8d731..0215c9439a 100644 --- a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py @@ -33,6 +33,7 @@ from .acados_sim import AcadosSim from typing import List, Union from ctypes import (POINTER, c_int, c_void_p) +import warnings class AcadosSimBatchSolver(): @@ -55,13 +56,11 @@ def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Uni raise ValueError("AcadosSimBatchSolver: argument N_batch should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = sim.solver_options.num_threads_in_batch_solve - print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in sim.solver_options instead.") - print("In the future, it should be passed explicitly in the AcadosSimBatchSolver constructor.") + warnings.warn(f"num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in sim.solver_options instead. In the future, it should be passed explicitly in the AcadosSimBatchSolver constructor.") if not isinstance(num_threads_in_batch_solve, int) or num_threads_in_batch_solve <= 0: raise ValueError("AcadosSimBatchSolver: argument num_threads_in_batch_solve should be a positive integer.") if not sim.solver_options.with_batch_functionality: - print("Warning: Using AcadosSimBatchSolver, but sim.solver_options.with_batch_functionality is False.") - print("Attempting to compile with openmp nonetheless.") + warnings.warn("Using AcadosSimBatchSolver, but sim.solver_options.with_batch_functionality is False. Attempting to compile with openmp nonetheless.") sim.solver_options.with_batch_functionality = True self.__num_threads_in_batch_solve = num_threads_in_batch_solve @@ -85,7 +84,7 @@ def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Uni getattr(self.__shared_lib, f"{self.__model_name}_acados_sim_batch_solve").restype = c_void_p if not self.sim_solvers[0].acados_lib_uses_omp: - print("Warning: Please compile the acados shared library with openmp and the number of threads set to 1, i.e. with the flags -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1.") + warnings.warn("Please compile the acados shared library with openmp and the number of threads set to 1, i.e. with the flags -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1.") def solve(self): diff --git a/interfaces/acados_template/acados_template/acados_sim_solver.py b/interfaces/acados_template/acados_template/acados_sim_solver.py index c049b9cf33..33a5ec34b2 100644 --- a/interfaces/acados_template/acados_template/acados_sim_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_solver.py @@ -33,6 +33,7 @@ import json import os import sys +import warnings from ctypes import (POINTER, byref, c_bool, c_char_p, c_double, c_int, c_void_p, cast) if os.name == 'nt': @@ -153,9 +154,7 @@ def __init__(self, acados_sim: AcadosSim, json_file='acados_sim.json', generate= self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder) if isinstance(acados_sim, AcadosOcp): - print("Warning: An AcadosSimSolver is created from an AcadosOcp description.", - "This only works if you created an AcadosOcpSolver before with the same description." - "Otherwise it leads to undefined behavior. Using an AcadosSim description is recommended.") + warnings.warn("An AcadosSimSolver is created from an AcadosOcp description. This only works if you created an AcadosOcpSolver before with the same description. Otherwise it leads to undefined behavior. Using an AcadosSim description is recommended.") if acados_sim.dims.np_global > 0: raise ValueError("AcadosSimSolver: AcadosOcp with np_global > 0 is not supported.") diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index a687abf334..6acf99763f 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -37,6 +37,7 @@ import platform import urllib.request from deprecated.sphinx import deprecated +import warnings from subprocess import DEVNULL, STDOUT, call if os.name == 'nt': from ctypes import wintypes @@ -139,16 +140,16 @@ def check_casadi_version(): 'Please use a version >= 3.4.0.') if major > 3 or (major == 3 and minor > 7): # >= 3.7 - print(f"Warning: CasADi version {casadi_version} is not tested with acados yet.") + warnings.warn(f"CasADi version {casadi_version} is not tested with acados yet.") elif major == 3 and minor < 7: - print(f"Warning: Full featured acados requires CasADi version >= 3.7, got {casadi_version}.") + warnings.warn(f"Full featured acados requires CasADi version >= 3.7, got {casadi_version}.") def check_casadi_version_supports_p_global(): try: from casadi import extract_parametric, cse except ImportError: - raise ImportError("CasADi version does not support extract_parametric or cse functions.\nNeeds nightly-se2 release or later, see: https://github.com/casadi/casadi/releases/tag/nightly-se2") + raise ImportError("CasADi version does not support extract_parametric or cse functions.\nPlease use CasADi >= 3.7.2") def get_simulink_default_opts() -> dict: From 5847c74fe774137a5fb31c9791bf39aa980b8b05 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Thu, 16 Oct 2025 12:49:05 +0200 Subject: [PATCH 163/164] MATLAB: Add convex-over-nonlinear cost (#1670) closes https://github.com/acados/acados/issues/1243 --- .../problem_formulation_ocp_mex.pdf | Bin 154656 -> 150220 bytes .../problem_formulation_ocp_mex.tex | 4 +- .../test/run_matlab_tests.m | 17 +- .../test/test_conl_cost.m | 160 ++++++++++++++++++ interfaces/acados_matlab_octave/AcadosOcp.m | 96 ++++++++++- .../generate_c_code_conl_cost.m | 153 +++++++++++++++++ 6 files changed, 418 insertions(+), 12 deletions(-) create mode 100644 examples/acados_matlab_octave/test/test_conl_cost.m create mode 100644 interfaces/acados_matlab_octave/generate_c_code_conl_cost.m diff --git a/docs/problem_formulation/problem_formulation_ocp_mex.pdf b/docs/problem_formulation/problem_formulation_ocp_mex.pdf index 98b528f288fd5e0b040c5737d835af3ed4c7a831..5eac93b9bf4bddd0898a8b6c5cb6351b8ce48850 100644 GIT binary patch delta 49305 zcmaI7Q;=>^yJnfTZQHhO+qUg5ZQHiB)3&+OwszX?zpA=V#Obc;88c?An|Zs|80&qW z!Vc(}G-yV0B{2zlW(IZ`@`b;n>oCkL%tTB?jwUuRe0(sBa^?<}ZdOFB989c4|8v1) zIPU|a0bm3KU|ii?%#H0~yf&}(WnD?zk^8PR@2ECpv39I_RMON&uR5j#MslJRoc$78 zx6x~;RJ4;f{s8=OSVdi!M);%R+6g6e zU~sg-K~~ORzR%BWJFr7iA3k;}M!)SI+*lous@E zBDg~f33zeHVHy;-ClSSQD%)eYY(?;ePCBm3wUax;JD;O0i|9TFxm#?$uB;Rd4VaJ6 z{R@{L2Mz=V`_B;~`O-fx#>|HDXIG;>+y?Rm(Si*5@6S7SYlq9lUXQtoj#>Sm?SCIC z0PVMLm}tLm#Dwnp+m_yaloN#qblF* zx)B8g`KYqOWVu7LDqqx#Wz8ULuaPQPw~rose(tQRJzL4036*T zk;qUCjvhHkKDD8g`xY`(beog*o@L5q3%g;4X3@%)Tkf0eM+Q`7_$T#Ji8~IP8Df+= zNXE95Htp**rDO(Wp0(R!HvRpk%9IAzqV04vJbS`wD-<>j$v^2>p`p$%Bp7LLX?3wG z7si}voa`{I=#Si;v_=;w9^_Im0CZ{6H)Oa0RN}L6zlPl|a@t zL&{KC4-p7n%MCcRrx;!R@9URKK}}&QZje(^qEu#5Q99J$&*9~j`TmJT-=Dj2WYPSL zd)L*cC`@(Xz!$rx`5J(pf6iiWrVhG3H7VYupOhRH6D{67U+!I@OdZH9z_i4o@=a5w zmXZ+|Fe3q#T9dul9I^D}+*YaOSe=-$94iy2pqvUuRje@9gFCQv@=uO)`i694O`3_; zdY;O6W*!e!93NFyiksr}91gn-%$_e^#=chJ&1gDDn6xo?;k522`7d5#Z3}P0s8D0~-s{loR!-dV& zpgO^38q+Z)WeJB@iJcQD=!S?UH$Kvq5QN7K_QMrq8+{tX7oLrJMLC$&>9O!7cZOu7Z3yNl%bvUH- z+8KAeaE0T_6<(NuTw7B(LBUq~wgujgAzxvcWU)c80P$KHa0{E{!6sc}@y`p5LkbAa zRMc2=-M}~ketbD~71QG=1;`37W+ZiqH;&`6SEyh=JJD#9Nd9oEOwqBt92AO_7E5Ly z%_8xzz1TPDE| zSs7tE-uG8qjgg6vB0V5TouA`>obvd-E2B&6{OM!aT0CukMlfbyjwaF*CVR-cCX!ZA z>;GbbA8f`m`#xsQ6iHk7mkDPM7BSF(K}<-(wrBFMBhGU+7iS2qy^bZO2Hgxs)?E$<570oT zyE$NrKO*+gZFuIXxEyhMj*5HEJUIjzdHOzwMU=dy|7;K94=-Ky=4k9vvaHRk!tU)Kax*M*UMd@0eFxOE7K z;JlL5q_D?{m`G$kI}*}Ty;@@dLA`_5j{RTEmfx!ljHhAQ=uOeh{(v6z34l_ti~!e# zm7 zRQ8$%i#s`mgx64$;RThr zKJasY8{!ULu}i4MM_u2)2Y?xDLw*T+eu+4!1c|iw?Jk1iqRt`=o-;q(g2#AY6M;R6 zvmRUV$|&S6g_jVYRH`F^a9Vg+B~{>ao;q%V9cF^3fINsw=8s~a%LN0Gbx#O44C4aZ zheG2QWIjQTf#vXE(6xWU<{Fku$HK#)*;8Su^%n>W?VLrmJQv?uJ^(IJiHfM)O?{XP zUI#0xD4$C2x!2y{d%d>tKATR0OGFQVi&Y;?fO$LL)9*Abbq+4^6f8Z<$c)~-~ z4D3a>5rq}OL8?u!39!^?dauGP+r;OP${iZMWLVB|V1j;}iLoC^_VMxFxklE7-WoJa zcyTGXw9^0S_AHz6QU+^f=L-z)c6_`}oe~Ghzn)WSZ`USO@Yr+=D!YkpHTa@$+@U{E zpyrk0IpQD>tVZj&)W(>nhp344LCoUW7a?_E^c%Z$d;^#B1z^>RT)x_Q;>@~m59Suu zJ_)biz36|aOBEenC3{s~mxrX384xex9Qk{2FHKJD^?;f$DLi^jUK53QqoZCRS*4un zW`Wp3>t?_osLmTGyU`mDWI#!X^Mpiwzd&aTHNTY(Du~w4ZY_NMFY!&C{!N`&0*ASO zzwLD4D1#=T1(5F|Xz;mjQA*(+^{%h=A!BN46I`F12C~&oyr`D~jMz2@mt!5Tx?kB~ zyCk1K<9#?R{N=6hffwHdL8y8GZNy%yCP2xIBZDYH9w<%>jmRGo)hIqADHXS1Rn)_P z|M{<|^~p#SrEFwK>UGMvy1TTo4ekAJV{CX=O8wfg9Uvk0xq0b=@!!Evo_73lgDXZ|^6nxS}x@?3%@&)|7a%cRc+xm3*)eKx}%f(Bj-N*Iu^0Hf> zcjNEj)Y4!IQ*B&@A#@;%-Z-+M89uFhA=^eAP~RVq??hN&3TYN|xQJ<^1{b*h zb`fYieSO9FDAvassmpTIKc@$29xKH~|GunVVZ*9* zuRlP;0j#Rw=t`Ma6&r4sCm)KVYB&7S{z&9CgJZ!=;JWo5`RV!Xmu)P#_myQ3K z5P)j?u98F}DsL(p9W6dLf74gtB8>{mkU->P>~B5pvSd5NX=o$<(jKnTJMdr zH%P5REv3t0!$Zr)*LP>WrP|SL;aVe=!E1S$TfEWjI`)9o*);Sa6%}Pcu41z*ViSeI z$gs)Pt)i+=R{RkwR!JVrVE#beCnI`I4gg9aC;nINEph-&TBAqc08` zM^Q0CjkhnIKpNAXQ;~qEmiUKrThhQLgWBF6eK4JyMHjT4Ks`kC{HDH{q5rCr^nl7p5Z z?x9WHJHG<=Gg)nt!hWgx#hMjK%#C)#M_y$Fu!x#X_IkleLK2z3OW}V7OfN&SA)M1Q zGyO;gXe9BL(mK=F8QDQ03*7>;Ag4Pn&&abeG$qo8LV4->XvIik;&BK8uGD}_g~vesu*Ae#a26~Xy8Ex@7)ZaNgs$QAK#_!UJizc!#QPKI${^jTEHOn%h{KiGN) zAf!fx(_fB$kSD%MARj_pS?NfxM%f2`wg*j^dlJ6=F;C*SfWnNdi zH>f(pv3iR6O1*X_B8x?yaqv+U%LMnv>4q8K}9yuhyc{|o~ez?4u<>*BhSc*EV7xK&}9GE&kb+F{5TPWKn)zuZ+qWNa}GY{*jJnk zdskMRTE65Bze_7TMesa^!E~}@cg-t5vlvXvXU&<;Vho&qj zi|=g`xCs>#d9KZW1D>V8Xyf8n2?Lc6N)Gf$hb;IsualtAPf6B@jfaW=OyQ42)ZE%o zqo=d#KDL|5Qx65w4lVEcI%ynx6`VD0Td^(xld9#~j+yIrb7R7#;wlH-;86~8THU`I zC*!DN7r9s^<};1#kn!NF+Ai8WkYsc^sFd<{G*6bcyy=zLHhqp=|(-_k! z)Mz($R|+QX(I+gRN1>W7$bX=Nbcp-hM-#o$&X2dBJsKr*>JVv??as&RpObR4%xW2< zD%2hGg-RB*2a}n~+6j$pwg-&A8nogn(^Kt*xdz&4D5$|S>}T19M|08om<8#RfxbrSRjOAk zYU=!b;P~lm#)(b4^rDd$*_q%?wY_%tEd&+?Sr6$#x@MLFDpGYfu+jJ$HJ7YM{w86a zonPOS@Kw1yEo$}@eMf1Mhg0#SZo8W)na7>FN@lq$FHI&3SH_OC)mmGO;9v8A9S)1I z@n8%OZc%W+o~aa#i*5hh!fR#YDLkTu`)3On$P^r8+T7FKtwx_I3AHqt-e~ZR=+RI3 z5gkvn@~-^naF$2g$*ZMd4#ZLS-CaqJh;ZTfdbAdnOEj#^FOPPpN#Eb!AN-$_N6(MF z_P!IL5_aBaaXHDT(7g8SW4^paBy0TevCt`y92M2m0eQ=K<3-@ zdhV#c#ofk|w;%sKM{!bwMc(J*+U7>$nE21a)42 z4}G$H+e20CrRr=UKf8g=c!tv9_G(->{ap{hL2%9S0^nKqIEc%mZ`2ECz`A7}=(OJ} z)6QM#Wxp%OGg2`!X9T3wB?QHu*~$nH0+RRRhRKsG8{y!`)lNE~z~E;zFzSjdL2(!c zHORm~lxCv^I!l02WKJJP6N!5^*7sBM8E(%seY zt(!)CRpsoQbvY8}%CE!9V9@*fy|n3l@c>q66d{*+-MwW8w7Ol!;L|#1Q(`x-jg^X1 z&F)P#UGhQ#V!SH9+W~*TEO^L97c2+Be9DDT`QA1oB#3!8*C|btAXtDKGb6}e`m!nM zvzD7Fa1hlg6F%zBXqdq?pgk_^m6C&rG5!Y`ObcQkt}K!l zkH*o^G?_A8uP-{4PIrO;XdxF7#}}px$Ol}8wL{NewB7HEg2 zlzG@MSkG`)>K86 z$~?_Vq90Q8uE(j7%50LqB!$UNIEY7ixAmG}<-13Dh;+(TmRu3LKAKqIqN;DC^~p5M z`>@XS1{H$7r?61)`uF#5H6cOyHT_=+n%c5JZP;LoUehQZ#Q3`g`HfxyFPA_htzl)| zZs$r~F=2AypHrB~&-8dz_f4^Y_tK7uF=uC?vZE$%yZEM!q!84qp-%?}NUsB^n&4}~DDNdZAvMoF31BzoyY5Oz5`Q>AxKSz;d98K} zw4T;}s$C}9A|$z)rQOw%Od3d+mRyW-F$tA z9Oj;J<~n=aceQJeF+5VH?2N_>renazO;`h1pnh33z{#5@vC5%(r+NeXVJ`5M#kNY2 zU42Y`Q!ni*eIG~*>(Lls!`leO^tp@j^YTL2V2DR=%43VI))cM8-iwgl--$3*Eh(iR z3B}+>K6%17&rsN=+aoW?(IpmUC>LdtuW6p2QPUO>22sQHeGQE7EJJ+J!4g)u^;c*QXKvJjJj7SEB%scCB3zr;M1K+($%Q zh=Cv}HHDmXYlj~mI$)m0X8v5%xl?ZN8-};3&n0=vXx6q~@hunccqQ!IW|-GlL8B0S zFgii9G6F0Wc!K)F-~Qk(mw&3R90OO-8>I!=$t-n$Ou zzc39U|DaW0r>mPiHZw&gAS5tl8ZR~N1dR3M(r5xhcVF=! z9;=zR&g&s6z*eRtY`EIOBw2wfEe|SgTF`cjh<3vWhvEV7SD#bX35K32p=3L}qG~hU z_*Q$l>G1UXn*LTf+suA_Okl{!^lHcT zIV1b4N{0oU79e!3%11GEkq*0e4KLK6dYu<72oxKR_jej1x3gJGR`v6|6Pqm`4dT%`K0C0MNm3)&)fsT|?9em3CV=6&=RsGje9 z`hU447svm`E!jB#n_D9Mw*lr##*GUw79iOTZ(cSdMO3zR7CTB!iYDsu>dfQ-1Y76d z`tn>1vda^1HOwmG&l)kj!z+3yznm7D#N0!D_8=z8V zj{)b`j@Ao?Pk(JC!X)W4AqzkUQGDmoNqsi__QGT9Z<|&(tS6r%Y&o7nLYiNB9Ek$l z!<{eitWTC4j!Vf~tfgpyhAi#hKthZsE^{|oxy(APz)Xmz2Lc&xdTCXmmE6B zDZjj#xoTPrFbYzg3O=Vn+$iRud5GkCAPa_P3CE9Se-Kb+pvUahG>Urekca)7`9KEo z?b9Nem>Ro3b&-4NFkv>vgeNXhwazwQ*T6hj@m}%VoQYWn&=upROPsNZ9x4iC^3g>z|_ZfR3exa4f}jP5C1)VJz1LsI3!Z| zEx$@cFsT?766(*R192LuTUjwu zIu?9API@8$HpQ6>h{^{8fwQr(3pd`Ql$U$L+3J}wmd_}`LT#=NO_p_=2(CglgtjA^ z8&pY%cVM4v_3IwHdVw{`#X!wwHWRgT`z*7u1^9W52Zlc7m4|h1)PRg5L4CGVfH-~n zK}>*55=-4k>?KjakJjhFS$ND(3luCQVA{53p*o;6*LpkII8QE%gPmMiRCOvZD;M& zX0W;rDr3aRX>4WZ@Kzg2P4hftB%(6dt+iESKrbqG!bM?FSbtj3fVdov&+sK^rK20> z6UH*km<_M={U*^2ug3RdjDss>$=lyIK~lpfsk{y8YhvuO9_V47m$_Ue0V+MPM;1Q) zF~UG8vD?a%O6fWDt3oCk23q{`O zP$a2YOc6{3pnxG`8R2*36Wd7jQJcJI>qMI@B%S=d0RCtwpvCxMS@iHq zjaI#3s_`vKy65CnvP5uo_!u~vxvbRxAvz=)S&jZ&7jGqSF(Inei}myuFS?@Ow;`K2 zmuNI|REULqR~DMJ<`{D-z$B+qJuQpf=%)_%FDR-3h5$FXKUC$;z^46OBQVB3Zpfr1 z-X#nQUG3H>K$+bh;G?=n;!h#TOOvkWet+w?i$VAxkkeVQlTPfQIo3N@v(m+otppsD zrewcd4T3c$@-12=b-Q7`Q4wSAvZe*st+e-spd*QYu9Q$f-d2T1vJeOaVjjDfb1~^u z%64qr|7pf9QMi6^uRxH%Il1FOIb6$lWfe@>4J=C#N{Mq1XqA2#iQ`GHlH9*=O{Q#I z{CpTW+KcEqYdVQu1LUPnn-?YN{Yo*JJ&pkJ&nH(vNF+2?Jr*UbjehZXX}7qC-?>6S zwf0Q$I@)&OSN9mW=X{(DISK|gM#`dZC+rS>Yf#(SibirD0}i>qr%V1tU%v`IG`2A$ zG()%Tt!@|sCYJXm6#$bC4?O0jRO=LBP73O|I7Dk5_=Y4ytBhZ}pCD&2=fJ5CZE)fhQtAs|e?T-xEQP89ga&;9CwqzuvIr8T0(8k*Z&>gVoqyVp+_Jh=Qw|ek6i>%q- zqns;1&rJF*6*du1#N@u~Pe4l87Al2=6-5UPAEr7s#lLWPv}W}i%dgCxH^mHx90;5Z zmkZp%;39Os%Pf2o#Ec<2QY71o;w@D0>XnF|AEjeLMX1r2iAYu!fBkn;=!(Ro&EW2M z&{-PWE0g?+_?JSp)YjL}y^nJ)Fr5no56K_^1_+0&8Y}BWJW>r2LkqtGWRtd3W*l1Ce{)LR!;%Id zEbe!3cL~dVA9U;UDfZMKbpnLWWJjRJcq89_xkzjvhdNVt699?BK5P{#drmH9)x0w(qO z&cigYPL3JFR`UfLJ#+5+cP5{I3;IE(hHKt`ggpdLSZ*h2T|GHd?UFfC-_C=~$ZD8S zEXgHiyMhc1e2q|bddpkWnfb)THFI;W}VG6`T4;%^&D=Mg2l?&a@M;#OPc*QN?Is+sCm;+?xPXMpbWaLal6djr>r<8$}bn;jOVN0e=rFnwYnI%_cLhRn$Whl0L zXF`92)yFq|KTRtKKM8ofhCcV4JEz!&vS!nkrIjLyJ4!* zFDN| zLjxINsLlQb^<5vZ=ZhhHWrQC911K6Z zd3bm4o5JVqqV?;zBhUZdA+)%!*V`vSOZR(1;G3_SXkKqo5K9RGMGBLj$0Gx0j~}Fx>ju2d7tD$gxmG7|AunxP+x<0 z=s+ZjO^cf^l#)@qyMKH4oWfJ7;>bti7wDD1oX!ujSkHd)Sf9p8)0MwWv03A#hFM6l z4^l0UYDx+KMH^s@{qt1Jb7loPOEs~y!j;Wl`4*0V_#dCAipLVipJMsb$fab|Z@aK=9v#1iOskcift z{wTDX3Mmuw*Prq7KI)wJo%=}KvPZ`l?*l^Tp&j`sN(!r=4=tgSbmj6DMBtY%MdwR5Hzi&Uc^riRHfD;?i;dsonGXl0p#0uq=mwU#80(t0g9_uW z;7j4GIodF3CUCTvIc*_*BDUp(SXXQzuuh_CsCZqub7s ze=&%X)f0!JO)C1|5k#vh&FtgiGzlj_`XlBzX;i?=xak*FL(D}FjpYye{MD5ZOAf{n zAgpYwxa!LwSbpx5@yZ+9V1_ZqyD;BytgHs4?iFi>BkydKql;BRj-9lV)e#@+$R^)78P%UZvT%W6d6zVL;L(&9}{@FJCSZlWhjf0jLMY8hP|75d}UPM z{hL@pB*T0qbXH zDw>DV3tm8(S)xGf-u|fIED@x7S{1vjPUGR?Y|9f&1yi_dZ3c#v())of1BuDgkR!bj zD#_&dYdGVFj|Q`-GHc=lEr`@@>=Y*oq=f+rL@tSy2RP%_#qnrw7#{`H%1y=jsoNDy zXokOrb>)`uNzSc^dum`OT$*=j=_Q=0ls>*34ym@k?=Y~BKHF`p-STGJ5K!qnZYtU8!mUS3I0C=oxz zbag(2i`4O!8& z#+?>m_PuD3r7SX@E4000|Ct$VW3056pn3Zr(c?{x;fTN(E~--?=ZZLpOA&J?*+&X` zrI9s(7Lh7Z#2#H~N>{R*oT;OP>||}aXOL~5nkOd(JL*wDmu~~=r%SCs4!{nQ?Yre< zvW>j5$ncqg%#5*1|LWrKN6B<)*aTEkp0J^1KRn8Z5I%ZdJEE24eySMEDD>5#P#mD` zkNDKQ@0{3SG|P~>EX5H@9!134Azeyq2hpzHP|}jIZ;_ct!c%&wsEoP=WZ1bPjS9&> zm4j+GEP-C2@Ol6;|7_3KxO-^{9i-cNAkSNQb~`1m-P+-Mnbra9Ii z^GD_?V4$T8uoY#>HPP_EiR06_5;q^nSvTj&kH5P5sKx;_JmPVP;+@26H(7F;#qua| zmL_6DUWD_Res?%P8Ov_eJ4`UQ$kk~Hpe&vn?ZnTK7I8E-BL-YUbQan(4AUTSzKEg> zQmuXz1tw3VsHcuJbSo#*iY{dPqbDAt5L-U|Vo7~uV&aL zKoz^h8Ylul3Buvpa4b+mRr0|I-opk=3Fsa|Y%v&d_F(<)V&pZgQ=_Qls7Y-OvYh6Uo??AJhYH^=KORg)=GGJn* z+QogvkGo=s+jGOe4mU3XsE@+{Fc<@VBw4eT(vg6}Y(k_GwnZI0kP?Y#o#KTyM3yJn z!RT8MS}~j!#PLa@z@%^o@Wvsni^s@!9uuYTE{S^3;+8{Dh`)&U?al>?&dH<8=FC@) z;X@$>G^pq?*t-Eqg9B2r>>-CXBQ&)pyI1T!vgqE&DyT4@Two;^dIKC+RGdJXZj(%})I#Q7b!g50wL(&*UGRgNY>N%fVF~4ptmXRz^<%fA3l9A0B-Y*T|EkyIcO1 zOHQT9BTO!i$fD;h6Lf)~dgQ2}IIdgOCqw`=gB7|Jp(M32nO+0r5M6RKbx||eq7qj1 zEQ0YmgZ5vc^v_bd*eE(-wm_6?UjAJkd5JDVRAGDkgL9xjUYS4FIfjxu9ymmM>E-1! z3?Cl$3xOq2u4=a(?fi#(uQ`@bziV7lS(14{>50X5b@}(Y)x@INTtS7wJR||tT~~m9 zrrJBuM%%Sr5zo9qT{dv&c%s%`*4t#mUNoN{bxZaW@gYFn>`Gp0ueJ46AYt-wTr785f>}_ zf0bBRm^hjK&-tQR9eY;AKiY3=(_%g<~*7}hwq<`<3mKB?{+TI!Nu7l|g440Vl>H`Xbg5W-Y=;UrB4FLCA0 z(9TjnnW|@0VAoa=xVfY-tXx#+rgiF$pmV&B!QzF&`C^B{(6JyS3Rn_2;i^F~BD63R zXy*aY5X3Jctq^J_L^uC{Vl#wH2=*dp@v z{Bgzk<8C@CzU;K@{_%)0Fg$c_;h~Z5u;Gte!r$eJSl(@E4rn1n=gvn1?~@3mRU5zN z@zWs7O(T~OPok~1cD;dk)HHwVGs0*0TtDGwJ0p~Hl{AOMdoGzMDgTuFUGsrQEME&L zHvrh*+@+BeaGv4F{E$uCZ5|X_M*v`E5XBoFPe+R7W^PP|5vm)W`<8PoMsvf*d40nDCD0b&H0!OFgFUu*-ujhpmU8?&2$9slb%eij;6$2EX}HP4WE%4~V` z2eWovNP3)?-kYNCms{AOw{(p|MZq;~KFK}jWf=~ELb_%1>Ct$|!Q{B7DZ3x@hbtcz z!Hnhfy`6WIN=T>+lOK0qt|9&0c!v73^7qs7+qKyf;QA|sCyg+ZjKAFyv{7qcUk%Dn z{?&{lCuF56)wkKEKA<7G_iA^|zw2gl^_zWXqY$|9kOParuvc5-$TDSPFXW*8adhjx z7(~Q!m@r%r0Z{?GlNl5#PE#;?Vl<59y@@EPIZ_<)1qi zz^Q{x;Xn7wN|xxreSc0Jj!5?%G126pz8pI#A~tt7)=&=anXW{Mk`s+)VV}sJN}g+k z6M~fwo|wZ(;Y!Fko*f-K|z9}J8^-RCnb2r zF~aor$HJ{BobfoZxM6%$_D+R*l}4osfY|{~z^MNz{6IgV1 z2%cgj36~G($IOa9KT5{S-+jMkyemPBql8s0fHS+k!Ut?vY9a&}nLQM^MjxyhH_S=5 z*k9t-BkcZu9Q{4MwF`g2C>e>spk=O7CxrKwag0a`HA+(4c3kK~&R0lsk{PpPKm)rJ zqRXE_Do`x7qh$l4rg=AvaUhQam?-gR{E#a+@@Ff8Tzrth11e|~tzxbRhdMe*$MU1k zve((25Y=sn;{*ol0_SzC$u+uQh~{tlXN5oB2sN{!oq}Wx5T&ARLeA1lmk3G$ec5yZ zb7QOn&?UOorGB56>5?2&HE_r)fcSJoReY+X>*MsyKhhU?Hjp_Usyc5L=4fWT@hXsQ zU^~|(w9Kfw%;j~+w)|4;hbrhfuC{tMVsu*U+3FLz34pjnN<*WPUQIgL9^~odep@STS% zz5pr8yxK{3@QB$vL;Qn?g4xDzyayhj-B$cPecL&uLG^|TG{^0E&KT?gZQH?wmNsM^ z7w;yEm&i#SCQE=nY&)ObVavLkgBh@zpVB@&oywIhQ z>~{||49)KHB%kEpT_=Gl<7+S{4rH1aj2)>u6S!J5gtX9Z(7@iy$47B%$XbIc1=^1E zPlME|nC;0{f3<(N&i{r(nEyi}{{trRGCa=!axo^Xb**#{VwwLtY#i;Jv0#AQKupcp7d>P9a72^%qM zNU~O=%8NHO;f##ZLGfG^&#c|WTsB880Y47ClD?v%kDU%r2M|+wG0kJN*tO0yv`l^^ zGTn?z&kM6x##Za!eny@mBOzg|#0n-U#@(*|aUrnL@4H*H^S-F%$9AbpG=WGbnSfFb z@1^^7>zt`5K!{MoCj4+f_N$uMN9%JdStBYlAy6ftNo2B7y|b?%{u;CaFwHZV3=Jcp zjWF~BAG}_S05F-JQi@B-GG|}x@JktKUWqG{zQ0?o_L}RnykuX*OS<66ZplTOVPW`2LfW+Wx#*);V zl;o!1iru)(1S{VB*E^&iiiD6&q#KH~k603NOPYfW4G5YJ6jLWhGqJ?)5#WFGGm=N7 zL~4YaIW!tbCaSY$>Ee02L#8zKdY26zRDV1klkaWri`AmfC8LDwC;VyzX1FQ?6VHU4 zsujsu(IcOsDX%;y8oEGH2uD$-=pVULt?h+g0aJ{0GHbsV-G`~g>IjQ$&P^4k!}^zu zn*}$R3UGrW4#793HHFVZMpdw3^^&4uS!XPlDS4akzx~10S5&>wH+ApIp9V1+p;;r>&Uo+mG?p$KW#T8Up#rUr=xcH z71OmJp?7vH27xcqGZ2*88jppeMa!a~-MNZWVB&whWhh;x8&su2FTF=+SnA~*0nsrw zE@@`$8w={p2J(ELq9Y1aKvaR?AV|Cu^)c({BjYelti(b;xe=PqK%NL1ftFeFNH1gmutVfKP~})fHE!)Q8G$ibZFrvSTqRzoc~{Zy<>EyfAsYib7I@JZQC{`wtdIW#I|kQ zwmq?JC!KcnS?$05wQsN2*LSUb&ffcTwy&`M0JDx&N~$DcV^nHaN^h~LW=S#@hKJ!I z(;+&#@rlGcC++$NoS`{Uti z$ViJgpe@|;%Y#4=2)|QNS_?E9$8Tno{xpx(+&h}_(SZT+Kmg^(N{#^-i`D};Um|Zk z%!3im%xm-)B5#S_tY51n!eJ~?(2OHA1;clxTFWY*u$Lfw9{g!q{9z=$*(g=5)mPhx zD0k5d@2jmOld-D`TZ04vfoj0$5Mw=+s92pHFj9F?EG;3y%PMg|3!}x0h)xX1lD#~2 z%6Z>4i9_4{i;_Z=SkimGmkzg*R;N4lcbb0YeYqK<+HF7_EAIC#5gHUbN@^53a$1{1 z)u|sB@0D=oHsNblqq1p!-=J%AyjEid4NLQ`8_|i?n#A6QTk917D`ZC~PA0WYn#t=0 z|M8W!L_V#pbsd@`{s4)qOzmf|oWFI6e2kZj`845#gNtGU6 zkJ-IC-pk|m#9NGDRB9)4vwmN1r%#iu=Dj(*QQM_J_P>1%l#kWRvM7-eaWy8O6Olr} zC_m@aR3>%Pi2OJKq?iZix}gv!fwBWMZSFyR^~X6+>+^T>BkLPF!h-Q~*&jT;?@CPB zhl{k?AGwLNdbD9tvz0D6bf0mPKpa%yiJ(20IC-pyiWyt8apWk~5W_w;H;b1C4YvRl z@4-taZ4NK2-)x$Qw)94sjZ7n->z^jBec|vRI!u+Rw`Y}r?YYMJ4MKE!@0~5Zz;|SP z1QEA=(On`(;Rd~gtBZ>{SNBKa|l7qs%bMBv~e)D4k*JFtg0)6aV#%YtB21jioRt2&nyd4ZSFyOg#4!WvDApw+DVVR&wX& zZ9%)NIYY;+{HGt!wpj7>U}!J?Hpt1cM`SmQ;7d>`4je*LjC?f<5ZNssJE6ykMAs8A zGLt4C#LiW_t%CNzpDc2DLBNY*$fMHhmrz9&{$_dOI;p$-lstV!?7dTw3)HRZh}Vm! zc2D;>ISMDKA-Kkys|g*W2|)TMq}Hr;cCq4VBiFTh^{efa2hw`gi-!=z9XQH^)6mn^ z=kfYUVD`^Wx1YEAXdp#t>kXz0p+;o;#>b{O6faOOb?Nt(S_!JH}1)wN?z(G?nu3Ky3aiHtd-D z)3b{%n9bt#XSMxJ?)K&HHOtygr^2Qhg|H?DgzRP#m(6rXSYZt6aL<-oZ0fDSC`4}; zkBv67SQlBMWzXinP`|%D&)-W2C;l+NUW?1rxw^%wj1+Nq(vq8g>I*8z+dZ@KZW6KC zAx6JMlTxT>Q?G6lpHz}zh^R`db%X5IS?_@$XA44;m6M6u2&6(!-m!A2h5AvC!Hh=Xk(C{ddZePp<Yacp$m#7r#uH|?f8z=E|AXek%<}K^|0m6-S?iyZ+=l$GXgR0z zJMZ{j{;sRI)fU4xp`~BHP7f?{dI`j4Jo%9HuK^&Gm`sMLdaDC!R}xif`Y1EqzKlSE zbl_HkSQtz>Njwrolqtrnvat83e;ybA8{Md=Dbg;ZyfT~&6Pjqx3(J@9-RJTA^AwQk zt}KQPJ#OjZhlV5_x5TZiN;0nI!XNwnXO=>HxI|a3MAitGKq36kMaL99M|1k8^Eb|A z{NwZ7FRFq)vgv(;&**O*PH9b!y(-$3lB(9k^5dit`vvZ7+44XQn^iOBWbOIa$>0Dm z0d_6RW4GSI))NVVXF5Ty0qwudK&yc9HgHrdO+umII7y_)BI&H?O?hbOMK%4ibd9p=Mucx1y3bk>SMDAGch#-8-}T?j;_8$Dh1LAgVN=@ zOj<*>NO0ufX1B&DS+(C{@Z$T-5f1}&*kvrmvV>XwP{IDH8`WPVd9e^k>%M?@Mt?D3 zkoT;am0BUJ$5C+jaQX1yf4zOr`gUFHlOr`4$qwH$yA`qI<$nYv)pRv>jvdB5PxVbE zsv%&yh(p3cjr)1|s1i)FG%=MB&bvH@X*Eee1SV!8QG}G_7ac9Ej-b(y=orwdnL5Ui zX$4Sem`}J6BZL_wCp7n*T{wXK7XBLK0%(q0rGLZ-f@r`3(JcDh!@7cThB8pml^)U9 zzcC;#keGZ}aSWbM=+b`y+Lzu9EIv&1Ca5%-7YzRo7-XEHC@CH&G^sI!;m!0Wq?Du* z^l6e8Dv2y9B%VK;aS~4)A|F0z1pz2|R$KT!g0${2SA;dHk|G5V)eL~)9w~h?m<1!% zht)a#hu--M?D49Y{(+!~{-J4UIp}vAE36!n`laXLP>GPV7HLwmaBOB#w&*<|r3eUJ zh~%c&xJN4#C?8r`ai?3GL0(>g-;dMN`}1i3U=;(HhBA^<{p!(J57dtabq-UpvM*NH zLbMrkIc+c}0>97%R~_KL6i5U1c}BDzF}1b1sc z43l~^stdc`md57t@A@gTJk(>p(!KmeLiYq7Y2QIy$VX|!k(Yxwq1gVBr}IkDH%erp zWJgZ~#6hA7qGaZ>huLT)Nmn`HwyJ|q}`+*=~cAxxs z3{2HFb7ufn=B{m)kc*SX2Tyva961jd*S+>;-2vwzy=hPSJHG@K;@B(NHtjVH&*@9A z3D6kDvBj!A3?bkkp7z(D1Ih0wEG(fSW(Dwj8$bz-^fT|Nv)@jZ&AREOKcqkE>{73S z9TDKUr;ph-*1wl)Z-QL03rcAg8Mblk(ARDD#O?w1?q&4|)#aCC#M38Nn#t8G)lj?3 zf1iJ0chvA3K!71m6Uj%#%JJ83rK}X~wtu-m3=tdYS!_eJ$i_)>CksyWkq@y;>Xq++ z9TjvM+xosS=i#q;tcE0!> z2J-=HCy?s-$28)f<3(I;&XySuI9RJF$ULf1uT?0R_qBvuNYsW@osvJ3_w0hY zQb$UsZ=?h@PIZUnH}O+&?o!qEYTU|N+|idm#>j%7+wC_<_vdJc&ZS-^^bI*lWNc&O zA=&vcY2$pL(K-#eyd~?_BwzX$iXxUuGuQ#_KG$vRt&95`Ns9rUAeLkg7mMsT(~Vl% zneCn3(v{$cxQES)P$DSIl60dFo@R^69lCZDiVE0R;4uuohDtLBhKfkunEdGQ{&9NL zj+%$Qv;!mAW}oyvMuc=RnMVOYBILc)6aP}DY+BekomsG;TJRNZ-L@$FFN3K%9QFXa znuY0x7RFRg{_f5We;S4;*AW?WmOzx-27KWHmOq4d`Oze96FU&qiI>{8=6GvWH?|G~ zbL6*M>tH#$wpG0u^;X?eRva5wjgqxrLnX5PW5vCY#=xRfm7UAY60p3TfC;k|; zou63z*!8*e33{5zefle6{2Xsnc=rG=p)!o%OP}?$bg~q&`26%u<}I}4?4Z37?pMWT zn<+5fVKfI^&~$CHN?L`86Qedato32cgQ%&3mZxrB&YxUK4z!i)-Pa9jt4ppa0C&IuLllk* z#2*o|+nwyp8$LL2SoS-RP4_!PITbL`T&KL;404el{9b-P=l7@m!ST+AUg2a#(8Y_x zGbTa832C;?cPbT_VJ+_o7_D`cJ_I~n)|g)Hyw1thpejJApblfd-IYdiVgWnlC1Zgb zB)c5Vv^P-lKu8MC4N9Gv{t@8&MC|G~<$J76N}kz|BcL9_hH-qWB!y;uh&FkY@Kb!l ztt%a^*aLQCzg&Yo2@b~TTYeBQp&D>@gecO*HDAfVkj)hr*oziCnVhq~2EA&$q|zm^ zJ!Z&;xuPnlT547A=Y(HZ%b5({CCfud#bmj<-MN&~heJ@}O?8Qt!-uP9o$q#yRrK`W zjf=-d%QM&G!#5fLfdO~&j~U5=w4AxyX#s||>=l;R@Ci$Aoe=Rvvw@ae=*s3_@$vHVM#adIP!)UB~I4bI1D1P%LdkHKBK?79fXP)%xpU|otsCHX@qNAI^h1+2&0 zK``>~!^1)XFOK=(H?`D6cY8m-2Rgese=Z|hC7(927rdto3ktp2Ju(W}2VhuGe6l^C zgcu@>RORzCwKXKR?Aq4NiuEduC@)tO(cZsJPhMy*MlEtWt$(Z0EpHt1$BPj|F=1I5 z#j_TS0xDqv<|U2Rs-SjTO9{S4Omk$r{fZ)Kn~BMeC5dROfr>Ms3)r`#w5Kc+!&C*o z#f6)@L1BU$7wzfOGFz@%dj1BafFrHjz)V6xqC{+diYbMwm%Iqru4YpPJ?-PD(C<$+;d7oVZgsvN@s)*5Dqz6@?zHwbm)KQLJ zMT0i_T@>ax0>}?oSO2nNDXAY~zfAkIXmacXh&OL)bz9*$87Qlc+{&a3b!pykY`~VxscW(tAjt$aqgm@)?(jyTZf?lcQq_1i0gpeB2xEWl0 z*apI-nj!IJY#7DIA36;nXGU=#uds+KNx+f;yjE^Ppw+|TAa&M3uN~#S{TWbz42GI6 z{n2c#FYV55E=TLUHUlDlAp;ck6S(FajuzZG^ce}!E~a0}(o;Du$kM}gq3;bp`5JNb z_w=);NJ7BXrB*0pAS`SY|jmnYrTlYh+ZsDrGm|j`cm@i!Nc@Fsic=YIBU#p*e zi1F2C>=oG(XN1%$X|nwi{yv*K056XQ7$qZ3TzXO)FJ@##?;mS- zz60xQg(!kC1c@r4pK0cJ-u_h9gfXiJHP4THp2bB>iV=1{dBQf96s95=c;E#$ zTqY(S-q9F0M&X_UCfxZm<&i{axN0OYhHB(H@~A0Mrcj7~3YEzF5=^iP==>ercp-T& z_mv;nr6+(o1{Otv*co)o$|RmmtR7ga@;e+|9X*cMf40;OMziJEIJLIFLO*ZcUp(uBIpRN>8{3wcriyB?;D0XPTj zQvcqwV8~i(n~rH$e*Rk*aB`hi82WLj?~t2)Lji%EXu%c_apcX)R96?snPF)q!V2m- z=dU7V{jRElDXmbFArFLeZ8N%hlA-LZKE@XCh0<<@3}ng@T3NV$ED7T21ulgWM@Qzx z^H61jQo=mYr)r4)xjtSp5)n(0*$IjSoQOF1%$apBH%=SFb&Sadz%(609Y_(nbvKw0 zFKlZg@e#n|BdQ6&OanGIa-i1aF7}roCGwiyN*%ffCj-7-I3Df$*H= z*9bG=hq$G;#ZVk#HL5Mjn;;7@<&*{DI8UG3>zl8OaM;C9rcOcCrKNg&`+`MBGt49< ztl!~Yv&lodx6V}tK<)^07O?NGDnOC5a8`*?)JfO$Vn)N*2X_M$=ErmP9a zC;~M{hnD94rR4d&D+Xp^;0&8y1VA$uuy_OyBt^Cv5*DL0GR`>gL>zfCQrHwUB`FuJ z$|~kEguCGvw#7*8`IT$Lz_Nzd3lmSKyio7!@jjU_8f|?KVC{s9s;IL#Kc=RS#KZIm zC=liZJ!3*)CZ}gLqn&p#o(A!VV1wO>)Z0UTEz8*0i082)jGj?T#fM19SUg%m#EI}y=6^~d1E#RXh(^i8T&>oSX)e^%nx!%$s zO6|zKiH>+wFVu2%@++i~cGC{V2vStwPsQl*rfY_^VBJupbB?3C7t5G#_NDS(twJ?M zak7RTM3-wa&SyL|htO8N6Uqx=jTFSFdA@bUG`UdXx#4Avrs`I9uzHzkt6qt_#N| zkYyYfpfrPpJ@d%sJ9kd@S(7*-sKxbs4b=i!6q+N?^6j}LZe9p=L`+@{(7>sw92nyw zY#}2?h)9Z)MSCqHh(tD>ryk@pLjAeE#RJvQRdD*{@T#|M-{;Bx_XwSJKyj&t^TYNIaG09;wRnJ(xp4XwIUhiBy1>LfmR}oq z9m(Qw@n_nf)g@sI)!dM;FgA^dLxUpp*sD?3K;*NLffecw><~!=&Il6z?^PLQCE;8N z(5iV*G%q-_mEco6u@`#kkC_*Kf!@tTGtKYOH1(D;^leU5|#;ma9W;U5;g|U^hK|@$lQe*Iy$yb$2rRzw`(L1$B>BAssKAgxd3l*bhhY7J+3U*p+sX zd%1ZO2l$3uYF6~#0(!3D zFKMEij%9mZqpmAoa*Hcd-^zc?;pN4xG#?czYJmVNyQU87OSyWBr^NIJ<<%e##%3N} zbvOJ#V{Dq!m7cMg`%fgi!I*(u;z}75tS1>{m7{-6)DcGZ@Xa45TRDb#z{NXKncosC zMkK<+bj63f!ESbg+dXKRw~}I00Mn&G-Zc_RPYwq!-~+x`K3WI!WPidP-GxXqP&QIw z?4Jj@z~`y;C5+j#1_|JN%4=ouBMH$|zF(8I!flrKpp+e9{!yU?PftjiN9nja1cOE; zZQ2gym;$G2GJ2mNRX%S0O$=c1dwBfoj@t?w|FsG~h@GJR!f_^CZ=~K*0zQAv=nwJ6 z85aYv*%OT3jor;Me5G}%6Y4*|*X5`d2L$;1`KX}b2Y5$WLjpe$9e`p*+E&66c|v-i zV$F*==FQW{g zs4>rWjGIwFTWstssMVF_0F0np9-VG7Yj+ZVK(Wvsa5dLzfAC9I;>LB*B*19e3<)-r z$}DdSeW9GMc*N!j_;k|IM6Ms0gVBEr2)#}BOQ^OPoG7S2w_{7KIi53N@fR8Ak%Q`zJ_`Z7yg2??Ho}<#y0hZdo-n6u&E`m)cxuP1ci^@eE zw)oEYr&%0LB%=W!!C^n^BkzAaXF?3<@0@rucL^>R>{Ne`ES>HUR1Q4Zhx z{8OR#<(P+mfKxWG?fzp9W&5x07z+#Y|Gw}CB_#YWfPb_0q|?9h*!BfY9cks1TQ5Q5 z!&rJqWr8g76}bv^c(9l%rLag~%hLgwDpPRr)*aE7h6p=iz#Cbm!Lv?e8Q_I%I9Scjf9nfZ4yXF54jg zUn2`?h*^(r;{%o^PgfP&sXf+(kRxp-Bv@N@g=gHNrBboMG%*2K;$s^kc^=_<=ng9&anak9n~dsIY- zKYR4VWo&!)6d>*bg!+=*+;U&z>ZX}Z>zSg+CGDfk0H?Q~@j!CaIKl(4vIY&7X7(5! zt}4A>XGyO6p;T;!_`t;K6eHXmRHo_g;QPUh2>%GZDtl!Z9Q-BXq}al`G&7=gj&O&f zhx)D0C?1XN4$pFg%kAN}%Eil<6E!WP#d|V2fj?>7$>44Y=%M>(DITiwm2e8(E~^Jx zQfy#RZdshb6}>@U zR>bFHk-Eu1qOH+CduaN^X8DWDGkxVJo0PkJJZEktOSUd|oQ-@?XWiGWGy%5ZaTHC7 z{|1fs7p@vWwn+CrM^T%6Qm z(#x@?w8~#g&iX|~48f5tgnXdMswQ%9kep{_p+Y+~cLJCQkXd<3En=nP5L)p@YlpwT zbto+F{BZlw4jtW35$tv>ULJ8hAg_eNLlvehy(~SXDi#p}#3MX4s1_b!RX;m5NQenj zufya^j>%+99i`kKgZO~m5hxm-5s)T_;eb;Qs%f%Thi6xqK;+Mao_I+m2LHZCR%ko~ z^ULtD>XwThkltXW*))q0<%S*i%c(g5A~~w7h#2}#i^c|AFH`x9=@?ux)-wuG@Hv499(Ue;F5E&tB;v-Gr<#L zz(`;t0%S{twVXyoFGi(}h$710+M7m#*-me61IF@Gz$N=O;5}=!X`#nGE=K*k#HM#P zijGwzn?+C|;xdIY7`1##RF%2#@E4BM=zQiHw5;rDPZ*P9+0lStfgU>1-FuF>oOP$| zp10g2Cz2d=6MnE9c2oJg?3$MYI#K3U!>G&4VQb6YA-x7t?acI5daUCJImte zUFakYFlbnZW6p?gJe2-RTptbkokp0hGrWIUTp4LYRqw*XW!Z_et)>=PHwdny-<9uI zh@pk}jOlOMCTU5zY^>u=G*-3zr_*}dg2!sP=Gd3tV;!3FS{T*7FQ75$r}8c(kvnP3 zEdtM8I%vJIsdwmLxpqu1pst@G6@3-=;Q{ulfMcebC^IYV)X_dmmVZy;B$q^XoUx#pagnH9gnSM->c_C&*Nm3dgV^rq8hj zfT|mYRm-zl8y2l4;5>OB+uDEZXQzvXc|S4^8`oh~ZAYloY(+iJ1A@yy%H z#>JVwN_xLHYlrR9t1EbX)T@VTFpf97rt01EAT*{*+j9o(<7euVD%J;Oo&d?datN8o zimwxoxqcEA7kGEh0hM~L?V&IPpK-ChfPw=J(RrbFr49U;jo3M)jHmg0{&E`~SwaZ{ zluv!5z^K}==@NCLpRVyqE8osp?k{zQ$HJ#P2X#6jsfO`X&-K50CBNCqAGu&P9al#$ zz4gWQBJ;(pD~sE9^76d+$aTgLgjOhJ1{EJ+lQ!8wHk$H+c7+ z)Fi-G0s-- zVBAj2QD0qqD@QdkH+El=#3n{UZeOY;#SAJQY(7=}kgnrAMa3b=!QDC^>$mMictESp zQ9NX?2`#fadx3j2WHA+=L1lU$2*5|+qXEIK0CxTc_8~wRJMuD*t{>m=2WUCgB4eZ_ z2YM|w_QG%-ZOj+fV)ciswR5DQ}SKu>V4*MdhnRJez-Fd{74MV>?5jNFA|Q-}yd zjeX8vjfD^AUtwV*p9@WV^I9E02@;6DgU9W{#D2RUpa4f&i^w;@t>lqT2;<|i4n2;MdR>PQV)fP;7=?a z2oabqQamCEyxsnlIO$yiXZ7pLlt-3Z)I^L7uirsBhabz#Z|CQ8TQzdx6X|hc%V(+d zJ7T5wU1o(0`nQn@7m5a2liY0w`x6(6X46%PolZ+B0P78rK3PVp8WDhrSH?~Gp1GE? zO{&@W*sTs0!c2C|nH7fI31n(1pD2wIN)!G%AZ|D&g|U@lh=ADeT28=FdjKzaa8WNl zxr*x%c2A_&17;0T((ZtcnVn1F)Qhx_XTyDDGwo7F`-FC3s>SudqI|z}@v6T?fCLOY zki0AqQ8!g7WTtt{U>R^}aZRh`?$*F^>(aD6YeyPIuXToabmG?$RsSV=G5rr`qfrc{ zhtj<7SslQM=qKl{2ETvsg6ETOvgiFIJX*BhT+=FP6AKbRauS3GQDG)x?8$a<`67#| z61#wcyy4x3~iSCs*61{`wTZXQ`|{{0mI-ld?{M?L|nF3{iIuq;n^KgNt4Xx>zqX>w}= zsQ$9qyZcBZI>lffZo63J2p$r4Toa}uyxPD&jnm2zQ?sYM#Qy%%rj?p!Uv^%ehGF;X zKKcT2iT}sif`5DNy$eRP%HGnAlov%W9E@D^UU(%*rSRrb&iNVA%{KkNN|qe|VQDe5 zvvU3)*`fdZ>5sH+?H9+;zyBJBcB_z#YKVUhr&3VXT>ER(J`SmjVN55DGv$lG`g7ECI$A4!>LXY~n$f z#&gJZz$B2FJC)Dg*(as(Mx-N9ol4iTX^*Bd@aUboS16QPI;04ZppCM)CUwQ`nxu!V z3O8wEe`dUhVpTJesVjbN>FDh*@Kzp6cYjgqHVzII1Vd`=+m@&TJaI6dN+~mI(5dNM z32Dv#U=c!@e0Mab2F$P6HN{nJ*4sRee2JmTm$F)ao2YHNZdQ>}nB*#+S1ov?97P?| zo;W_%CNeok{0rl8P}?_{ssGTn48D4Qab=~%-2Gg#dNZLJf6FOHw>q+L}T7zKXgG1V)ue~ zPwha}iMGKyWG}an5F6rI!+k#vLqI5=cd3uF%Cr)1d04D_hOi37?(51;l1D9@%X(nD zS4Jxkb&0&q(fT`VG!dxW!oSe=BSb3G0XobH`?7G4v8;FjET=J&WWU!)e`2V6sgmJy zQiKlC6iY~-bHO}AM&n|oY>&WIH@K2cXm{Y+96Q-5*)CoKHfRw~Y~k^94Z7=7%n&@M z1d3P_^@yp&vc|q-&WB^e&!|5)qcjp98VvVDTtfFErC>&SyNIroVota%xaag=8>k9B^d3*Gw>14MNB$ zCB7uz9&epmBuFoxcUE$QW}Gru{J?H}1itj{veU#AAn0)UQ;CQtNL&^5Bx_+0^;saS zNAa1}N+$cy_Vd!2DU4?6cv(r1PsCBYE)t^P8iTX}6j5?o2{L@f6KbsxJfstUE7tlN zdvtui_PcFCkvYVT6>5h8$A|61ghSVmW3VgXdO*B9W4OAtgoy{G33J}J;YN=VfAV_( z`N6w`gV5szGARORshkCaJnF5a+haOg&PtxDHq>ZDh(P>^d^jH(L<`Ni)-uAUSAxBw z15086%MN*QlZMIsX#5FTe=zr>aehp;q_5GPL^~vOz%`=|128JS@0FS6-TS@OzzrY9U|Nk)$8>Dd-w=#TNM;b z0FvHK(vdVhU|B9_m2j?(CWZ^2FIDPSyFRQ3xX3W; zPH9;M+t~~=;UWIwtPFPuarl09xZBf9be3U@dE&~sUg_Ul${)N2UEam*%)CxFlUf3` z@2stY>YX|Wk*cXR!na1HymSV?c;L{|W0DYk4{yV1Oz3%ps39cN(7}OMhYbZ!xU{SQ zf8z4*sABWZHsvrv{^>rk-SqJWycKTzu7DL^mFnquz4)YU%gNHgk##B)o_OB&jAhEa)P8h7(ZERD9X^}nNThka*}rd=D}!Wh5s`FmhKRg~ zDxwY>QJsapg^;7M7s2sEY89>(-)ezjhr=A$UYgxv3b}d`M;0lDdw}2u91Jrnz2BGL zw>?D$Li4TWUA%{m-?RS`T=;gkTXIehLPDo7iA9+W2;SjlFXCu z2;YsN*~(UziPWWXgWF6~$S3=2*x8;@o1n;&Xomhd%L=q0*4I`AfD?>`s-7=WU#$Db zw3sL}lge1u0BoDyt_TUB_3@rT0Fic#g7)x1-sgyX#7i9Y6Xt0_r*sv&F3y?e@7=(S zSOu2SX*V`YH47(iqZvFw^gTNy!fc$(h%8P&vl!F6UcRhli^Oj`FaQ%=Dux)?#d& zS?Oo&5nBASYQ{MtxpubpOb$H1piS~)TZr8UcJy2vj_=1pNMt%!y;wzw@K|Ze|MzTF zPu|fnH{;X!dbg{?6VpusX6#{-b4dH*=`qsyMaqhA$#Y2-z_+8puyRRzKG#?DJCBK( z>rglpL4V9=MD?;G5qQZsaIs>&u;#>n)4qY2{wC}%UOBOeeO44FFhr-?^r{lt;B(St zY(@6#gg{C_9@ZpKp}%lFCAVlLGJ!G8c1!|-dQ}HAM)iFQ=IPnKp_k#G@Rzh!C1A)W z)$Yi+GX3Z~0GKIsW9-n>B+Y6!m&%uuj86ZwO-D65M@6+w%-0oar{{FD#jHXp92x(B z%ZFUGT}Y20>kh-ks;-F$X~Q)qXi$8|cF1EGxpq@*mi!k&SpH_Jhrr| z?riY>`;66TQc6Oc5nTh#)45NAoosf?|23mg|2skZzn~zT|8csOiHYTZlyTwy2l+WQ zcNZ8NLwB1!h8U7PLT!}1rCsU$g6A0-$OaNJ5Sv>Jb1FyF&5(UN?l&b* zSHF$9F1owt`USSzwDt>hwZ9iS$W#(yA~B6vI_>GiW(`L_1;-na^p^*SW$|UBXBwIk zrjB$pa8inDieeyf`B|ijH|tq5!?irbmwcX=!?c%Q4R zsE|1A@&`5^T)Y7T@JUpezV;#xi&z=2Xv*fRBBaQ@vUyvHD=P~o-E#Eh(Q(o= zZMXXidb=ki4H*tRiY%*awT7uj?l&X*saoBxRS-DvdlCSyX7E+GI8->lb72-5N_+Xa z6&`a}j4MGgKQHB+$an{67?ly~Pg<^E=x&#*EdO)?_0Q&bf17aq78@Vexb35!lGmXU zXCkm{Q8LF@|8|+$$J2WO-}%?PYa6{MTKIC)B1lC3RBrLOLddkN89s(%ni!zds%DsL zA-bmYrGWz*PP}Ga8Z_)k|2I)sI?FEb#txMRw$K=R3<^%S)3$S&4k=-+b*jK2FdHB= z$Di8g!tW;g`&v)~`#cQ~lgz2jF0Sx6jV;>XoIW>W>o1w5-d?Tcb82^`HjZxpop-pD zt66n~ZQ^nT=^tbiGnq+Jh6YCINS}){@&I8%Yzdbd_q}~-qF;qD0rO(C1~JdD8tbr} zl`c`R_gbxnYcP9VX?z>{GS_W?WYHg+7*`*%8w27*ulV`!mGa5xx?u;fT zaa&HnwBRUsly$i`x_dmw#eg7FmR}wP2eTN_$Afd)vN?zlLHqJ&JOaeoK3sJ#B3)#j ze-%K%ZCrF^&z`jXSm8?7Mn6n!{lg1U(m8#~XLhXZQOUhjjP2bw2J{Yq$YWkjxrQEa z9pPT!0)c}63Tlwtdeq9P-?rEwChi6@k8ny{HgdoI!#y8|miC@V78(OzOB zQoTmx9Wg*$S$n&kcLVATj`dJylk z@Bl5z9UbmH=AN)RQ^Oln3+J>>2NLbfj{rDlanq`--`s-%eAemLiEY2dLGfEcbe2N- zNpuB{?`pD@F98=WseTecO;J8Gk9LfgxB*A3-Wnnh2wX~|&+swbAq?>T(cZ_`Hr9Vbt2KO>^fJ>X3Ff1{N9;YoVF(9Zey@o9q+!|NW)#RzwSG}lu zAZ*$Rj#H*rBoG%0xfjhd<(L~n&=Fp+Mz0U3&%nC-8<)bM4syL)>bztL?T@T;((Ke( zsWYm2tQwq$NQ_u{JpHvS=aJIk_A#jBp^#|Wr840zeL(gekX@;9&;_tyJ(K-}aqR)_ zvh8&vHQ6XQ}*)P>L*UCpOU_<$Z!VnenrtAC1DIyjYqNEyg{8*3NdG5 zEC_>SkQq@r+c)f56r{(p^`MCi1#M<<2|Olbno~ZK3KF|^@#6{%N)ad+I=FM~Jp<4F zUvo0kov$(d`{6T*2Bvi}dxntS|7{aX9xS)kMiH&AUfl$oOtE2_Neb(06!#fuMPI;} zz&PZ|hzLZO%p0a8ea7AK4gAWCc+lo)UNn^8nB-vp{^Zv>S>J6cCbDrM3u()|LVmr& z1U(tpgtxD|cWavi=v^b*%Ek-b?;>9_D8M#_?jRIu?@I%;p6XW5yZU^S3#cM|ySV`w2etA|c%!T9VCo{Hp#r z@_y{amW4A{7$Z!2 z&f%z{mbmk{iY*grl1Dg-7kz*{z-e-YH$Ru@>rPs!aCh(>>@aD=i3~4`{|DQ)KvIm8Xw!UqqDbo?)&##&SBuq3DTz0PG+Bin!liQ^l2c zR$=k`@>1;ywFH7FV-ZdUu=96-KUK?|ygg7fz2!gAfl z*|fTL*V60OAAnNFdMvzgW%;8XeYJo5oP=FjcJlGrD0B;T;3^?I0{jf!L~zrk2ikd` zmNWQHaK0^>l3;FPnF>QE)W=(x`iGR|Lhg{403)9B6B|@tbuwy`sbUV`~)2BXDlKTlAR zHU|z_^M5~-!hKNw5CALYn_}=gxSkmrE4^9ucs&txlR1F=hg@O&zk53W2@%BmKPiG7 z?Ee=8`G--Br7HeO0@5 z?_v?e>4j>2o6D0K^tRi4oEE+qy_o=V!aL)PSkmHW;+xBwMkH>=t~BR$FT!APuh#qRWhOR>3lJ{C`}4&Fd>AYvSm@;*a|WCsIggV0wG zzqc{SkUdu#Ojrvg1$JN!y70L2Qs)Purf*>*J~1I%GBf4_aJ9T)tx&8 z5L)hiQXeabz!6hZGx3*Ew`5Y~e7=LPE#6)w9vT}=ONq^jMgj|pvBE$HBTXQ~>tG9m z(7s8En@E|_aH-&_reY5T1X|67DV|ZUp{H0)!14mKL%-=^3x@8hlmzl{mL#D-V-4{% zzdCqf;wnO3N>P!4C2v`WEAR}O_=~VX%}uX%3bLATPCK5y;)_E7QmqmpP@s+cbgaKL zKhD%2Vb*_*{I%9FIx2lAIIsuM_}OX=i6(xnCKyVk7V;&Cb__l>Ybe|stGJ>1>@0m3 z2WCqjJH}UhuvbfL;HN%bpe0F1)_P7;b8EdD+uh~j=t2i}tHFHk5GX-^qjVCh2o-U1 z%?5wDl3s(zNvKRM?%fnZy91T(T1yO1{P_G7nN zP&IGILa?2|)5~XiVh=o-NOm2E-C{+Do?T0QjYoVXA1Zk}7}Y9Vp!tRw_ovs3mm3pH zZ#IKWOWntP<{Al$8GV{8PP*=`-&(dJ99}v!byT}BnGT=1R5j`_E=Y??FXv?}a17qK z7NH*IX``uG7dATSVA|S0JAPgPrM>j*^%xRb=}e%z z>%?{`Sk_8!*pe;6r3(sq%zwD>Xw@!d@IF<_NW!J|%bs_Q0|y+x#vf$&z=28desfaA zS9>;#Te7_i$j#LWr!S}CD--kWa8(zsWIX4+5LsW3H7?=7LT)7|4fl`f^q2E*PJst> zOU(0!Bn&z*krN~l+z_Cdk@!B-zDtr)qu^e0)8fIPJr_P7xK~FD+pN9dtx1(`vYA!W zvwxSam?_o_0LRAfy}~emqbHwJ{wy>ODshJlwmd)N3*pQV_I+scHvP2MKv~_JtNYh# zZ2arxzyxd$CmaP4#x7#lkPqL!k5YVSN~m9#4Bu!M>2~n7!HY$)G4|EWwIvq!_Fdrp zN?oVW|DY~NpAOcxnM3}jVw36QWJANVeH3W9?&2D*xCQpb;$zDP*L(Oe^1UUoH&z%6 z66#XIV7!d`N*R!~vZVrLV189q$Q zYAKVkv87L2PWq!ZBPng~p2iWK@o5kF_#1~;$%dpd@A5KllF*H4UBp$fqdl{&vJKPO~6{H z={k#*NPRKprvm(d4TzJk#J-@M_X#zRhk~^in8sNP3OCS?g3?D7*gyxY`}>#nBzAHD z6!?=hjS;({IP!LVzXQ@umSOnikDT2kP<(~bnOM82F%6Zx*dNr;;^nkNH04QQnb!pT zLIc4BgB)VyTI9*S0%V^Yh6RKZjGE|lig%#&lX_XAb#Knx!Hy3elP>eKIRf{rVU+l( zJt*`|NZDe>T9f6Jl%S%|m>o_tT-ll(w;R{K`iyb-waOcUXma?k-@b7m-9Ut`#S@gd z%I_6a=g#aw#lCtV_vEW2Ej)jY#ZmBfH*N!rn>Ptl0E~!86&y@yQZh7(ahi)mWAatq zh>jn_J0jf|M|S0=-Bi<$@#O#Q*Qu}2A>SUd$Es&f_RZ4w)OZK=MA)3>tF&5;-sSGI z4<{DH2nL<7K@AATr=nN@8o00szUa1Wi%K>OJ{#<`kdzz&$Bzxd zfEeA9Rd&@j>(vs5(Zt}fi?*$>T6LXPbKYy$#VcI!TynbY;*~1ynf2nTORI)fdRb}l zQv~l1hKJ_*TX)yBuRka%3UY&R@3M?DD6$h7O($q^c9-|~qe@BYSGBf$9FrmafCz!) zav)ijhhV;h8h*aF#A!g3CLSs{$O!tm()=z#M!^l%d<4Ze{5EO7LQVP-}-KpxCz=!xQrbsyxx~^IB-9>P)WcaA@oI_;FT4DQCc_8WCk(0*O%{! zjXCEfwJ!Jl@%YVmG_ zqdJY#vucA?8VU_9gGLTj@jMw(bD%`6-?CcN@pVzzN)tUsK7wo?nf9Sadq8h#h$GAu z689{2dNK6|Ovn@-Lz@T1^uuF$q|A?s7doXI&=ti$R7>nJYN8ml<>cKkU#fZpRAePi zqd-LSUQf(-k@yNwhe%+@}kh zEbWa7+R97XiUGb*PqhOe4nTtFZeU=sm+Pf_t?fMEQ6k^4d!-W7eSF0p!J7TpG`j#W zRe%<)kuedQ6aU3~9?hyNu*JI&_TmTnD}2kGnZ7^^C}mMwo28o_QGHUEHZ1G6Nv0WOnA?FCoc)o}$^9I+;Q4a5Dd97V(>X z^@8e<8ri`A-kuziguXrBE&r7G_9} zzwrPB{G-i(wh-H|pPvKx7t5D_2g-?ut|g1F`i)` zk|}u1l_I}PGOLO46CTKto)%IrYxu$!;i(4X6n1&FG7|RC0RD8RpT!T}+1}>6^UV@aYeA)x zxZf>MyY^Un^&m+8nDIGz! zO&?STQMtU0d76n@Ld~tzG~c>BIxW@sEFjCi)39uj-_M^H^4VD#4){c4{N%P~1Eb2Z zhP|IVCuI4-2vf0L2{9MZh)7HTz}}8%Mr?;T=Q|k)!w$>L7_^ED4>dv87tMxpw|>MS5Lq>^-YBVj`WM z0-7qmoj|?FsE067khAl^2?5CPz}InP*pI$1MAyy+<^Y6Z==^mbqhQ_bxmH!pXLF1Z z;idAx7&{pv>AMbgs^e+92Ur56S@0+`H=e4k;lkXlwUl!$_=GWSYZcE8*c_$`wC34o z`eu>>y!~o?wiZ3zTm&rFM=C2hV6UF{2ktFmZ_qQjTlc?A1&Vm5vQR~CK4x`<;#M=b z>#H31B!p+ptAwvbm*6XU@2%!uQ(1N4alM?$Fj#(nQSc47IHNTSax$g}E`JPnPzMLD zK<;5?T$py7rgO657PIt`Kn_`2Gs$uc`>alpbEZK$Y1iQ-%ep>XCzz`ln7s0TqmGnn z#^RDx%+XzMQ-k>ay%)3clbMu$sD7;{+NgEBjhDK>Af}CU%6KAGce-_NAO!W%avrRo!Zwyn(Ko|@-iUQ+Aa z*HT-?-1CvtcQpa#gL`1R0)?$t@E)j1lNi z5tK!P`<+h1?>K(6da$faT&Wr)8`F&{_3%1|vY?2HBJ&~RTyEdtEc#Z0q&PFJ^vLKV zL3$N+N_s_z?rGT_V^DAu3g<|EoCp}OAG0}yN2~xYFDgFwpmBl4UgYo_Jhs}F;ES6B zXHMLQZ-_ubE*~wv!EIoH44UCp-gxTs4n)zeQa6)q@DQpGOpx&YanLO1nMKNVUFfmv7nkX2Vg)AxW z`h!Oo6VOb%(w>2nyyceLtzX^;5YE8+^rxZK;5^nhpgStKMJ=I_m)4^?a*@Z3qH@1> zW%?#8PnsJDzd6ze5Bfgo-zX755SM?A#G~|GRe^UpWn-*3W;~I+jk%Lkt{F^Lf|||` z=opuw)>lm|C-62eq~ko?>r8ZUUK6G^9IYt|(@5E89swDYE6jF1co`tGrL=D4^p z>#IZ9xK_VtY+Q<}pvcjm9i>ybhTOA5d*%a!P3`duOL`X8zvDlEjs3Tw?eFjTUxy}% z?9$MrbppXq)X)GH)?YYWXJ`5+uKsdtTqg>GvizCz@8bdhE60D$dB6p=3kAu^`iE@( zA(|ZQz~8X^*ECUipq|4(a{hg=@IQmwVhG$?{1a~DAaF})r;z)mSncv-TiNQ6RgB0F zsB>-~7N4{5;hx0ByMX;((KfNt>c+Fa6V@6ZdGA%??65EoD_Kw8A>QUAFV{~zK8kgd zb1Qf1+A6ab7h|KkFgIWiGhLZ<-l|9G*AI}rloiV(8@-O(aPDlzA3q&5yK!GII`Y0@ z-9df0$1r=+|BR-R(G`jTzTKUR>f$git)3w(CvLP@%1Jh?#^SedPq>~!U8X3Udc`V2 zrJ}t$!kq2e1nDPl9xp(54;RO*BKtKOD*<0OxLaaI%EnJk_U^$`sH1zQRrO5nU@jsZ zZhsnLYxcr3*({UewLY7p=~LeH)(R3GRGl7H_Tm!aw_xrA4c;Gtrlch`E1#Vwt;l(u zGPVR{<4&)gw*J@PmZwg+x#oWrZjXM#tv3X2Nv}<1&Rmh2@m6|@l4?IWcqSF5qg4?)Xe@pU>Gl_br>JB&*5oIuji#kVlxxnwSi>s zm=#^`asN#CmvOgxT%re6f{Zd=B2kAQGGy+%ueQ5or(D;my&Tu6?SF&V z9jA5bbI=JU57$X-WW}}kMUU*yC>a-8AA{xJptge%5=Yx3JAlnQZb<8cp?go!x(O!q zE!P6>_wrS$5OReEi!fb^yIc76X{N85uZ$_AAB`IzkfeOdQTe#ql9rijJkU%@T=A0qu{ zxc#e4|2MexgO5d%!WJe+%O$EUNwKJz#Y)5Z0z?e1zPZ}xFdR5^@qES;wd%G)2-foU zT)H!#ZZ-q^5C&5(b8MGR`CLAXd8n$oD}It)K)TXUZ6q+gdwVj+wyA@<&b7GtRuPpW zp_ye9eAh7XYX2!W!lz_dh@aU0p%{EyFu=IV<$QwhFi0e&dk1wrdU0&yt}E{!5hiQT z|Ap6Sb~d5`^OPMCns6@VwWZAQq$f*(_>f3^--1#)CY25ZY_mS^IoQ3rP5cMgGCpL= zzIC)}o|YK;bYR5mgsb4OF-+1Z;acO4Jqym5nxB7YuwG@03vSme6V8irkzWkE>kXlTKq zq9L1$Syd(!d3kEo-`H?penXts|MZLzf4d#Hfn07dFL@{LBXI+@Hq2Vr4NC2;-nxvs z0jk^+cX45;6w_6UNEGr!`3Y`i3p&WS!7MxQr-g$?U{a{uF(;ts+{OIny8=Pa*U~_= zYxLEUr{u4yRVt(2UpHIh+t&^~_nQYJOd~JXw(C|KTYnQB3X!wKmr5mMp$@v5`vF)F zy(`arMSgSNp5uT3ql0z9Pc@dgbTbDn>Xjm=`bGSM1kF29RP3AhDcPDe5NnauX zxIGHik*?6bpede&9Y#THd6A@=bSuVhXsf!u*C`nz%_FO&Jta}ku&s|z4#`%gW_ zub~Tqwi3`Jpg-i}56JtUj7I-|ibtb{_C5a-Gs5JokFWo>hg)4YBWI^f@F?c{81PPM z8KEQfcWX3Q_Hs7mnKZpGS0*wENf823HD9<2q?`G^Pz%aWZ+;UwXOykw5L?l`<a)X;d(oT9;}m~rarPQq$~#y!dFQy;YAJ^9SEmyuLHeVxmh>tp7N?5w z!RPoBV+jsf1m!+ms>Icgtf}RWW(!+vSj&pnp*0247o%zG;fXmpfti7lcQ2vCtohW5 zHi_ie%j0nRh|1?QY0H(vxn;!L#ROi6-tlGT^kMS9Wnj=`fG@8TJzD^;9Na=G@1f!gQvZ+l1J&N0<-)AU!iI zzJ43%Oi`?+Y8DZvcPt zAHS&3>5ZT0CJKBr&E5y8O#X(m3b@E?)~r&6SonB3uSk;Dr*vO3EV}H?ROY`+M(g5l z5Yeo(1aa8&P}V&ikVuj0@u|M?bG)5)FPuSN{M$9BYpe`k@^@&-sxcDB`9Opq9(XcJUqv7)0J2HTqNr(T*7l;yYD`Y~2Svkzuf!ISB0ayXyFU4uP;ql-~ zU{K%)?KB)a0tMhjWMZA+MW96FGxZ#p66U}PC*i(1?rs{ z+9UV-XZ#i1 z^jrmoJ$!As7ZRu-VFa(t_iGcGHM^k{i(_whMKQ8x$&Qdpf>R1U>tYPhX@Sykrw-dN zMYb(h{A6+=1?YrR%-gf_n|6d3rcLjG_8hC2H%DhCqr$+HZ+5^w)VtFM0BeBwJ$Wn<1Acl$z_B zK5w57E}}5z8T|~Are(z-&6F-%7x9)DP8VfvUWgA3rad;tLJb2MXpb}dZ)+bPK{Bal zpupG!83!&NdmERQC(r?E7nJsS7C2<^3A2=BZwD1JFFE=JezE;xw)p=pXTgO!A^%Oz zI51~XAUkXQSSu=`u73CJpwg8xPgIh4(mM2AZn3^OrAE<=Fr`R@QJe=)=e<(o%3$LS({Z8v+lLg;UmomAGGhGt=>tx0i(L4V7XtgcCs)84mZ=KV0t| zVJJ<}U6h9&JE=snUXT;zgo$kU1V!ynr#8j14my~oou&I0UI8=Gz=2S!MeWnk!dAY) zUwN;OB=rOF+)tJ?35;$oZvbWuMA+q|Fl zMK*M+146=cNMPxu(>t{hOC83W97+S)zp?Dnq2r2I9#G;@ie_u~AD z;{&-}oMt3>Mya_2kw~Y682U>{_$gyPwDSV8wDSsOMlX*b;^y@=;W}Yert`G6OOiJE zXV{phqZ>)sI!@Jwq>0qV9+h2d!3p;mPAg21jrCpeB60)e@aYwG@=EbJT$fl)g zAMv8xb=mRZdhh72pFa5)1##WNm_^s~{zA?;{-mf#Y*vNFqhh9g!N~9IWM=C?L(8aS z?qp>Q`LF0~U}0?N1UVCQFxGdnbx1^0g~m2zlr(mCwRJFZq=97m#~pK98zFrsV*rg1 z7c&zx8;}Xe4umW{0$J#on5mhVs3Ga3ZH@kKNt7J)?d*(=0E{B~R*uFm66I8(`M}Ho z8YN>jfRwq5F#u9FGoz8Q3!{*+fw{g7Ex^)F-^t9;SRdeG?BED_2mm_+3j^!VB(y(2 zJox=fANj9Y+}uu! ze7r=wvAWK>aDs7P#UScOSjjZGT!HtI>17$7jZl>p8cG)V`6lanYr>4%pPJ6CKVryG zo0DrL&_oA^Q_GC^P^Q@!r%bTr&=nNahYE>HswSI#*nV9>q$)jYC7VyR76axQoT6Id z66e5=0DOL4Ans4~yat!xgJZN<9_#XFA&sYgir1ST>xF0qF`}skxN4{a#HyKK`Ds!# z)_W-y%q`w%87PRt46p(fHbf|TC&VQ>6Fox2-&99WP^!=gzBUP|89_5itdBcB9*Q|+ zWiFsiBU5h35uJG*GbWv%sg(jYN|TH+5~m`n#uCqG<-y?kiq!EvVPQ_{hrNF6n<}m~ z|I`94zZbW0&@|N7+|vN0bj1zC*z7L~lJLUggN-nQ z2vcxj#;T1ZEruwi0(wK8;>?P7qWKI5On=lS@ilBX`g!B%Ik(i((4gGlJmK=O3?ECmv5|nFX*#RrD30q^d%?VQ7Y_e;{ z7_K<3FIKc%Dh31ucGWXTN{v@9!HtP9XPA-`_ z^^+Q@dnHomLh5;Ht;z4;djS1cYU+E%Jht)4r>OOmPrcjcpPrN~sK8kEKI~vrB#c*F zyHp9}qcJc^tT~Qb-~BP7UYTZUc-9ntPQu$)Yqt1JVaJIx7fq)rDSUn!>^m>Y zo5!6lh6pEiY3Q#XG!mDw#u+!wyr(w0O%|?ukEC4JuK7cIz>e2F>W%V>W$#Gf+h-@+ zz@wf@?DY5UFkjb-IgzF~yR=*O5jAS*s9!KFH?$2V;}EQbW{7~{gWoza3yyMSU&kpv?a^$r={4Oep!#xbl<9K&IyoJQ%HWdQP|JVI*sefIfj7H z`x#Z(SXm^XL|mR8oE;4Pft3o|F~vgBP_E;JZusi~@4y5@VYuQPt}(d>i!X)a64!JR z0`$?cHAe+3os!mU%B0;5eVL!9;7qi-8Yw@!%TB}!rUNfi5-6kG}-z|g#FsV!{8WGH#l zVy&8y`qZo|NfdXkR9-%LOuyM8WWOH;wj^pVLx-6oH^wOXbY&i^FYW5%DICDK`NEBU zjj7i&IX-FirbvETOMOT5Ig%$CLE^bfFDZ;YA29Q|Une4bv`9_Uw|7*m!dUfK@Q;ys z{CaOi9JcQOGGNwW2JzB+8JU}YMZU=3e4a1a%hAjumSLRs7c=cBeJXfpd9*V%2@U3K z$n5AwsYpv8ObvHKkUS;hE-Gx>kuvIrPl6vh!AgZPE9a%!ggy+lR~euK zllDDj#ZkwaCymml0nb2NL5KLlvDCvZbhl%iBThYaH?ZH8?~X4*H$rpnrSBz_*2=4I zaN}z&d|j_9_0G2Zri0iy9@AmmeO+ANQdnEqvp%F;&UM$49WfpA9~?zP%zZNF)~j`r zr;aqSPix^lr*Iq6T{Km41dvtR81OPhR%YE2Kk7}|qLH#jiu9^X81~kjT1H5uI9e0EL2h!?G&4S{MeJiTzaTTn+&aK@e7YLWWJVsG-ikE3EMrCufo?RnznwOC+ zrr=^+;|d|t+E2PTgVQ|*>IVp!eToFS4&~k&wk2(Sog3RhsHE?#tL!wiN=$Dm6N9lN z)OEq=pf9fosx2ozvYW}lu+AmHEgP(TbOVu7Ma~;u`d_>C`jE-!xT(m@?FT38hNP~} zAtY3J!M#^$t8#Q{9b)uh98f=LC_aL5#yv0qY7XssImT8=s+6xsIlnW^nBU$_eDAGO zoOlqbUA1ppH`|jYJs-YY&Y3E^4X0TSmu?H5rys7YHv)H4saIlaV(We$v`8o`RsTA$ zZ!BMaItr&L3U8=8mvKOM*ZXJy#vT-&^zp#GYXLv5ZJtb$@)P)hxCtHg1`H6O85>}g z^^&7C7v;q`tW2$q-VR&{GQ1(*$g9}3sG4T*Qb>MHT{QLm!{rg$${GPIhxh|DNh&OO zK^u|V$Nu_$wh!H}AS~7ET{oZ%?|a;?CQ`mMd=>l$6JLI61jMD`E4tCPG@L;LHf=To zJ)Tr-?Wi`bxEr{!j;odARGP+=^$x$=J0A&e zmE_$?!|u0m(qktxWB!(pSuX4rgB~Bw%#_6uCKa%hUt6Lbo;fI%rOynS9%Zz8fbX;N zRBNotm^iFY-zmaEZmfa2AaRD$~_uCq9U+*T^!Y|1Y3OA zcUNnbcfa0T-sqYwLzr_ruX(dov}^FL`zzG^4c|H)KQpv_4tZ><$?K3J z$`6}vrVgi@n>WSRd&#t=gS$UA*TJJVfkhD%W0mM7wyDxHb&#C}X160`H?2S1o#oT+ z>^kY-s1AC$aB44)oNg_=agkUwEr=rF@#f@vOtt0M4x10=aCizRl$*2fqpM-8$%)rM zx;2Vx4{{$(9PG*PtMZs{A!{#A!sbxZElUuVS^t);q?b4umaVl^sa~N0w+0?|YSpPo zy$IjQ@Gin&J(epQg!gtRAi6*Ny0$iX1#|wuf#AR)KA0YJeo_@Xsrw0)Mu&3eXnPMP z#Avw4h~Aazy>0Rs5R<_q`xM2cDWy_5nK4!4D6Ss-*5hXV+Q`WK#RM7$Dtt|T%Q6)E z1QC8%0l9?wYfGDx@{Kz}9XqgxkI%|f9W@(ocKR;Fovtv8G%q+mI7t_)=zGpkwga=g z0j6h&f^xJbwvciQCcDvR3$Kz;1L`=QR$qa&y{k6{fD&A-2ql>FPmZ8Ff9y~cMsKEU)L@UrPM-m_qGXV zoHD4izMW}`AGZFeoiK@LO!X?M_NC$hiesWjEu2a|v50XOrIWvk>-fO(_)5bwD?x1Q z&%hE9AwFr`&kMvm=@KRE{U>v_c)CvVd!E-jV@;*Cnl5jM``%H4`)Mw3v<+65VD@<| z>e)Nev3ym}G&zEwCuonkxQ=xlB`+X_jkjMhs&LFzHpv~hhX*VuKUN=iuFe6EV^Cr%#LDuXVNG2v%=GwGd=Mka)r zRRomADXopUBSlK1nm!Wi+iI^f*O$8&n{!q0Zr3Rp#E(xe3SG~2KxYciK)yrO7i!rB z<*ldJLanWR5K@(C8~-9fLOr-HIq^bhRe4cbb$X{zYf3Q;{OOa!B+HKL-l6IhEv+Se8+LYB-e$x^ITMM0=jxjal9#S$D#RU_X?j4iz{ zyZVv46e5sbt?PS?^&>Ac6O)K^FAb6$_5^zBdjvh_@jRes7kU9SsKN6MRXm1I<dwGq`#2m4nFWS3tx z7(86G-XsdQ;?SzzL42u<=NPv(*5^|b$VzR9pG1702fup^&Lztz#c@3EAJ3BrIJh}C z(WpgpdFy4dKWRp$z-v@I^RjtJ3rEtU|KwwzoK9K6pzT>Ue3vy2kC!42iv~6>9!}2Q z&ivbEyat^OV)%=iyGK)|#lDP|J%JmJ0zB2a_=C>F{rPd6GEa#GcdSj|G*H zL>7|3{J<4kXFNq1hEC^17pMiup&M^jdjZbu9|u%RCqptd9H2x4ij@ z%e%m)0|M0ASnWl~cRrR}Y7E5{Ft7U`RKRyYI6d3>+PWk}y~;{D1e&5ohs;WA;_pIw z_iy(5DIH)*!@{iEmi5(;J%)ffo<3~RVk6GYD+=kmn0{u6&+=p%_}_PEuoaaPw>B7_ zs&GdW8&~cJ>8J8YG_NU}Ds&0EWt$L6rpcCP=8yH-2u2r3h(?A=QnslWL(3LIEJZ4G z>x#3VXSY+wn(nI0;g?>K9+riw(q~ zH~PU~SSW_3Qf3jY1y#AlIe$xn%B=TmEe&-A2NoFN{Y#;iBK zNDQn&jFhLYlz>evc2q;_FmM*fpjc6%O1S=ZAhh7JE?os>_b!3Z;#HFS2SOnh+i!f~ zGZ*3?0b(Dd8$q=c#+rEg9P~wEiSgb>HBAZniu>RXgr)OpA@gnJN{as8rsQ2iV~72bvlZS}#mq${$yC09ICk3H*^dbX-Pm-Kx_iw2MFmT^hLR!`6|)^g3i(iZ(( zJqQ18$^M)EL`}iiF`j!}#av3r1CdCJ?{^074=aY;Bz& zkw1-U6n^F~v4vdy)Ia^a5&~#)gFx)UY#>gE%7{sjMTAvIP#DO;E-WY@#KyuQ0x`mW z2l$zU<-eza5cr+BDmX6TJ>O0wPR#M*GRME_vmS0>XkeDw1@q;^x zt4L)SmcP8>qn^I8^$$ig!b@^$3i8^{iB16ub{Fo|jA82SwAQNz?^VYUPeDXta4cGk zUe=}}V4xc`DPr`o9j_)<$Wo6!T=6Y`9L&C6%z}M3SW6A{|EG>}Z{=Vu(HW!ZbjOP6;_)&MNCE2t_YBjmne$*sdAHO>gBH{uuq3ZdoN0 zXXz+2x{PO9h#ggDGNSF4in1YW8oC|95$lS1^Q4>Uo93R*0nhZlF;K*n%nmfy52uCQ z>Php0$#>8>Oz<0gs_FQruW(99X0nhHS-|f~U047k0QaDP{gTF9!u+h!&Ij8<>GuSU zox`7NVN+s+Et)VJEX`U5ULA`k*d&kdFk zQ!}3_;C!OrZ63y1si9pkNJx>3V;@{mQZl< yeEJxVT8JK@m<(^e{i_@KhwGT1orj~7zJrsS1Ek*ovT`zUvcI6D6qXfv@qYj#_?8#| delta 53519 zcmeFZWl&^Gvo4GcHVm!<1C6`8yAJN|?rsA#?(Xgb4DRkaxVyW%3^s6K?^pJI?>Rqi z#24TBb2_49b=9hRR#j%@%B;++9!Y@7ZiE36%L<85)6+1+5_kN{eSxKCqyx|aZ1l}x zxwrtdl4jOMdiG}SMuq_Tw-SJ!nTds&1wbnX&;T&dv9bafndt#q09qLUJtGr>(6In`cwmjJ4c|cl|M>y_XT|?l<+qv8|79kMZnj1MT2*-? z$qop_zfB zjs5$acz6J`DsPjg2Qa>U-)1guZD{2BUhzgZ2D*P#8QTCDS>7Hl51^H|v2lF6$?#`M z;f<64#V2qQ|DkWJpiXqG!y+ZbV1VU|{sd z1$H_fE<;9oI(j;GCSyi+dLu>_JtKN121aA{w_nT##%%1&JX-IRcW|^f(zAkfO*hat z)ZH-A)z{T!gCSUq@)qkCp~FHTY4A}Hd&vS!#UTiN7WWm4Lg0&?eEbqT`hY8B|G{t2 z8gY;Sgdio+SKO#Fm@(K8GK}$~?;tE9xnf^u?>*3rW#JQCc;1c}KGyuFR&0^M9C0v^ zXF0?qxe(Ad!v4>G{`dL+OaK2#3jFW-`=6vf;QxBD{{KCi|7ZTcDXn)+@lW+Dt!HKA z0AP4it$*ke8CxT3egj7{8*2aq>$`G$(=Rekj+Spq`mGiHKU&G^nHWjEDKh}A5}$+7 zzwXn%*S|Mo{Ilbq3Q~gQZrO-7(lyf6)&2?i3GzXgg;Ffu=ev(EC{_It`E*SIIn)Ct7e(D!TZ8ggX}v{@z_PeVEvP246Mw5@PvW&Posh1 z&9HiBE(0suAJ;cCL(s)d*^I2rY=7hSsn%Bq>?Wl4 zW0euPC1$-=ydM(9)oYfskI_yP9DR#^MAOJ6Wb*WV3oRWy5pGDyhh1=ko5{*|`XR`0 zOx`ce9o9_Qc;&R(uZBYX5cGk=CyxdNWHBj7;kGeaz@KY6)g@SY@j&hNr`mR{F}w=n zmCNC*^89v9p!*7qO(w?i>}z%WZc^4TODsQjVbh%zCaE&H zX)0?|Q_Dl;8bHfVw6spCMn#43aAwz?Imjlyv9(3*!RCHa6fuY3Ri#$l1jeN&3ZiQaJf2gdNgLUQnIye%u~@m2-NgI>`OY=1`_&q;v$-BvY@0_n#bW` zB#<;p_FPQuO0g5h`-*+u8wyL*33X2Va0*d1qoi<+EnC0Ik1buR+rko$|C??iF*`!P z?%s~qqfpK&M^8tyEJZ_6UGTwws+z~lX?DX!QQdl6e&G`X>qLSNZgT{Ev>YkN#Fq`^clbvF*Tu-W)em{{ zx#_n$`81+hbH}w7kZoc@oVn#hF$HV2v|^;8Vj&wjlOYS#cZ?JvDEE;8R^j%iU&1)- zazCxgDq156P9rPbgm4t+cM$rC<(9FB2`5}+n1xRr=mA;LTEWfC>9Z!Z^dR|v-?VHI z8+{Tdycfq5a*Jlr5_s`yYH#bx?xRAg^y8oQm8UDA?GW|9cJ`IP9QMW{JH)7MzF)rW z1Jbn0G8uFRVIrxUsQXG*Qo2N8oCCivLf!8!!vPS6@7Zl0p~EPWgYFof_WO8gynY^P zOO_3l`vXZ5OZ3GeG3f+L{oPczlA+bzQlUhohOas#47v4SHs*ExlPjedKf9BC=orID za7m+pAO2C3JxL$x>qBs22-(ub*X*0|qxOU*_6s8?0EM|Qo_os#JvcWsgjnznjjR#u zCp5p|t{p^u73^gfB#WT&2;!jBUim4b;d(NM$OW8Bb((*}te6o*a1 zF^4AW75qU@3_YClV>`oav6V0 zl~rNyQ$2_U#J-@n+s1$kF*jWNw1;?*WNAURD1-Yz7!WCwwIXu25kP6Kx`v^Hhc?(8 zI^-mQ9z8!y7x6yiG!7qmpE&t

rm8VOQ2eFh7e+r7ITn#SpPPGE8EaR}J|%u~DEm zk+P9^!H=#O0|BvxZXR-2$YR0(KCB{{^*Q7H=sr_3Nxx)@O$@w}L+;HYTFR?&>b2PX; zr=No>B#*6PFJa5HroV9Ee56)_3|$!5L!IR-vmn2OqkotgfCNYTIJH2z4;@e`KrX9o z+PbgZ=Y?ZlD58s7O_$>?=qTHg`SP2oAaTgq$VxI@j~dzq_re*8hN^0ihSn9SOVMguph zx9d>V1c3n4>)oqJf&>n09_vltG@!`ys+b|_1a7>ivUmsRKM}}>!eJ-~NLH@gJJp^r zJppmDIZ0-5CgBMC48DibH#J>x)KztJQxf{+n^WJEhbz@6_>Wvwj_NufGL?GU!T@{q zC^!R%KPHR4;i~^f>^vgPTg+h-MusH0ISuLUqg1WO(X zsHVwmlMGZzI0K6Y@ZyeZRA>1UVZL@scNC)-KG0Pk%DA~0abgC0TTB@oHXKx`ldzJt z<(N*HCm2{#mY_++Kh)U*H_6!Q(8G}cbBf^KYre@hLle63PCY=6-ePi^Zht1vAt^NW z3Uw-3*`nyO4@l6GOjT1=tE|l&P)A2O!~<%qa?zYkO_It?7$j$04=iTKqlv!;TVtbf zT@{24kSZE{kdubzu;5tlCPk22pn#1lPY9x9D0Ekcuw#O zNoMQ#6aj(>5iMe`3R1WuL%IalI6w4@t$MQ<`C)l{tSziFP<96teiuLL%oZI`RBb=L z9lM|*CILs4;_oVKvA9h=Rj6$BK{{RfLm3I2dv5c6-e!n2!ktimU{2xZHpm0&dc=t@ z`}P1K8Jp9lE0eJ@J8c{hDOM5YOO-qUo`%!FBAfC!=rS?U*m3rc+@M+Kc@ATD1d0RD zeX!a%n-nvRK!e7qT9DaK-nqnr;cV6jm)Q*q0(>s&B{g70yW8xOgx&2Z*)?!amP3Az zi=r2;GM&@+zJ7+Av8mcn?xajs-AZx6%z~0KvCpk`C<1f$5~~9{dDoPgZAx zg1K@E=zgn;q*3WiGuW9M$z!k5>1B9B&n@BMoSU_=%`(B2Q8=Bpxfi3CVVRf@a;J3B8ehUq4XwbvDZFMH5Q^eU|Qnl-PP}OYOEEt;LW% zpKh4Ufyk?fYF(RAj<O5E2lavr2plGUY*WNq)D6)g{VixU%(2 zxwW505*0~Xms)JZTXaLcJuy}~Xf`4#TFE)U8c0+vp9q{_t?)86f3d<1d_;jv==y?7 zesY?{J{!QfP5wFp7dCy|bAvH@qYjo1P;B~90(9xG@c3wNibm2Bw*sYyfglZ`XA+=| zr|@(f=gV+D7}Xfn{h_unn`ErT0*PD#yu&AVBZWJYH|Kf_bVbd+4cs<*6&qTY3dJ(m zAO=}xw7TG;)sy|g^Kv-`U6I<^PX0L*_Zb$C(5RC(+iLvx@43sbA3NM+0`L%K^AEkO zY=HV+7>Vr@3#tX#l%{!|cM&p7E{2_LW!SM`c=Xdag`G{^j2qrK>=M>5ln>YCCv2R( zpH~QddteuhcNs(Fblf*6LE%Uh?=&ARGrD>gVmn3ie>N%$H8PagN98g#E_mlE`qI_j zs*ip2`ytU)E1@@w`mj)Gsfza8L~J5zSPI-vkbWI1e|CfdP?w-m=Hi}O^>RN9R?b)+ zDVyB%9kvKKoNBJvp{E@mzzd; zw(q5e@~eq7@~3I`7jY2j-wrr^-VTaCxW5nTKrEb!s+_vlT?m%1j>*w|8*>@@)?f!D zeFdI(MkH=zw&S)qeXRBRX>vg=%e{kMBs^fc26o!gg1eH>lnoLHJiz7{@r7b`_XdTgV;>{rIQ`AJNXQ}D{Sxe6G*jyIyd^+U({i5n(W7SU9h2?66vHLbPL;l-x)b)3J9D9TWU1ZVkq0ZA+r7tWJs5@tck@F3}B0|wq~ z51^h#RAHIMxHMSx(hux|tUYw*zck^eM4%S2+C+6V`3-vwF&m2i-LYrhWg`qo2_@`B zTFGCS2>$g;5}KvL5WV6t6qLYAfGARx1W!@v&+GFZu$;@UVd+Q%K`2M9-63(dH`D$o z>)p4~RErLwP$tb8`j1^HJ?XhPsibz`>A5*V6yPFKy8(eg80|CM4-FQ@D#C%S?9;ob zPRFBI;0CDu#Wkk>;(*i$G4eKGKsBj4SBvTPj>M`q(wBkpJRyQnxutWm*_3CoDj&r< zWN%Y(=ti^T=ng%@J1=P#>C>xcs>SBjnxvKw&=`o8GPH}=z_B0VcWf@j6USrfuqGk9 zG?97j3Azw==6Ndel4yms;vX_wq|I@;Dli13!b73)mRQq_Id7z$0XzbFy&>f==Rk z;NlG`+Y#X9*cNv0$7Qb`aaLY`d`;qY<{{;jRD6##iv4 zUJiNy0}B)TKT8a6duj$2x_=hmck%!9hS0Myv;7h1_?NrArm(+Q>=QlX`(FOPhNfqF zkEQ&%!+wLNXQlhsll}(H@P6a3p8SSp1kgJAH|%%+z<-PV7LfTDHY)@BdjRK;z{lUP z*%;~Q{*JxC4;n7|hW+j-`0ub;*!~IjHf~1te^29Y*i7uK|A9Rk0vgKkiJs{%F{XDH z6W>44=mBrhi+>j0pqZKfHHp6>GrYSt{zeeWY0yaMH{id7eBOci|A|HaM&7qT)V~SB z_Fs>gftleyNR7V&dI|G}{1?C6e!&5LKXQEb)oAqfCe zd+QbZ+@vSg4MEv@T;n!-oA-Om*T%7mr(=@7-^jHk*E5@^B&W`o^IRO_>UIxxQ#U7* zhZK`$U--FPteWV7evMM|i_JfRI4QMRcL#njV9dS>Q(h~x=0SM&o|ZhYvn$43c@~bx zbv>KcOX!b3r@NeyK z?9kr}&%(VMR!jr+OJw{?7s_vIvcrNE~_*>_M7+ho`(p|0Ckf*+!Ou zZyfE@nGbl!80GhAJv^Zzd0>(PuxBO* zGh;WcGxQWo!wCgfC^V7)~-lq4oGV2hl*mKLqDi%;iCSLBZ1vOueE=E!3t* z80Yxnpl>!*LXZwVx=x$_dvp318dKG^_`)r5>3}Bvthw6X% z3|1EUzxTlX3MH8ZEAuYUAFd@wVoHopYw=rE|j;@rPEL0&!A^thB0FupOGK}z4zxWa0+%4x9m;awV=e~7$E zP??<+BBhFleVyh1Ao6sqw950JB#OZd+71IG~PLKwPU|aW0{cEAJX`= z+7UMQa`GUZJg&5NAEIcUVQ#XR1mF&3oPHv|qs0{IVVl&>8nwo28H!uf`p z8jPbA3W1OQE{(&8LK5%N==Uy-xf?4&sDDT!ZBt2umd(2~!oEo(4f?w@;(RjdWrK>u z%8M{ek~0JU~&fRyW`#nCM zX%OnDR8*LlM^u-JR6d|jSsqCR~FLD)0 z=_c>#(w=eqH0Q<(+;CVO@_%We|G{-{bK}O6s&iWYB{fB;%Ym_7 zV>9Wq9!Br}2mU@@NnR$cZ=^G2@l(jc8YZitF_1QAmLU&)2kmCZYi=mCwnyYTGg8dW ztnSKCoY<%~<2|PEY0m3z!{3v67yE#EC*wP}{b{^6b%$wpN}9lWlca{i?aS59(C9UD zV)L4YmL<29b9j-Fj*+LFZ#;Mr7l&F^9mOT1wkt{n>va|V8WK&&!_}H@1!b>l3ZO}? zCmC}=b5SRP5@FOi)bI`@wZsHQb|e0MYx06+1~YczV8UA{IkK@30CY)1fGjZ9HUyR` zCfAC_Y#R03&O+XV(9VH85p)%xTi-@45Fk%LRp0v=0=s4b`NKX*ZuWgrx}L|NR@M&Dap@9{f8k5=2;Y>8(?fzS|X^EXQg0Lf4bNS+~NBNr%=ojT= zVX&8NsQYB%dJFR89+cef@RRmJCFc~hhVK2CAL*$XD=^SZy(-4s=ez#( z(F}Nh#bg3K_bxOm$8l0H zPN?y@zcJp}UdNqptc%w#42##3R~ml(Jc#}~!!XRGN4!3Hmq^3#fJkFXtbSo$y#C^Z zNaN(K>g4#X`t5o|yV%_kXSrDuZ%N+}Z<#|&ta0#l5Zy~+FwvY?<3wAcK0nD)+~(nw zh-x3ynVi_+L|$U=tLxoRYRz|^XX7MA$`k|T(ccOuYtWGEl?B#2MA@u(q6CqbQQ!^S zQVMKghoyzrXrvar_fZ{wzZ*r-JP&|7&j^7fPb81vhXoz>?*c~3{Flw%KjO|Z`sNCb zR{yg1m>A#olmAFHkOr_Z0%%3d>>V5d|7oMGYAo8Xu%LTpRdnVtlQTObmP$w^7-?v4 zd4_G1^VSlnABmS}9QGw%-#XBM)I(dFlcuB;lcOyX%{h!~*}}+AQII6prm_d%^%%-+ z%rkud`pi-Az0ao0k?hw<{7kuFB5;YLD_z8eR1F`;3yqTND8qP7woQ+CNg{~eKemyN zaFdyaE|rPQIa8e4ZhvCTH68YI^ck$<1!Tm>zW8V|8YYG6YzlDi#A5FmOh;NA1!*+U zjw@dpE(PLdc_`{nG_ATEBfG@Yf&HMWR`h`oB&Yd>Fm{`!#vfB)GF*tbw1B815TRVA z6J&|r7LFld@^l7h1!TmG@CQVu3}e#)#>yz5-}RD1msE|pCxbJkDSs-VkFbAkx*16z z){57ig8fOIOq$6L!lne^BirMq$7iB6gCpjvp$s9~QuB5aUP9aeRf$$n*4Y*n)K^LWUq$ zi`DOfx=l(ol{W-i@;ppU(Q@ePEf>ft9 zAFtVv=6$cBfkXz34zPX$!_8*;EJH06<#!tja#qxALAiBCe{7yX!h>o%w{l9v_x}E3 zxxy;V<;MP)nL4L)mX12-SczHb?=;ttNhpURN~x56bqQ0SOLjleutLu(`%ykFliE2v zxmay!u?My?yTr(OGHIy{e0AKy%k=W_dcF_(hA}-`p#G(en+vEuHjW9bWYfwVMc>z} zg5HLp_<@D~c_ruix4CC?s+Lt^NhQe%CVT{u05!0Nd(oq%7zE5@r9-P-V}#B&;9jlg z<1Nzgcumb{A4e&7n`%_n7w+E=df>;$ySt9`@bw5K)_UK)c|z+jyN;f!cs$%4&)2r` zaKV@K+tOuNr`Lfddlybl?HLCBkcQ_=(J}Xq^uzES<~R=QzD)-M&sS%0E_j*D_nxgO zVKqZrJJ{HD(8`2t}LsXx@U_ITNi72sT(~QHoG~f4O>*|Y%y5j z-AsgZQoJJ#6?j~(&mJIqX*Nlu!)_nV@FuE1%&c}7VuJ#UpL||!UN$F7RKWT0G;NCe z-0L-KDwcsy^8_`_5MTC$D; ztnl+i2=zq50kwH@By&TkYPl&z2VW@E+p5=*OwND?BQrk0`(45sef?y5<6J%buv3pn zJCQV4W_nf7s{;O9TRp>t(O?OhEo0H<5NzDT{(#X?M-)~bf{*cnG&1Xo*dt>)YrKJ| z@#}#_4j^5Qb@1`F3x|h?$+t1q z%V~fVJ60-1YTZop*N2b3m+K|7v6s7Gj#lkk<_=ZGw!ht)gu8Ft?!N20LTnu$eS9RK z-4DZJ=GGcZ`WPq*uKl|{V+_mQmT2G^+MMX|9vnKzG5k_AoHvkBu!Ih)KMUV12u6UX zUxywZevMDEe;uEqi;j;q5wxF|!K*qo^Ft$D>tF5jYbn z*p0HGkCTh!)jy8UxXrlauJ18+PuwXePY2SD8RTja9Oi@h+I_FmUxD=sDeyBh_xDLF zs~O+(I$jo_kky@_N2!*b{zAo-mAvL!c6j+E+!iIxIBXs?sDQrhF3uNAr}IMwnH>$V zEt71IYb8d_$fr)$UZ6$O{zBBv9An%r)6+1_Ds|q!M3xxwgI9XDYyX5N*FdbRvov3< zYkhF6r>IZ%)=`ZkCVpmy9gz(ihPtd9n}&|S-KB#9;Tk$w$o`(DR61@^zt7tw`+l zqJ+HcFC0l0%&4nsmORsijAPF+`Go3SVKpLC=L4bOOb3Sp$t~lO)&lm1neI5C_P6Fe zBdkf!jQn1X^B6fw21II`Qnp3=o@R0pqNTiwfn)EVU$0n5{mvl)`CwY+wZSGaI}ugJ zT*x83SQ^I%6wWbqY5cm(+#1#eg@y5gdR5O{L5XI!g##b8wX((vlTMlZbmWH668BwG zLVt;Ihp&^KZX7Q5Yz^_RHd1Rf~zq(pzfWc%sL>9(RZXjzNdR)agzZstwP_Do_VJsDAF$NfwX8H2jgym5e-wmcy_U zJ&IL2y3at;@0-uXmTp%iM1-Gq13OV)!OcM!M@3a)Cq@KphcuwvLLJl!cwlmQb2vFi zKpGeY0S9@=smFpJK^kHj_Q`^+{39l2**@QQIE5PmYBA9gXQXis1fLr=b#7`_)(T8N z^E`9J?NJ!;y7y^PTp|5D)nbG60QJ)K>70O$Y?aa%!FukV!q#?gIR3e3#;y`ylD87X zvqOnqV-b(d->&QX3rj#+n;JlznI3-4ubw*XZv}%yltVh{?Zc%c3uKv+ug_<#RV>L) zi*3wl*vwT9R55Fs+{^|+bf4{UMYEH$kx6yrajXReKWxh@>E9t1hdBJ?xYJXvl44#P znmXgUQ9yV@cQc~vYvj*D#@gMx))**qwVm1flWNC$4$}U?PwEz3;(O;x1W{$^f{&qT#CqN?;yak| zzY!Tf3EO;q&Y=}{4OkhOiW7eRRN_atg8F+>xd$2U(*Ke^@_VC|W2a^Ja)It*Vy5?j zAarQInX&$OFSLwTE`4e@t*{L>*?6wgU?5K-W;U5L5BR8gng=jMm3CP7R(Y;zbk*sn z0S?E-x9Jj5dKZ9{{ZeJ_r$o+YnvMayi-Biy;F!+E{PQQ^@cQMiXF+fq2~Vy(7oS^5MA%Fj+LBApp2j9O z7rV$zFtee8W)GkY0{&=3ZGSn=Yji$So9L_Mrrlnb$8}=U$X&b=UPdzey_2YVu+_xu6d#!lP9_VOod}}pV8!={YHRm2X>K$IO-QzVQIaGfeb(s zUq*w;7$P$8Ir8v4^$fTAC$=y1fT+(+Fq&ih4Ac4KOvimio3=b}Fmq;)h$bmF)2uX8 z%UO}zPn(09Qhd+hd>i-4#aS29yHB2q6D78#g6kzV3CGx!29$UEOV&rJOkey~2O?2~ zzkA|d=Gr-}Kvdw)9~o)F(MdOpg=5qbQ>ce0S@lB!c^OURX?7#8&+n?jFD+wWe46p+ zyd{I%<5}Y4g$QS@hNffo=8{ywW44vQGVH$CuU71(9ctT8Vz?52R~O4lwO_2)T)&>X zMtBV6kMktP(OlhDU$Z&7K7EuS*C?pKOWV}Xs*rwR;L*h8vKJ{RsflxQ`f^Z2!%|aH zvpe4hjLg;&-_cj8Z5v9ntg!w}mkk`m@g%^g$k19K1PKk{&DkZz~V%lw2+F)NQ_GXF8O7~05 zL$^$`#fDgFrp;?wx310GA_J0Y`kY5;8yw#9Enlbuj8n1N#_p;$e;g{98y);;ubaV| zEwEjeiGkd@08VGUdSokSh_#Imkh}(nC64&&l$m(6rIcGIUCAn}B$8Y2o6oI6-~L_) zHG=IxT3(~6w+WH@RxK3;6Xn4r1NA!keMjhQ;bFCQPj|9su(NR4=gcVT-gf-t=*+%6q3Ci(D}%7! z>C6IkGInUGAQg$^nfQ@7U-+|kN=UNv*97lATo;wFTjRP`MS&lF7a?m>t9Et1G1m0y zITX{S>QeKC7)+D=V*07fv#jw+P*k3yi$K3{+++gQFEQ2q5|S2!JVAO1Fmen@Wy`|F z1;OKF6XnHd6vPzMr1Vv1v(EL8^+o2o1!_q1S&Iw8H@OxRK>=i4 zwHMrV#*?$83HyxRH2s+SDKHd<;YPBQ;jE;I4T2>p!KZ!2Go-Yp=H9UMP zr021$tOK!Q{R^3iD5|5o7?G<=C2-+qoj4Op1a3$fQ!l@JSsi6|veH+quSCNArBcuj zeqH3Xa?C#o22@qd4?h#e9u|NafG(C=kHHv-zfK~b=Y)5&oY6<%8Pe8zbDRN7OBD3C z&QYfQU_T8NY!K7Opw=CWNvH7awvrPmYn8we%7JP7gD(+cQlnqlq zDSha%gqS^*IanUvIyKY^CnibrncnhfQoV+fvGKxKN-0va!OWUf`EdDntMA2}I3LSr zyLA=mJ;l3tgX|mVHGZQVTre}>*3Ou5=>E>e2k=~f*QRs^FI;F7jA5`$@K+RPNLW$*Zd!OGEmNbk3IrHg z(*~589m%vPR2LuFv?>sk%9l%GXJweFA1lo#EjbV)#hS-cv>3o+IxaG5hBivf>lBvQ zku(!-RzZ!%ul-J%=l~T)3bySDi;gdWbi>fP>2v>*p1W^f~h+bQ0}%nXu2~A8Y+;fz;&C(_|j4pa$+oI zxD-u$F|X?l+rS&Deys7yO7uaZC<1I`el<9wcu*~#9HMOlp+o)%cioF=NajGb3F`% z?1ze!Ibh`)cu1jbBcv?2PLcJI zrGujQz=Rb$ir6s@no!E5-$+L1VLK?1E@@kwm@fAN5%y*S6r~$3zZ8|B3#FOPOm@-r zfVgs+hAcv9c!O7$Kry=>9jL5;R&E`Uq8W<`?70S#>%GMPM=}7IJt{&SrGH zmKe3+#TY!JNpM8QX9zP4V;VV8BSHvl-2*|zXUQ=!+0O)oU}*eaBFJU|5r##nUE+?I zL#7e=*Ro78_)7g^8{N6dOpt|JDi(er^x*&sPPj>-QawjLRJ42(6qI@paO)8YOIi~4 zqF7J=7z)>5wkn3;Ibg3W&l6Zn-_TO3&=dx(c5eD`S|A&H8$3 z60OF0MWC+mSSxc_me)&Pa1Fmn)nw_pjrE94f<1Go@>qxie8Dlnk=!t`mua}l*yeNq z+nv(qT4FOMe|J8$rG?F1)Kp#&D=;d=WiWAWU&ya^;)QtqyFkZ>W*jQQ%O3k*c+Ql# zr~`&l##NvUUaiJvmd*Xf8Zuvv8bWs69^hFgY zj0!{w&H|jkd?{<<_aOR4)tw&c2AFsmVwz$I?_rg#I8PQm4dB|BKF|@}qu&J0F6^y7 z7b}3X87OJdHvpCDA|vU-O}p*$H-|}H$~Ku26w$1dsw|JC=$ZVM>s20S3yZ&TS!W*q z;2Ga=XSW_3`oXlAr=V<*b>V+~%7T^3n-}x#9CxqWN&`-%OxntGiSdAt4PIv8l1^QT zI%?NzVS1HOTp&{#`56Ec@{LoMx&C~q9{DhOg>-7E<_3t0UFa4s)c2OfP*$eW43$44 zbmOM*LpL?L`2J8YXNl!_x(EMPeSqp+4> zk(&F*yECA9V~-F#FrU$5ngF&CZ#|{&m+>XK-(=|hy7`nh$c+7`^|;aC0wks7@MOiL zj@QbE`+Z$>Ud;+1YuK;47xT9ERW1?7g|*0l09z=0*KlQ~1*bI6zGs(ji+ z&805=NaeV66JNA+)wvs#L)=*1k`!WO>*JVuZbl-ak;L2n+%5tVO0l=?g4RkoE83#) z#J8ZSbSMbvpJvNJ2NAt@d4Me2Q}AU;#*sU@bIN)=%(#ANjR$uJmlh9JvZKDVc?>>= z=Nh0H{hg*YRwxNH%xm6y)WFmAgxXManei1;Q;u;*#tw4a>~WH|{T=#QQ(AiE@9Z+^ zE0C-bmW8jlzO&1b+V)vij!kJezTxyYu?~YQzZ-b`8a})PF{-sm9}-9D5B`elX*;;p zq-Ezn`Caowpj^@EKA5Ap2mhVr3bRz3;Wid1_*|}Wf_sN)(4bock@mD3{Jccqc}@}q zZ{osQxbtPmT6qwE;(%2weeTGb^T)AKx8wFHywC2fab-!%W}XYrvmLOe>5KzflN+`G z%15?Z&hgh4pSb%MtWs@ODA(MrDDJ_ajx*YUubtw-?kk5vSo?K=XWw6?4J*Q)0on*a zQc8OUG90_h^n=_^_G2_!fok{Bk0@f&#lI%7U#8{0Gs+UP9SS_6`uMdf3KF&%1od{a z#}1i%C|khMcFyMNJ|yxWC?4GnO4}?g(ze-0g)#t!xbNhP0E^>~URsKtqK2?E5Y}Yy ze7H=oCA>u^Cx(xEkbD;m7_(uI2A+3Ywz?dI4X#Y_i8gC-B2 zl%HgQcu`^4VbXws-iY6F+czN~@oS@*hY0bwdpgYFrb0T#JiMo8INgEL4!zIGY~A$6 zDPd@L!EN?RS%Z}yhq?@m#$B>%=Al!BJaMm}z}7V8Q%^8{#ky{DSK=G7QuYK{`COipDq5|qKIl)!+rG)g6Qw_fZk6QH=hi!IX{S)y z+}vfOdpV-Pwdrr?Br@7?ehQc6ArVV-M(SZkVi+=xxA`OOA}$3)@)HU}_uJ2)PT266 z@X$fzPGf;sN#j5mNCaQ9#ANY=ZN*qcyTCXI@g@+x=;RRK_EQL^6imVq)mFT1jO6$F z;*#ArD3#?aUUm|787z zP9y`3Jz@^vvgG$|iSVJ&gJG?q`XLwsY64?>AoWRHDrP5sWCI^^GPG){Ny|QSnX*irJ|PV z6#1r~NIj^yA3^@mJbE#>j{kl}hVTXVdB)uIvG+X|EG|YgQ=7tb=R)2A6*O#m{s0Ac z7@rmc`)c0#4umT|8&b+*+s-Tp5zY7USGoK;n6Dp6hHidQQ%?v>Y@zNXAPpi|@)X6j z;DAh10Rs$)xW7l!hfdY(uN|e>+zJ)-vOcmh6@dE2LT^gfex!o9Bh`fP7sj*{yy(li zjAE;&kT4ETV|2N^Awa#u=OTE%CgJ&ni2C?bdO|_L9p4f1h7Gd^&Ycu)GkQ&lj-cS8 z%Rn|w;+n%YIaIc?qe{8b8w+egK5|2l`U2mT9cXAnkEs!wCTMFn#F?S!Ta|nViti)t zef^LwX#(tZ)Gd(U^|e2ZN{;jv1maAmyji%?kR11N!2wjHgel=B>hQdJ?2O)XZIfnn zvf>nuoso`;gT}%Xo%zevsW4f+aB*oMzFTAf=v0CaIwv_k>lx%@YpWTP{mYx+b!<}8 zz!VRhD1oT1K20;6DCzC#ml>A5%^rQ0)B?#7;pvVsSJkX8yeNYWP}RE>>~XD2l9zjP zPO%49oU7jF7^{6(k5u;|naC4dU9K2e=z9e%xsbS?WF^Z`C6&2^5SisxiySZUZ5 z@hu7A^(8EGMWL>x47{gT#@sRlE(~r>mQn7Hp1(cuT9R|%s6uvs`L=Z05g;gtag~$x}E1uMN;I(>BJRRJ3aQ~tyw|W zg^lg&5iQ>+=CvFJ02XsWSC>~(ftOB;<*B)pyr>q8dh171F5|h2DHppWn8CdXHw{lx zDrdGTvHf9p&E=e$0RR5eASC5y1IiW;C`)rrFsf`HUpG3Svr!erT7s3HHuLB4lCc+4 z^Pf@=_^j|qP<7PXen@@~KWpPeAT&Xj#;mV^$25bd>?p`5Y@s!dm(})hz!v+u^_@|6 z131q}u?IfAgH~0%bDIkin<1Pr(BF|_=zwESg`CRE)2%sZ34O9*WU2z6*`96huq&nZ za={n@?+9^>q3{|%;_Lb{6{EFVBF7Ts%u;>R;yxucrGg&NireqMrz9Oji=gV-S9Y*( z%I%s}wa+yio8&((OrGEs1DD`k+sIVyqkZh6(os^sqX;(+4u6uaE?;;w?z!RDR-Qff zg@{M-AFt{%*xtK#uUkhiqSGAgi+VCvX*g>^3KWONgEbvKmJfolo$Ud$-8u#f|K3V= z5|zl>u4=`1WGwj+CGhA=a{i|C%;Zj+S~IhK%RNQK$V7k^nj%*CFffxS++wv4Z^_+Z z8X-}M&yS!bqsseSHea&2=$k_BDL)nC(I*6Bmmo!b@dc2C%)?!s3IQtu@(^Si1O98I z8D87`T<$cPJE#U(*fSh%vSi2uE~io+&Zt9uH(X-#pSYf^shsI!oR*;E% zdOklvh;16WX9ZfbVBoV9id)&MCmJ%*2Q$vI+Fi{(#KkUWLy|IO{6*~&_l|(&23|du zsrfpN>7&op+^d1V!{1vfsRdtB&QBle^r} zRgL@h>)^zB2Q8o`pX=_j70ZB@S{CO8niz>oeW$&Sg7WCGhrpd(J^j1Rf@MQ7olcCR z9BxlMPjNAp6`G4E`7oU;RTCG;4Ylm+YWucP@mjhmvgH>h$SnIEOY2bM=*^tnDk|dG zQCK9qlUlDC@6(JhPCZi5hD8{*IUk-St^Km!ANWIZ4)e>V8z#Kac!*6#Iu{|%Wov?-)Jo~ zvkxg38ae0=n{n}d8-_=70d0dQ4^hC5Uxa9=aTCIBa~DY5>k?6?d#~lS+zbO608#kE z)m|Ro4e}w)1(YE4yE@mg%Gw#rZgDN@x2pO;yqeP!c~7gF@8v z^M>*+GQ?GR7MPqs%%gUaM0NcoJatO7q)!_^rLuhQNO*Z9oGg!>bj21I9Gc)*U z-?LwzeNXp&(eZv%WYvnQjL3?rTysqLhCw?56UEcmAXj|wi5XqWx{gQW!1zUMv3GPa z_e(sVH_UaP1y4vZO+l=E>W??wL$FjZ)rat{uMNosQ8gl{EixQSIpKpPm$c`NbewcX zWDIuG#Nf9^!stn;Utal*%K*!#6~I;ZUUit?pQPeKIw?Rpr19W@UYpO;0RnZ|dSNZl z$*p5hn5xGc_d4|1Dth)&!^|sD-$c2SyX3-{Nr0{0*(EoPT=^heL>FD~e0oHTWmy`G zPsyO4Biw>O11}8~;g2)=K{=q#M4`e9@la5+^eJ#wI98O>wVdP)11NzPwyr#IuJOW? zdhUKeC|>xY2BUXfXU^ZU`&kKDEU^8Ur6}ZN2C~OmW;*BSE2tAo74M$kkb=Z znN}?>WXmG#QoL)Rz5^oW04T{wv&43nQGvD!?1R{J5}2~`v9zpdp^l{vl^4iQfp;gQ zKc}UoWY0)zeH|yK>OjEOkMF*Vt7Q#s0`5;UuJXdPAnA0nAs|L5w_VCB951SkT$p0u z>nAmpUkkpwuVSq^x1WEb1c7JcN3!|K32Jp=vsGF7{w`~eoX2i9nW%`BhW3&E57?_5 z4{+9$Jr)(U&ta2f#7?_Lb8J0Sy{cgQMP;arann}9mFBT`tN`R zY}%2M92ND|V*_BOde{SM?|X#)-d^>J$RXr4)65-^&bP}=Q^gt>UU_rBbi;ZopWrlv zOCX}``!F3MW@8qKyzhlD)^#N@P8xnQdG)GL8G+dB710VroP>yWQQ)R~6hWdc(dO-6 zl(xX%6LN2k3sol__EG%-5Bfm|S$AQjyh7@r3j??0q7SH=M6|GmM*V!D8;yT(SoIm` z;kaa{8nzEP%$(CKUN!s~9##<~L&d|E$G6>2-3s@4DpCZ*$V+*5@QWJ^_gc6dh z+WS^PCK-@^A{QY%8OPzgx2$loiqul~5$VU$)i9D|EpNX4HH>{htoGf#VG2^5{@qRn z`Np!yG!A&ayX55+79spKAeG&qUq8tqmiWZMfrBW8Tj&lpg&rmO1#%FLw@XJ%$!_Ysx}CPES@F6K8pWR9?X1hLA$fS` zJaija?Xlb0^SZxFOWbpS%A!Q)q}`u#?R z>_zt}fg?*w@b0Hxz;OcMdTpvM2%_@0c_Nyf4hI;DSMd zjo+tpE-XV8^_eR>6wy9k(tZLIROEhIV$s8dPtsBWFuNo$WUOsqyQ7u}MD`1=0nqz| zsS?&R@9dBEc+a~OyUO&LAeb9D&y++vd$&WTzJ`#8Ni{6J58!#Q&{r*(!Rw-cY#tJ& za+9{ZKqZl=iht@Kx2Hb{#jzwGTX6pM)2B1B*r#Z^r!tZ`&0t8g+i5CfE)+Q6X}kYA z;twcdG+caA%;jIWx&Jln@el3H`nM)#{aX{W{_mRjUn~EkCjS4F#H@dV{yroz;Rkc+ zKae{AG2?>&zj_`&z={7|6OYBA5aIxJlGImiPM<&5iLRu_NCf4lb9eeMVlhUO47;3K z>d8cd*<-KnwSv6%F2&gaD%Qd<+fuUQj8tIc{ECqz!?(NEEKkN#sJtH>ovT~ZsnL_R zF?qAu$)k}pR&C6i{oTsiEJHLF76}QtGYU6Way(1YH-w3ilJ?)E|1)n}Kf zM|16N<;|m(ii!xaa{;sWYMjuXmHvWs_cb#> zE88KL6*5|Kwin;RT9V~VaVSTbLa>e?ArHK$cWuTcd-W+TA1=ZcQ@N%0p1I+3+LXXw zcP4?&p_pEdoKw^FHTVs`k7xQ`d{-VrG^h6k%{qG9Ogg6PP9p{f%P9mnnbIP$fB1Q1 zo~h&Kh>@BC+^#o_Kn#XTmhx-B=l;QB_=+ao=f0b;LCq0@aIx1RF=@D2Okva*CD6)V z{=@PM)iX5~_b>X;itA(sq6YxqU6jBbts4rr_h&zzdlpSnV#NBVsy>g^0!S+vceDVX z_9b!&DRKKfCpl*>6INrwWU@RJ>s0gkh2+hd*BKAZ(TMp@hHV1Q#4!!ppK*f3ILLgL zIMdlLsxq|$z}RcqYMePSJDhN+7TKo<&&fT{xL?FHK#UTy8%-X|pxyon7m`1F5zWeL z&G>SO&&GP0aw!y6SQVLf5#nt|rVdOcnhLI~G~ocB_tWuOL}Q_Op*;G{_W<^-0KM$N zxS3jN;!KTLH(@IO00xnK6C7A6s++kuf%l#`$k+QD2y#fM=czj#P+|8x z%-79qKo{c%)<{d1LXeFdoSm1K#VU0ndVpiV==n9!SHVU%-Ol4vqOmtKe&~zBy^Rz_ zzNep0X{obbBiCk0poMRwCO&J#%!3SDZ=sz$PSvHM@@!S2C#E3Iw(oiaM%NGiTwfSp z917YDiIo`9ZtjNfevxu1Pla(QG5%++Y+#H%fU_Orm~1moCCO^~XgrpcM;zDsW^scp z{KrksNyAA9IYN{m)zG*6p2^HiU_y)BZ$^fRj#*74esO}Uf4+XK%XP2;q}1NplUrd< zk~&W!nxB!+`|duAR=H4ZcHJ?Pk}8!J9z~1*D)asNc(8UR>EdCO4g{_^pcXC^1pK=I z(C+iFGZUZqIiAl1UfWh_fD|=>{nsI)#nL3W78P?M0ZYo15P;wZ3n*qo_=h2*CM4Pu<)b%&1H!4m79HpX$G z!~V~FH{yY@TSM#s5Gvf}yx1IiE^R4WU`DEds(AOFG(J6YXmwGdGcNY$*WDt{8sAkJ z?vE`UvJ$`Mnnv~XcNxVjw>A{exQbLAfUK&-O6GSW^j_$oaap&HS;{-9q!1(274>O> z#9(SpofRo5@P7L(#veC2%=0`elPI+1`2Olj;j*Kb{VNMHZ<$LjLD?H%>MwBxbY-5i z9qAYQ6VZA6%Bx36`Eo{sCQQPi52MQs@(3G33%S=wr!P`Qr*@(u6QD8@xrjZ>xV~aY zG&X^ObDq;wuqrN?TZclBwzCO(U7X!m(^-kj2a~^>*iJ-?Ygt*f`z(kEf8;#NqDRbo z8nN)z-rLI?0}VWvFeF6-9D`m9u-l4X(JW%PMT@6I37#i{|M6)&1q0d{1SWy9R9~){ zuLO*Y1mncXTH2E@Q9;PduwMMLSa=OAxW&Y0+=OpSPpt8sySy|Dn79qw-sS0k!eE); zv(o+fab|4|&N$r`LEtZcbnmhuIYYu?&D-2`7Nv5w7u&_~pU@53KGg910p$7ZB2L;S z%+4(vO+!}_94c?D>MJ6sR0ItuRaWXsK!U3k{wSoD05IZO_eVx4j>a^NFEG~`7X~J? zen@wZD}9^^!Vq{Ug4vN*o@`%mf5r#%ow{a~4ZvXV^3nAPMq9+K+PJc=h6S<6UYc+j z3x6-tPNX=lQx4~-PUc^eBARm7mMt#nx)=2@x44Hal5^p@8 zJomj3&c4sRI`9K_AO%=idhU<4F^=EZwzR>Hk<`DzD2e!FviON`{_wefeRNqo2v&?_%)=Kk@`CUP&kd_*9!KnI=jM?EdS}&hJ!@7iuMhjw@gs%h&#yDl_z~&7PZy6 z6_RiW?n3JDfm@7|Urb`hd3-OXS(!~cdi3$9o)LkCLo)w>o8vRYPFzYC9<1Cp2e?Vg zy@i$gcVV0dbFg#a{vqP^JsWOH&WvwDd75T7f1dYwm}-A<0a8$*A<4N)_q_Y zYAS36+=Q5FFl?(Kc#w6%3$B0}*ZWM+N`aqvgU+*fm4O9hl!sZ!V(%~J5tzQ^;=_AU zKP%^qg$1$qt3Ul+Qh3M%m(CeB? zVFv3`OPw4*ad)x$W(mV|slp_G5d|!|OGROcP^RT!m)ZW3XcjwH4kv(IAXXk(aaVU> z*)#HtrvrRXM<~F7mZ3OVY7rDT;Y;dkepT=wD+XgGnr1Z};QFA^N1 z1~vr^s*0+Nd{7u=NnqjOocg@9hqrV6%*?7}k1R5+ENT)31lasQG-v%oJ`V&ozVi3`gR(b4OSV=UDpr+0a0QaRnkR~(jUx?GwqdxuHW_=Z1Tng zxW-)a4%opcVO3aR(*09#IQh1DfHRZv()tQJ)j*u2qqUbtDjEXGa#fr|67L9$Ue_!C zea5cq!*55KaI5d1v>KbMSIH~LAN)I_RVd~ToX*!<@S@Pa)#m|1oyH=}XNUqS9EQB?ZJ4=XH6m4R+gM?2b<7-Tw0 zEXBMY(B0RZ8BayV2g&{MM zkKz1(Fg>_G2KWEX$6)#3#{T{4pL`4!t`AbmKZ}HoCnKOsQ1Jib1V0Dof9I_l~n(KiNF6EZTo=m{-Yd<`EdzY0|d-O3QA&qL*=wgr1?#tWq&@oEM9DVc0Tm}j>w7AW}?i{g?a&2ZS-edh!Icr@Ct zn4&%`EJ!%;YYVsi(PqFr_}Qt4HC!N(Wha!Ko-Gzt&YiP^|7lM>Rrd07FcNJ?<{oG! zZ69w!3#5yF-qdK%yCRb5nyJR;`)e9|AanX}-ABin?zn6LS&o3F7UhW2UYKwz$RrABWx>t~Z9 z&z}wr44+WKb35tC zfO~2H3KuwcniSCOmKjRnbk!vj)7DP`j!i~Mv&0dy>u);l7uRr#AOM~8$ z{c5Qxd33*Wun|Zf!`?kO@=RsM&x^F!*BX&E8$q67#W|@UU^3$v&?nOQ!Qku*JOv+-jDxxaN z{%6e8XCV3O1p$-QTa22a2(91dCi*{A5AjB?1AAXxSt+I}Hv3~&@*3_`)2 z&eS$xh^j)pxzfS4-IOazuI-P&zPA$F|IUlA5UN$1Q>6l!7z?F`too?9Bltu%*)&p5;vZ<_%%?$NucCVhiV>FwXIbCtXq- zEkso~go2D1L1Y8_FnKq`eCE=<$NN`J$w`vUF;(G&XwSbdE0mJpXAg#&TdW`GcGNOt zl19?5a#O_&Q)S$~rrZuMmPHbTUwUb+#fuRZ6qru2rT|)GN90Gj&CQpyvi@Ak&JJwE z3v(ff1zHf{18tlbD+B&A*}c%Bb{bVs4=YP!6!}FcPkTUF$9=G9HSnYV?!Zcqe~_FO z%r2Te7k(+4v9Lc^5%y-peU~in3$wb9-z@1E3t@S{Q^H`?6yCNQ9je5>dhNswbL6Rs z-YVu?-7k!%(|K*~*&m3Z7#;bgGGyFAtMD3xZ$*%|)Xo*5n?S(UO zMfv!@_9tp3KO+VeygF*{7+-Zfi7SpYSMK${p1#l-qRIKmmDQ%|>y96gma>ykuRS?9 z`B)5l&QZ4D$ofiU^CM-4kCQHbnx)wA$@9_)+RykCbo!5~WdJqmQgO-QnKWpXTS{OT_k=WO<_!fb z$>mFLh0hR6tyG1EAllWT!CKl-%|@0{MZ zV3@x98-?9m9xP6WY&Y_qI6#v4#;CQz8(#+SfUv?3`a?e9H&jb?ii9ns^0RkgCp&~R zfPs!eeLq+gS-m_hr9kntrCH{r~pW(oi1BN{y|`i-UQB~8#Ozlah}xgFz8t8QDw zB1pm5#Wnqw49ER&9_3Ryy*C+)z68Rt!pca!#BI%EDBJPiqs=O%3g4)a(+8bsC5QPE zdPH>CD45I6)ZUC@AXU$~-kMO|VfO@J_xOq_%N!T_RssGPJDngg%(XhR<14(+(^+_a zF=jSO&H;|tq%&GH`)~(Tw;7($B2qGorS=cn>t*m%@ zH)piDF){LG_b#ztAvr6Ur@CN+F<8p6WH=l|YGcHq^MIQp#KOl<&S9gZiC=qwPG^;q z3n9uVyDbfYU{2deRX4f_H#ojgZMx2~{^hnN=FUug{~bSgc2=;Oj^b82jI5erSau=&W(w7;x#%Zj%E)c(snJyT#HuJKmSTmn%oXY3%=w6e}A zx(yDiE5g|@-&HP}8kV;8kyktcm_HE&`J-HCewop^*5wyhSt?(H*&V=yAe}|Mbe%Wa zO*M9)4HhbzLv6JKTo2*U2!t$n&DW_nylC0qPR2_{Mgpnnmkrk2=G0HQCKlE#+pa_) zy}s@gHJ{EE>ecxth*Ex!*}WLzQ+e%?JvH^?WV`S`5pyUAAApd&cFB`10k>8uIrw1%SE3T@q*p$4iKw6 z?~Y#HdYpIGnPQo=-YyHT`09NWz1IlyN=rSgC!hsJH`*?|T$?>x6gLX1+kuDLmI~XG zm7FXA=&Q3y{w!WV@l9XlxzNWg&VbPQZ4+QXTFZTFZ@e)A&a)1DG%SXQpPxPt@hcPw zmOj+w_GJ0+-2Dakz%F+tX%JEW0gdoIJkdykfeDNFB`2*fVY%1X#AxpBj9&eAgGiW; zrUUpq_HNJ1_jAglJW-$r7@Og1kDi#Ay0$%jokf;Eer|8Jzj^oKIUp99 zi0*A%>n`1cj_W8P()+2`?_{&6&Fk{+v|8`8j402q^{rj;q+m(Z=WuTUHMg2Ga0HXtJ+c&mn^cD!vz_cmFDs8}2h=zY?`Hb9?*=1$NAMLkzJxcS zdP!}6fj*=+Q+vMx@^SVIZoNcOJ)bSvzpx0MBZ;Mvfd5A z5mV~EK2i(LQh#Z(9@4#G_SgpnByQxyFS2a^fCwCaU}*XG3y9qZ3FO_S-wj?y6aROQ zS>)8lex#(ayN_E{EM&Q~W-_K@HKnmz1E#Tw$+v;otbHzkb{%$wIe5bVtx%)U;1BIB zXSCR1dDGCfPGDV5rc+b<^<{_Ht zyOFqS`tCf#=oeXWd{Y~enL}fenfWn%EO~K!TQicGmpc^lqlXmo&y$q$$4u@$g>igW zXGyWt^9%r`{09UC{aNWcFiYt=UuzhhEisn*h*Ex9dmR*Jgep)|Y8baRK9=p8meTxq zcq1?`^Yu&o>{rL&g8h1>T?Mu*sz*ckSkJ&Gy|p1ZQC{SGi*&k5XhF}+0(;KMDG_}B zhG(D4%EB23zT9vhr!rkRWt}ySDdGUc?U}#+^l_$0$iP2mQvO$$>j&%cFKzbWANF5| zf7R7E{?+;V;Xd(Si~o{u9|+F>RB?ZIzW(3IIPSkb{SRA=4@LT4EC0}O|CVw8b>{!$ z4TO#LKah@l+S=do8!>$rDi^tw*+~6gDB0ZY3AiMw6JjOmS0q_eF`L7=e>Jf;*D>1s z@kI7*2a%?i6m$H>#YEQB2gdzyK|tW`>~s5+6+eW47CUqLN{5bfCBx!RF|+i<{^IWB zsaqDl1F+0^ImM}$F6Ct&CSd zdd7h8hXs~Qj!$?@H-G^5K7CR@#~1+&}`I|i5;>?#Q=gRoCT% z)rQ3QwZnd+1+B@5*7;X1X%MxTMuA^P$AzXEmt=i~g^30)LLfRZV+Gegcpv79=Qch9 z6i+|44x%@>o>;r>q+%4xy_C(sDgqnPr?JSKzLC-{eKX;~{Ea;mzldm*n;;ra@*~;W zhY7$|P05Aa4P23YkkrCSB4jX@yR=X(OX|;0ZD-nl7z}jyla4^367aid{;osTf(R7U zaBB3iQ?WH??OAQyxIC@go4vFmqu$)O0#j+UO9=v(^teiW)Tgz=EMnv5pb9SW^Epae z@z5c`x|94yK>Tl2Dfc45o+6v>5IK>@fO_EFKvm~gUl-&Efg4I{qtRO^?w=tgl-a5>VL(66qMK@=KS9E$YebS)?8XtG6AYX>$$%-^An5^H zq(U&zveBTV5Rbiwha-JvqQaXsTDf>RD7+_!cTZET$9 znj44F#7$g1ij=h@&OuCs+M9fRBy|EH)eu&NMK$H=(6A6(hP!tM4WwG2NoDf|2Q);? z`}$~jO)iraOse!{=EScm0sdFs=FEcW)`5K|TD$$;MNZh5Pi!#jpk|Y1@|Ul)yfdMP z869lx+MEL@HYvZt!6VO~X1N`|i?9tjUe?k~1L~a~k-irMhgeiKg%ZvNv`7G-A*yiI zMSUjmtqNY~@cFkQznE5qF0KK#@U!0;g(i#5xWv+k*<6TMe&{G{5y}!XeN*wj44D1Z z4n@?+dQdnmD1VoLSUzS+u_11gJaR2~QA626kW2Br zJn6zt8!iL0wVTHncgpqY*!2LW9(79z=zek5TCTg z59m34wJuXJ8^yAXLd^YP%p=9zA?(*H?tvMkrfOxn-Cw?u}fJ~F}U6|d@1mnjNZ{V!kjPMMP#4062A0loN{=PL^R$8OUt zKnwfMD6M`0)oq@!X58*LMjCxJiZh@}-Wnor0NnItttH~|%&S=^VPuZJGJGGiC*sNJ zBTQw%&%hKL3Gfv;7zChPn$s6x!!2&&_4Z>sCrDAP4icnCnt0l2D^rJMW9Ljoxb~db z9!{fBd2>Lr1&6>;c!w&Y7BTwbo|c!!X@&_^J+NObWO_P?`yOJRzFO-C8M za#ZG^!`dqVfiyW;5|F1M{hC_e@C44>s-gNYXe>)ZsK@U!|SSe2&!jum#)rIX%X=zI@NW z+b;>`a(G0NpJAaeZtqjHz{envAG$R~w0cYh6$b-Xx!PRQw=(V+194d7&s16@+sAHL zCuxPsR_HO&)3QLg(pd>#Z@eu(lGI!tjrXE#v7FNLr4UU*1;HlmToec|cX?v`z$RV6 zmQ5-I0)HTavciH-;+}`ux6JpImZlV0_!CP~kVSJ4l;F_7S7?z`!myicn5YNw$7G-V zV@jdGsNv3#1*pKHXiiDe`B2C}m`taT7OhGlo34xE6Cps|8Wl78-b$g!Ky9NrpJo#) z?>fUuA^~rW+78+eq7;lCDOyCGmetuAGfoiI%o-ZDczQWb669!S*%HPb1=eJdP=i(h zSV)=u#Z`?=Xd!^!I-j$VFsMQ3*#p!fC*_Re~QCt$2QBY>H z%{lb;au1XNAm{cG5Hq|Q6QEa2wE0wq? zB(4>L#$`BAOZL?EWdzhK5uv%w<`nMFTH>A~^U53sx3Pzsp;=qOiwMw#We0+P zGN5Hd-J{DO^4_|b*Sot;gWy6ClB)lF8*$Oe>khnK7e-u6=uv2Z5Ny`8GZlaTq#_@< zi(@T8OD!XggO+`1tS(#r-7xs^D%l`1?K>`BMub6%JK}>6ipIQdT?hB)FH1Z+P1GMa z9L$!^%aN@3h$m~s-A{j<^>$CJ+I$*^)%%wP9BDT>iw(Ya;TV(0L%~_4y&zqi&4bn?d1+ht9Q4+vst1MB=oHFRe|J%?v_Zn#H!N0>eP7A zCoX13t{Ruk5vm%7mae=JuP+4TtdCzES~6r? zboRAiV>ap;ePv#5adD)nwrU;z$0q7b8_A4anOillQcE?TV0nJgs~aGsN7)m+ZD;_< z>Q`c2L8$Xxn^%pIU-4c&c+otcsLF$nfk9|nTPijEPLc6D#siWz z*g080zhYbLP>?8L^!7eCs{!ri71%CE&JOLSb199OPB6uVW>&PN6S$QNy~_4N5Gr9LUMU{g^V&~O&d}jx z8at-07xDclW3HXJzn$2yWc8AQ(lM6TJ+AlfHW=kL+TM?H5ou@-g>gVWF?cNQsW!Xmul^W$=(lPMbr?ml z$QgB!x?`5szGH0A^1TKypc~$Obsf$e8D>;g_wVXu7nynqIR8B210c>r_vy51Jy>PP z(hWrx)bw%YB)a5VlWC#yg1lO2p8woT>V_7|iFJ#(+CKsdtxuAd4^!uC%NwooE9w9>na%=yas$J6 zf>DpaJlXjEOuN=X>BlJJfQbvF)`C%mR3T-m8L!SX1T;T+-t|)lNB*#_2Di~=Gm8aT zNwl^2*?YYOF3iT-V;;lJcrvry2ERW@UZT9F8kAUJbl<=sK2fyv=q8zo6Ak-*i4EXW z%7taxBJsAtK0{dzOfBd}biI}DTgC{7J%#+hZMzdZ&JmvJSH zsKQEK$}0E4t_0DoEi5O~b zhidSHPzutZ?|uK(Fz%_f!Kve!soTU~!#iP4MWIx(p!~3Rr6IWezMYl?eBo{7c7MBn zy`@lXSAi<7_#iTSVVz2$x=gleC=yp)jABCin{@DN%)+@TshxcwVkJnWv{m6301xr+5W~~kUK=O8>y#7^M+;dIt#fsXh24boFf{=VzDg!0yOp;7?ixdFueZC zQ|98Aw8RWHeA%~F#4Igsd*PyyA-(q<#2$4vXp*cb-B^cidxHjx=wttjE&%)`oc`8E z|E{)uptb*!Qy=M-zYc#pRsQXL_|H+qzo!%b?=;(gNVb1@8{qI>{6`j1HojJ z(wq#`ve=LWVPwV4UTimra~I|mJ(p3UfiU?rKQO36jqt|Q%z?Kbd7t@SIeH{4G1nH9 z)lq>67Bu0WJJx5%`=isjyCtTJ_)zu_g?ISONfu91?3`aD9$R|oLG;GaLiCM;wcm=* z5N>DcDfv)Q74yhRIpu0&p|gjmrZ&V(0u_;|82it*xzk3E;^;E0x>yINSUsUUd!f@U zRdMW9Ae~#ki_HLfF(#om5nJ@;3l+oY>N((gfB$^?k^@QeRb#{(-kvb4%J*E8L_9EB z0yR88JR@>N4HkBPYgrB>)B%?T1O14~kSh#*QC6DnjDneZhCS7(AY&rdUC(Fqz%0zn zU31OaZnU?hse1=DiW0Y7?)MFsWtSBOJO#|RQ#0(88krk}$UalP-;SL{rQ?CDe)a$Y z848$^W#fI6FF&5BN#U&_NJKKkZF73LegNxX6g~k;1`6y#`QeQhn=anLo+_Axhue|u zynBk;Q3jxrs>;Kq&$6YdwZuZ(hsqGPOOmI4JuDYNh0~ubh91OymCZD2BLRfr;KY%J zNlLsi(8FsB8up0}fw-Ef?e)9mf&hhsUZFzFP-f8Fu*)+QQJj}Zw%{i)lLP48!G=(z zJr*ctk;hZ>@rLP&5IM&>B%;XXUq~UvWWqb589e+KL4O%P>zu-nMTMrF*@IatR^dWSur?|qzM(3hU(%zAXCx=GyfPCwtaU~)3;D%D+f|B8OKn41V6PiZ9 zqb-o+X3K#nrR(pav#Lvw4(aFwV=S98-dvbgWchXk%$mLj&0?~5AI;ABnKrS*%c82C zy6^TC2uW#D#McXlq*0WMUJE4VgJ21fU;Ogw&1y&1F9 z*Mlg_rrFlun7MI=d}L4*GzJ9A!35xPQ0I&kjFCJCJVJjuXmkr2SD;+Bdh<|0m{K1l zk7bG0!T5j?E2k4)vxo=s_YdNs`}#zO1|Monf_>qEN<~NVWT^@yo5(k&7iZD=68qEd zaA*&$mz$UxOD#KO%(F4}2@CtZ3x&=p zJf*z*37~JoQ3nbghO!3@dOe&~bG=Z)qJKB>f%e&lI!gRPIDTa_#!$*wlX20CL=gzm z7-$<4{rW_UKhH^3?*))LL;tpBx`f(3ucnaiiyqneNQfaRfNdg+C2ekV4+nVQ)n;Oe zHKC)#64W~}nGJASK$Ao97#3&Zun;J&4oTjV7ev7YJ9)KzI|_Y5zrKoL?jn$Mw0F6_ zRq6*pkYStD{gNyPm`sO@DN{83V(?TWXch_)Er>IEJUloiXj92>(~SU zMT|3}ew|}d-FNsUMGQqW*K|@BpcUk-U+kwuDYqRh{6#=0rG@UzzW5k~g&;pt=p6jR zYe^A4^f(mNGcy;N2L{SXO9;B#Z@UEv4DZ+v3Cy}iT8BwbU+?w(9^gvq?0oZ@aY4^f z^Y1#@W8`gY?*l{)pf1MFM|&wc+4?C)YE4s9^6mXKw3Fn1=U;A9CA?@eymry)r;H-^ zT$g9LB;}824fC)Te^FjKDZ7{sK49`f(5};= zLCgKRB6Kk&Yky-j4v&%uB=}|dZOuG0pu@`eY0MfLAONDV*^*^$eq5kqKeek1xB}jPZ0{pkPue<&d^u@~2@h>Mjr@vR z)0+fTx^Rl`>bCuEIP!kD&3{u66M&MD-HL61;w2W*`7HfMZRN4;-5c{aJ{&i~=c+nS z3iM0bRiNuqfM;SsQ?go-t3Z4NV=)dkJ6l5-2tBigmY(G^AZcG5IvIF z+fGMVUDbINR(L%>P&(Kb zE{hBBs~vmAJ*LkE2FnZQkfDys7wc!STiRrf$NnB&JF`(@BylIL%n--HYtsA4lQp57 zwh>E-fnL;KAL;+IfWM69AkRPo^@Y@s2FKcGW-YX4HwRpFv1u;y!WbLsn9Rc5oun&z z*!ITA>{ZRVCFB4pgY(KHJA7q@$YAvC(&+@)9~-l&(Nx3QL5pI1P(QHWGp!zXJF*tH zN6?%1mZ%#*rLZQj0PmN3Qs_{mStnHHKIt;kwF^Lh=JY!lr@GvAF@95xN;omqYMye* z5_3GDp1(s9FL)=Cz>OZQvr<ZV|{kodl*SVDK{PRNG6>&^LO#z7C^#gk+@2XHo<}p1Ap)t+-Vc z41?#&b1S&Y?)czjbn*k=pZ4~9`&&;0L`)UHjji{Nm?bS(<4YRvl&LUi0`4P;YibT+ ziM5(6c%F^?&KdO|YTotS8w_e7`iw8hDz(sB514aZLUiv~ATB^u;IOr9+eU!SWA|S# zX?qE2+K)T;JfDZBT3I6J3<4zgNvlCs}{h*%zch{OZZOth1MpT~-{hY6eRk`lOP?25gzK^6;?E$;poM1m@TW3PW zW9Q1(QeK}`t`5x_i;OGQb|qD~R6ly@y`7_<&!5NC0S=?>(CJwDiVb-TN=G9Ia;?ST zJICvlTV3x)T~&29p!>AWU8C{F)A4TbPX3ZN;cZ+ZH#u|iG7Zpi;4MG6B&RDpv%!Rt z%?&h`haD`*y7cMP3Q}yZSn(k0WIt>B!FEjL#aOyQ8+T&$#Cg5Vs>f~K2N##zjI2yQ z(ocZ~<^Y*ipPzOM*JaraedgwN*V6RYy$-l&u;p;1F^Fxaya(H@+i{fYd-Hn@TfCjW>!k^rd^9q9t-h$8*6%R5M0!nN%GQN>UP^u;Z3mAdoqMs^92cX|U{pt@4lGqE@yGMGMekmo-9B^J z3;Wk~;ClpeX~Uh*9^&qYU0>CNywIt%ZyNYrB<&=OJ!@RN#t9g;z@4D;g?%JDUox`Zei|?eV5@!V_?dKj9EOl zB^4WZrI?$;cl|}#OC}gbe+jD|dW1MV(a1vXNBQ~{c1h9*s}-tYR|g{%WDVokX066n zAcXl3Wg(cO25;Hb9JlU`o`*?(&S@Mp*2(r764R|FglF|{50Pl2-@t$ z5!87sJ3oNAw!2CLZ`I_=?$njEQ$eMRC_{*ql1Pm|(RhN2H@&-UR1%FxX=(M9@B^n> zjo&KFS5_n$gB+TQdCwD64!~=n;Lqm%z0Cg7T4K--cfaU_Keys;+n?Ahq^9tD05K=} zw^>{V?XWbgdd5gm(N(HSa;+6QPzqy!U0u=cjZcRasnA*=n}$VcWp|=WK$Z*QU%rE3 zf9c=I^HnqpJaLA2aaH&p{K>9D51KbocH1y^2AtWv;U-NVUK$jW7Y3{3@Xsiy$Pzt* z>v{!g2u1ugDIb#(m>L7Q(yL7dA7@T47|8Y6(2uW>VEjBp&?#hPmB~c-0L-2HEI7Vl z>_N=sj9F*6y$ibLy8=*_Yt4lW$=SUFvwNP+CmX4Fkeq;WqJoz1nsy}w1vB)hfF&y^Sn`8T(wPk6lQ< zlpAH@9}5b)><$Pwv?@b@E{7WBDuqIk0( zJv3cCaN8!oiV>#}MuouTC`*r|WesV*Xj}1s8Z||L>+64$8q>^~hq#)UO_c;y7{l>V z_=a3E76WyC4Hr&jpe+^YvadNu70ucMLTeB2182iT$H{@luWNlC1(-m{2qUfR*q3lo z*0Rqh8_yajkSa@PLk(|u7vudn-u`*n>v)^q;>f`a-rYk&tHG1tOU+K|F~T?7cMQn^ zx`l?Y@=bUpiQY3<3G_BxXze@A+*NuMXq)s3B}TOjw*_sHqcrJfBy^IoBdR&~Lwgj9 zMNw{;D7vpdJAQ2%Ndq~R7y4DjAS)E4>rdqYV^DfI9ru*Zp$viKR^UEwkp`>}_1QZ! zGGkkQ@|E_{29q5s~7(9^_!W{E#!O_30qa3cynf4N0|>*8>v`3T_hn?ex}>IaRYeLm{r zm-A5zPj-;aELy25f{Q2^)ae(y0;SS3D2g5zbRP|j41)iGDeaW8SPP~aO`cWC zP*SitVDLkZ(22o%zhk;r@@Ee~JC8w*g5-^@!RJJ65Z!{#eERV~qOuWjj(W*xK~Hp9 zgT)y*7iCmC2IMrbTG1~@8(l7wa!^6@+E5KmtVMUFU?ZqqTGs;xX~erN*NTQw@z8`R zAS`>ZH)hc)zKI#w|QgKklaeby7a>{E$@E?b`>u zw=c3~Mihp5ILuLYMy_O>>?BvdHh8(c&ar%K9u!p(QAw(`AS05f=Ii+hUILHbgr7!!nqwqKq{%f&N_ct< za2#NK?VskAsHyW#zkJEP9Ou1DLh-2;ZxsC8P7LfAv};x#J99*p9PZdZ>aE$4x^eRF zV$t^~)Ky5dQ+%SRc~x zB6U@#4nBW2tZ(*vJy5WW-i9qA55`hj*5PxzkYP1S@)u15)6qv@y(Snc0$rd^JzOe_ z4F|d~UNVnNLtT`YWj8-ES_PP`7R+ALd%@Tb!Zg{BApUTkplKDmL*auRv)Wg%woeg` z_{`*q63z%|N0Bv5pd+BYs4I=_@Ra-^{^Q-y`FQhRa(X6qa}Dm7L1~8uevFpcJ9P*8 zXXYI4#-fic)iNggx?d;^9s=%y4l4hR9`uQ1~)gc%{d@385;(e zCRHql{R+^Ey@ug^KLX6)!7u!^1nE5OqMVqL9AEJdtnAT?5S;I;$oTwA*Br&l0ti-9 zygv7};A(ifnu1(-2SGq(WnPPC|6-EeaFf~yCv>NQSDoz1P|^Spm7N91b26LcWEFH++BmaySoMr5+Jy{yPwWl zpJeU5zH@J#eSX{@m#Qvc_Izb_)p&d8Gf+kWVN=}MoO5*UJiI)>?dvtX6MLCXkH_pR zCrI*iPwFH-OAD_6H!5S{HnAZjf#k435+MmUv8(*5GYFr7YcPxfTwoE($>Lh+`zU&Q z(a}}}n(yh89m;p>j2Vj2-ITyl75oVzjKb*UFt|J) zUPaHX*j@w{x9~pT7s*5|E4$gJa=v7)0GRYgjkB|7)PS*Mnw>Cj-TNRC2iO&94*3%o z?C130i<&oEua+RFBr|@|IuU1qt&3^JLQ-Eo1g2J>^`uS1HOX@H!8n+G z;ncPul_vHICKw~?e&kNvqdFE|ULlxIT}tLMhWEBf3!q_$c5puN{3q*zSh7COM6>to z%B{T4w(qQWjB=L&DRe5&YRL)G!Pm*g0Z*vs^+oFixluoEQoHk4xNo*dyg9wkT)>(d z)2tJ#{=jnJ6=n9tcje69gIB=`op!+qA1^&bZp`A7aomSHG<}}(+NZ(A#)F8IvR)dfXKydu? zrbLS4W9ohoap=u0rWe+bGkMpxMz`kWiBzLez;`iVy~ERzf;Vb;fSxb>kO5c;MXN9r z!zGcgtt^ZmPGeOt-imD#8M=~44B{4BB@V(DVYk=H39z#((N$tqbgemxUP#@ zhX`bm^V5urOT+a*)l(i!On5H6pCJIu8`s5c3V2M zG5?pguJsxt4vV6g%ZId8WTm4rorDXV4kqg*DZ@r}BY8^EuwiBVP(uC<*SC%>=P3Ma zVDcBqGiUkn}r(@XEax_+&m zaRw$beJaC*E64P%P%RQhJI=eLMPJt<^H(b<6-m*Jr=zl;B0~^LmYYR>EWEi{fkfYJ z(q&Zr;3?DicrU2HI}&EQP|>oqyT7SByFGVQP3Q8MTosO5Vnuq)OHxiWFke)s#lcXn zO7Rp81o_v>E@6cb+1?2&C~k^kL$LUpn6_czaMGr5U5s3c)#l z`0QW7-np{;bH--=98VwaWi5y?Iu?H`O6iMxnG7cWqgOC21-^c|KD%U-@-F-jk#1Y#&w3eP z=jhko-E^ZC9&I7wm9ZkGvq7QSQ}9+%U)LIBW;>5-o-vhlfMXX)oM;ZBCH=u@sgh6Z zYp{HzCd;B*1Ik2Y_;H%OR5?K&!BW7&iA~?``iVz^b=D(V_n4jsnp5U2$<8(TEs&I7kunK$1r)vNn4t^tu z>Nn9nnYu1m@@$P$yiVSL>Q>c+E66Z2&9ckQV2|FRY;v_KqtpLM@zc%#-}Y{Alx~q{ z0;@-k>yg9MMkTTJx;NXZZfe;JEUk(y^acI9CP`P5Nw;k)gx1xOVrt7mVVx9h|45Sc z7cr)7lg^32b9t}!k;L8yOQ8TDcXo!bFyU7{(kc6mom-KUfH}rRt@eVT);**Kj8(OmJ;I*kgP6|T_HZ0(!1I%aNXki8gamP6r zQ2_6iof3%aKX3@mdSS97+uJx07dE@!oi9NRtf2{hbj+O*cGWK|3dx8UqRzJ*J z6ZFL=Hc=W33kk8Po*3;V!iKNbtd=(r?FvR!RF8+Z2}qus#2^!#LF}_?@m&nY?|R?+ z<=7540TDdBI@xG~+F{a_0SALrMGq|3G!Q6jKOC>zo8#T0RyXZy6kVFYQlzy-jv)~| zp`lN^_cQ*JEP#REBaf7eAP2vN-`tqt9KuAJ@Kt<$zQ@lZH5-NxIg*t9?3vC*(9Q6( z--pgG6&=~@30T1%kyW7+QYAOJ1SFGcb~o!?D$eEmH0KirDcex2>(2+gj~&a))AvAs z2@%uA{g=%`XDQlaPnZR>ZQE}*kIvP8CPj|Wd(<(@?*gMqX3g61;&Dh#=f$}qSNdf1 z3_^CR%HIaW@49dF8^ zyp*8pSTkm$LDht(s3ikixTlkT~rG(&F>P> zs<;v&W{S=t7$T;`V*ZB*G~KN8S-4ucWo0ZVh+{)3W#NG;aK**Uy8nMVn2&^(xR_1F*TwW2D8?-K5d(QTA}5~bu7OzCQsq|ga{mjdhe)4zDUiT3R| z-0B4OvwKH$chapu@)`EQ~tLV$+C_-L90X=hSbWnK}iR%Opmby7w_cmzFn>Q0L4lU@y{Lo zO0}Jv{ox6pqN6%8xIEd#j$Wdl=|bM}R(FLQW-dalP4MbxJ>7ldANP{-&(y(D24uTU0wVTg0+7ziBlq z%Y%o)lg{O3G;f}+cFZtC4w%;bD)f9m+wr1UlqzMtYVo4G!0vU{5H6jZFEnFE#V-HG zFUt^V_f4La(?D6UD1P?`!JrySW|P8IM#CQgoe<9B;R^@1W68KsUe>a*B}FUIoT2=k z1$o(#>m97Qq`7T#usn8cE24XDDW8vf&%HeLco=;;qB3L{&A-2s~7vIFulFo*?gd38F`?$FU9LI>BoxiShb!j-Cwfpm&5gA(_EtGT`vM^cSl7O9rJsfV$VeZW zdHBRaX?4Y#Gb}({7jT{;I58-;;RzW^h=JrEtNR9BvK@W8H?0xtBcu?1UvCQ`eyPii zkOX2zmZ2@Zt3gAfwF4o}QM%J|T%{DjN1jcZHd}VHK}J%X;{BB+XEDWkJ{h#FAf@v2 zW(W(u-H9V@@rizx$ z<$dWW$6;`zYRBPMc4A&2y&L#_L*w(8&E6V5-wAe?_jFF5}!yWspY z5b}F$gz3*1%KwWro&O#h0sK)tLH5@g`F{sT{+Rl2Yb)3p|Jr;xL#f+lh7qOxme#>3 zuOj4_BQoj(1TmB~DlIV--csLUg!l%zy~g=%IM29CSRz7_(bE9)<&(+L;c0%OxWc55a7%W-Fg8Du%n*00YgX7Vabcc8@F(bQ3Z!W z8h!Xv+4lu>WD6&)IOiQ{JkVvZWR3giZ*VfioH&Wr@GCSotv}t6SH6)jrdVv!uNt~@ zcdcz4n}7lrm?Q%hbz#pk6;HNM%v4%9Fd}1?!+iV+cTm@~ByGcwfULfcy0k1U-Y6%B}Zy zKfQ|_PM>&7I;hFR352um-noh`#8VJX3>Or0Obr>wkUgUGw}jqYa}Lv#gk6YJB0(u} z%wf|PVZJV7KIeQ$Jz5I*B5_FG08_?tpw~~|Mc9gRlR1R(bZ8tByPt|F=xYG)R)7oK zUgO}0ektmcn`?ZM5GuoP zVH%?2sU!Qi6kTCufqC+dYW?a_`PHF=l+hHl&)mIJV)YEKsVTCqcfes1q95!WTzK?p zUo783j+zf!juwAD-59&sK(<*ZO8oC7k4*oy4hjgewls-2|EzKXF#aic0LDL~mw%DT zKO>p{hcfveZ8Bs73-~{HiGM4TO#f9Vw}0i%abSXS=gzoApe+(rwOy(Z@}z$b*GVt- zh2dv?fD$Rd2d~)@cUT;)TosbYGfE51V97`#;one0cei;OcSUmd6c)wy84^e7gW$cW zLIw^~=@587_=_>S-R#1WGMtI70uN$FV!FU{0(y3xoSd)D_xi8)?rCMiy|xS{2&H#P z*NJz&$Kbk&GqR}6r!5S|4bRjk733OuPX<1fT z0=XAoA~(3tZm9^=&~M@15!jM>r%YH>LzogXs|#y{?pAX6sY1?eA1HIDqJ%@Bve@?D zJ0#>WN!5J9yrfFR{EUK`<@@lHj2xR?Q>|bJQQnHq6|cPXX`V@&D4H^uAkipd@@sWh zA}8D^&QV5e%MkKtl_}c-5wMQydx^mZLcXYw%uVQJR)##XEN4)*i2(uVo?c*aH@Zjlfpl z4qIR6k{>wIf>G)cy-Fyo|FzX)6j)~9nHgfslnE7}V46XG*{0S|Bu7>YjG!DPm1)q? z8Ogcl`pOO*(Pn>H@d8F}lqh$6!>VcdZu^6+f^XQWKo~+B`<-9w_JNUkuRg5D1nU=q z9{pTLn%Y1enJ{kjcbMqiDC&K8%kRA{>Tvw5I<^r>MW0l7Iz@Va*yuA{G~yvHwmRCO z%{F`>(Zgeg2R;~l#t={KKxG=K#Yb+$)NocIE}u4)Al@vAFt`Wepk*wqql1hlmQWc4r} zlk$`@u&=!$DnFaQ=W;X=x40PPAm01)y=CgApzuX_yYBv&9g)QSOoQxONT(e*Hc$Db zur!L$YB53%0(akDWT-11xgR5$qAfjhv0$T%`A$b}zkrK}hd)O^iWu*H3IA%4>4CQi z+y}|^4a5l-5Xvy?Wj=ot94>adMS7d<#G*%fSRJcF<%=NHlhB>r)^@SrgayCi1iRqD zRKP>Hf8H1ZuNA}=06X7B#8*p34AFj|{ux9aa)sn1$)MD>Eb5sG7cl=mrmS)g>ToEg zMb&=1{Sxp!>%?@au&&M9AfX?vgw7}Qo5T$QO?fI0urq$@!V_bqqGyAEm&|3F9ZZ&} zNcDC=39UMD(+~l`sLG@%X0C}LY%M~bduIY0{$-)7YAbX#!1}vq)gU%%AF((%Ww&p3 zN!fjcCW*zsj$&uxkCf9-hgkUtRJo;T`N{__%MtKRl-G?#MQ~>FftPty3FqZsg8Im$ z1lz|if#^o29j7zTELFUu6}dZ1d9{-!FD0~96|AJrClq*zOA4mFa4PTmFnaC3et`l| zc?OtCOb)_0WNBIt1XUBxr||5c5XoLWL{`zREAO&g+d1mr7FqcDp_tV@>oUTE z4{)+U_QXjtpOdNDIXcVDujN9=5nTcgLYOD-y&|u2u@ymofvqr=DFbO8OYpAXxHlSwzAgWWXSV_%LwXbFf{xMJds zVj?1J)f5h6vaEp}*pMz9vUEmHZJk0+9Amcj5-QUxCNQ>qx|qG!H7#79y&Bb!gHK>8 zB50%T324!Qxw-~k7dR|umvGsk8V6N)@u!U)f4XutP1>D4>mMFrpO(CH_BsE_Rt!XD zX%G_k7KiaK{hY`3g+l2X?hI49)P@At2eneP(xpWk)#@E%Kx@TUUc=DQbJ0}(f{0K| zuDETQgoCr+3@P0J+E=tyzob^LqU5bSC?f9y zYI~YaQT0~vUH3FfI${gMxFrfJVpHIu8)IEL$=nteDe`0{I1E700_Qp)BHEcA8%;1B zhDKlH4gA?2^Q*a>ADpn-;>K(4*Ss8zNrp9Y7(wB*wVs`N!fsfF zTiZ@&dYLOQWYLs7!e>8FPjt$)L%CJ2y8C>%rLxIZ3|a%V%BIBuaSd2c6X;+SLB96t z2)+cUKeHr6r-~LoMGO_`ON7qOt=cfVS>}F)*UNN508>!j1Ygwwb7#nUqB7}y+f0li zR|eI(G}*#2C0}&!8CD&-MjZiUbFvmHk5_i>Q(Xm{x3*CbcwBNXYBhI({Bf~IYMD}j zxDJKc5S`NDUyIKH{?4g%zPM4TC=b-LOQi^p_ic(>a$Lj!yUjI4`ymbP2Na>?(aZ)1 zGZb(Z``qrREq=Ps z>Ti*Btaa9p^*f+P`U0~Pe8ku-bW#t2R8rw_Of0V(e*Q)nuC}r7YCEA+=WB8Ek^xiG ziFBHkWBD`bB4y0JK_ixlWG#@(oC*$wExx$5cs4=~CivmLq?s`dXhss_x`=}vUiwWN z7D$y7=)O%BY2tp*tJAJ~J$}Nba_0L(4-KWr{{8vlp$R?gT0?jVO)MJX1qP-J!opQU!hK8`y zunO`!Todad!in{}AS(!dXY$O6d-Ek}dw>864l=k#%u=X0ei?YuSNCla7Po$AX`FqL6&MYSy+5#k$5$ZcPg;21q~ zyF-36`C!cMQA7RpSTW2vlsOHwaM2F#J%RInPp){c1~H_vO3LQ)4%$yq-0-GMz9DrN1$<3v zxXztAx^-ghoh?Xnj?p=`$Qz4|{t4qE9wDY)LxY~9dH9Kr*oYk^1Fvp?71BUgo_N|-IBt;6&=;` zrWB6BSu*B9x{~+k6xdW*S8BC4$?%DNO_z( z-+(6n*(u;Zo&qBOn;c`n|2E0^kEwq<1%8X6{=#j79B%(O1KrZg)j%=SrgzJ`v~_tU zbl42MYZ+FYk#S}lAqmZmCQJxl{OM$<6ncZub`J~vps9bpQO)_pMlu-@^d8a@^S*;fkHf+IjPwU2qVV}j`^24|w4Y$(!O7%`Ob`eLo z%8khoAm1}~Kk^nqtQSfmK3NX0D^s0vALi*;otRjH?hzb;DMO;f-(8!{TKiZgi`Jdd zb;GfJQPsJ{Wus0j*DLu(+4eHWSEnaeFS5ru33L8aEnDXtD5Yo8l*O!MLuamNQK=%^ z_3M>K`SQ0{7vO%2%p!O1Nk_%Wfm7qi$VaUlODQy&H+ z#ePj!7uFE*yu6wuG_>)6W?{_rx`f>?2JeErm~bNwZVu~a-{K_V;RvI?zn*jVc6SeX ztSqQTJgBo8#aA-^JXO+n+GbgbpE>2kQ1Xtx^Xv%&(;nEfr&oc7H$lB?w@czTQgOj- z)=#Gw&nTJz^G-ram|JgSrZ$3XxN6Yi8#clYR^JIkR(M`kUoG9ydsOPXn5=ESw?9>H zj#%8RJL%jwBYODf>SaDcD!qrslDVX)tSz@mcRI;qpN@W`ylAvIQLNbRP(s^y<(}Z( zPy#DpYa|I|H;Fkc_wyPBuXuF5TpU?s#*5kT^@fk0wYDC$>?dWfdfuogxpZmmT^~T> z_9bxluf88b^I|k&A>&?u#`rMpQu?7MoFVm|-B4C=^7uwO?Y)zSUtusu*{&~^cOG%V zS?@Sh6rTe@h&W=F(G&t&1xHBtYvkvUJM&~UH&_DT6JR?XrU6o|Sh4EklQD7c+7CF} z>5s6I&^I!EY(dSE>^Y6@*b9uN!UNw#vJvn&Y7+N(Lp4oU*7P--MXdLjd#;62P9CUd zJH}k^*;&LphYAHx+w^zg)FNwLH<{f=Lp&@_=jS!mJTHaSJsq8^T=(kP6~pD;6PVlH zOCF5@h2SWDAS+Vj@dy|B-^#prCe0z|(`UG?_t_R8LO9-*AKbWaI;pB)^(eS2UZXu+ zS30er!?a<~E*0A_iw9X!))6sH%wssNXj$ur?w;D3b`fx52rp06OacC6N9W=rgY+$3%7V`q(=mI-f_H~3;P za^`EU?)Lg%d)2(AsPZsS8S@CK!(?PqMT56cddfak6af^22CPr4YRa-zB}!)l!xPg$ z8`oQTarMS+Sc`80)8j|k#qVGUF-T@+z%3N~pIdUqOdqX(n&BCdMns@7GU1*OvUv9i zV32_Id0e9UdnO*%h)8q#Kr6 z938fz!QB<~MqXx0v2Y+OVzDH$ZE{c-;F-03J8fVl;zvcHKsix3v~+1>&9PnyA2oTH zxKym5S{k?G`rHJ&V~fRs>eW##pbR#sA&n8wztGS}W_(5XX&zF|^U>Ek7~@9}FfbT4 z;Ruk7*|L;>ZCaNWf*3XngNDgLdd)o4YsLkARZhs9z8G07dS9Q9TbmJyf!;_ zJ$5nrwr}Exnk8RDd`x5tu@j;FThe*#hx_~*RpXW)5!rq=2Rq?-Xbk(?k)cZF#z!y^-H|?^Zhg~G5xTM4Y_)iUIA#<6B?>C|jwagtk zgBam+=Cscl?;1PZ^d$uIK1BwITufKF!NDcH7h9LB{jfe6eToFmyORuI@Sp`B94I6o zjuE16!jC$W-l*+eNhlYk;@ej7dQ9Z9JM^}=`lkIQv}JHF#JrOW;Y4(%7YJW5Rbg(@ zxzF$+5mDC9A8!tKkUYSC;NfFa8|y1;vaV(msk(PK;VotS$4D$3e|1P+{jwvv*e^vC z!^AzcrtTI{*^eAhckoA>D0+8AYs1M=@DkP7ez6xN74Io~)W!i?$)|E(z!bgGY&DLr z?rn6p)K^+`7A&gFHD}+#GXfuk3^V4sxT<-oz@~AYR_Twg-SodTO-@`MdEh>2B(cAJ z#P(lblJ*Wr6w|W3Vyl@{U)l1u1?%Zg=1SfJR=Y)Fr7C=pyn({;q`JUsfilGl!@&f0 zrou^(MTx-WX4Yeq%Q3t%%8W<2PGgI=)5BNoz&UKuQu4}<0H=^s= z8jUtjZG=EKP#xEq!PrH6hlko?8JWLpW9Mpmwv)~V7Iv!$`ptV^&BIXiY3Eaqg7*Ev z@LF{Inwj(dvf!;vgCDRRmu{$O@J@K|h2KHzU9Mk~yt}z332jyurI#Hg3C_C9!~Eo> zkUJyxX0%lc?=9Jpr!COp?Pm5Gvswa7Rg9$^Casl2t24Hj+o{e6@b=K6V6kLNVi>`12Knzo!9x1B^qVNwQ)LNXosYBq}o;7KQ_R>6LkOF?&Hz2=R$c zOukkYCYA=#)j!WWN-cobWO^x4{P{2f9~#`@Ee5*_J|q@O#@x^F50@&hcdfc#vHYZv z7`U3lV!V|a<(-O-k+u^jwuzXc$?!n^AUrypW{M9EYIL&-?N0}66$#3^47xfp6IC|n zNU3TfdhJEUPQ{b~7t$O)5d49$+5ug&`JY2Xw!=nd!(PEWu11Pc++7!D$V`o-8&j?e%5OUA}`V z*Gsv3sB=T9>RU^ezapD4Mw>&TB&eiQIr!TwFi%iH$7c8&Rs}{tF1$le(GVZ*y<)Kv zM#eW@Dx0z9B5XHbWk5;lz4$kcL*-F!#&h-fw+>nf5$RH*3sxDQz-X3I(;Wuf5}@AE zQZT}WLs220#m6I5P|3AMH|um+HS639!`jhTV-Xy`!)7|fxbZD2#Rw2^Kjb7#aCOMg z2DWJMS9tj%v8@@x(Wq`SPv{?Z5Gm1(3h)o&hhex~M9PXI(rK(w2&kiOtmSoC+{j?K zp{a0Sgff@FCs^gw=+vty|0wOjIGoi`4irO(gl~=PmlH-LXhuxnMl1q2GUrtDx3CuF zT!2~1K`WiTN#wrO%mzR=Lsc#e<-_V%0WGBcsXxoyji|)yuXW|ZTGEh_%j42f>g0ZU9&}s_rlmwGRI3Z2-Ao zCB`R1whZS)qisZ5N&wp6w}l}b6?zms5k6(B`D%v`=c>BE@>JUyKIMTW-O?RfAn+BY zphR9?=4Z+E@pb(}lE?jp0H~_2HinhgB^?DjCyGpzSiv5(8MB0 zt8kP+0(|>2@b7M+tXS5tFpxRd!YnYQ)4o}Ul{n4=NCwwjLBwT{J;P%7_G$7xO_`Xa zk4kq2wJW73VQOvtT`iCFeMiK*2{R@wJ2X$Z4VlP_(+$+qCwmK-5MlCMJCSAnEG0itMk z*i6+O%g}|ieTlwIGQzsWy>E5v`Bvy}O2;%jj}L5>E=5^^^G{X#4d<<6z#HvWV|n5( z2ggfKK=?`YXTFjh&Dl1aq?+$~Rh&3GA|qqQTY97vw;g6>v01%G`3I`1vu!VB1$LHXfniGRoX7 zvDCZtb?L+RIO<+Zh=~sbMsw{ct#ix}IGS_uQASb_tUViYKwKTmjvK2ZVwZYMBA( zrqHFKe)-go_0>Z8BAqq%-L6xydJgZ^HH9zosH0|8^UbK6_YvwUdsE)^K8_sPo{kw% zi@I}M&(8V)Yagwr+km>ql{Tvf+dD$FYV<4CE`w3CHm-{&spuh7uOBz&ClHgyxZpUl z&5DcQ0w+H`SLB0xV#2tRT@fvdljFS0cng;(vlab30q1M%lQ_{J=hS4FI6-f@zfa+D zt@v;*{`D%?>ms0vlRUV#Nb;uxQ&Ixh-jLapie{{3W~vsl8z@ipfYfk}%vIJQ2_P`s zGD-D71$WKO;vyp$n5lQcgO@6gN1qu~xNGMlbCM%M+xJ3Bp}S{onHn)XUd-(;p)KzM zg>ly9Rk87qWg_x9uic1Hj612MNN-Buiw0LLLT2GbmeM2>C#Qc6N`n7KwAquJZ35krdz;!0=2zDiisggotKH-UK#Rrykr4EeZjxp9~Cn$_1$pn;Qp)Gr@6U znL-cx;iT4nZ6F#k1(FJt_i(EMH3^;Kk3qS~;cmnjDP-g%4@Lj?Ey6^RhcKopU&Co) z^~zncbUwcAb+kYv$qekb${L^)nNyzEZIDRfo???-9}NL^$!&Sq_2nOGaqG)S*+Z?` z+<3#lmX0tTW+IshYd_l5F6|3Zm$EP3j*T%qmX19*)UPxy0vQsmYc)D8fj?9qk>@&j zgSAf`V1Q0jII16^bH%DOq*}c z)!N5wmZs?`%$x8lW@1jV!p-5#P_oS19MHCGii1EuT`)B=JS=L?sIKtBt@NL7fe@nu zl;A2`?Gro|;(DpPmM3=Y3ypIegQ8SvkWvq8!Pds`hwx0H&S95gIZ=rH69!h*nmTZX zvI5DYC|e1aG1bDis^iSnF0IyBjbOODaZz9hDHS`p$AjVv+s~X`I@WWo&n}nX%8{kP z1OmVv(vZ#EbFq|A>668eMnLPvqKk&*h7It|)NXw{UVT|6q`{Ru`42`$45#PR{?jui zXOjA)UI{-TcOhtebgbwnKMYyg!yjlMF*{Ilc470lQ}C3$!Bwxg(s|~cqHJE_PNIx` zM?F_dF@gklJD?3EhGu3~)<%J65&zH=`Gp_ofQ*}W$)WAx0l2l>y{}%t6HCBlSN;lX zzg0UZjZFAH!qZ)yW$4M*F1EzJhzHLj`)jbg`UvapqQqT!A#^wI0UG#yn@2xjl3C-Gn{3Mo$f?K!2H6!P$+ewoIPB56T< z>%#fOPn{bgE3LRV0$G5zZ`s{seEB3XKTyk_um1YiaA z5cxOTnwb#i!*BjG(bA}qph=`O!M zhu?S0{M)pk;T->5&t>$bhYucmyOjc7Jar6(<6o6uF%w>z30KI~#I$+ubi=$G&498lI^3BL zNzH_q?=OOUeBf$csg$y2O*Q1#W%=+Q?e7Xk6Mk@^EM}Ju@O%cbhdMsUln-aTQoKl9CS{g^A8;KH|QeZJzH`dR9aF2GLZ86 z3#n<)c;%MyGfTOm!-)k`!dBU4lez1$(4{O?<}?&?Q!_+OVqHU<+-|5}KbrwhDTe8q zL=3HAlW6rWuvKyF1e-)@JVf3H89RORdYcY#>EfK*=}L5;!qQp<`?!PJPVcTgr8Jt< z#Tpa^%%oV?IQzmAAK*A;=3#*6ocdtJw>OcxWBdPQ z!gmmN##YGfghb}vDVgln4w>wfcxLi<5Z!y9O!m$`meRy2mePAKmhw9359s&Z4G8Rg zm?mpybWh+$z}&u6;$aICnm z-k*5g+T{`yRC@z2#GKWae>4gK+u%1Av%Nn$zmz?67F|_7pQ-wD9$YNErsIztxpvNv zUzO>u2~MzFO+24}*z1Tb>ME6O%b6nd;kx?8^{)p>{UJ!|@ozYV9nbT2Op_QuQg0w- ztxn9zihHQ`&4SoXEbr@S!vM1c2t4?5jbzF!DszNmGDEoPOk>0ul9lfGPv=Px?xUZJTIKsx)_k9EGQ2Q6i6IG`u$8ZX8NMBnRhM~hz*}Kx z5t$62Gqeb}h`cW&Cpt#K%o6FEnYxg?qr}_()Wk{QGLW>_fuvm;_cxw@8E|Eqe%!OM zlPyj-Z?go^_@|smGgxHGgfChTtKcNU6i4`X>PuF;fqC#{)KIHh*;rCG&`gjSMw3?kEd7@OC+AN$36Ecs!Y!Cc|#u0bF1 z>zhYez&~jH6Il%NKzP14j}t|S&!+S3das=O3qVm1;7v-eV>hp(kxo@(z}3;%Wbc@4 z$7dkrf&l~NLP1yT2XivbE7#Z$nQuXt66365vg52INEZxrDHnQbV;UH8uIq-#<;3s`vb=F(iYEM#Zuin&_m^j}RFT4S zFoR;|pf7q2Y9a<92V;GwUvEq4D;cX1xtcqf5t+H$89O)_n}FVyGrn2U*#iS;iNCAF|X@YAspX;J-}M#|j9mx3e=g0=bLytsISi zoeKYG&IZyX{-rehBeJfhU~KZQV+z2+`A>h7u(6@7kueDWW9>-9%?)~3TPtU48%H8g zQ#&~aVIG7_B!r1HxjBpsn2e3tnAr3= z^jVAm05*LS7EWVE0F$9HD+?PZBQLiR=pzN?6R?;ta{`Q++4PM8EI=k^6Hd^dtcE5W zoUFXse>vC;4Zy%DLH4Y?>?>`khes|Zp+yE(O@`zB z-_>8D|H%*Ve=_+0!r1?R{r?Le;{RX@v;V=^|1=a74A*yXas!=B@F2f3JUO|DtSJ2d E0@3k%I{*Lx diff --git a/docs/problem_formulation/problem_formulation_ocp_mex.tex b/docs/problem_formulation/problem_formulation_ocp_mex.tex index 05c8736a4f..136f40a894 100644 --- a/docs/problem_formulation/problem_formulation_ocp_mex.tex +++ b/docs/problem_formulation/problem_formulation_ocp_mex.tex @@ -433,8 +433,6 @@ \subsection{Cost module \code{'CONVEX\_OVER\_NONLINEAR'}: Convex-over-nonlinear In this case, a Generalized Gauss-Newton Hessian might be used for the SQP subproblems \cite{Messerer2021a}, which is used whenever \code{AcadosOcpOptions.hessian\_approx = 'GAUSS\_NEWTON'}. See Table~\ref{tab:cost:conl} for the available options of this cost module. -\textbf{Note.} Convex-over-nonlinear costs are at the moment only available via the \python{} interface. - \begin{table}[ht!] \centering @@ -464,7 +462,7 @@ \subsection{Cost module \code{'CONVEX\_OVER\_NONLINEAR'}: Convex-over-nonlinear \\[4pt] \bottomrule \end{tabular} - \caption{Cost module \code{'CONVEX\_OVER\_NONLINEAR'} options. Convex-over-nonlinear costs are at the moment only available via the \python{} interface.} \label{tab:cost:conl} + \caption{Cost module \code{'CONVEX\_OVER\_NONLINEAR'} options.} \label{tab:cost:conl} \end{table} \section{Constraints}\label{sec:constraints} diff --git a/examples/acados_matlab_octave/test/run_matlab_tests.m b/examples/acados_matlab_octave/test/run_matlab_tests.m index 3701a899fd..73325c1a1a 100644 --- a/examples/acados_matlab_octave/test/run_matlab_tests.m +++ b/examples/acados_matlab_octave/test/run_matlab_tests.m @@ -54,17 +54,18 @@ %% run all tests test_names = [ + "test_conl_cost", "test_code_reuse", "test_sim_code_reuse", "run_test_dim_check", -"run_test_ocp_mass_spring", -% "run_test_ocp_pendulum", -"run_test_ocp_wtnx6", -% "run_test_sim_adj", -"run_test_sim_dae", -% "run_test_sim_forw", -"run_test_sim_hess", -"param_test", + "run_test_ocp_mass_spring", + % "run_test_ocp_pendulum", + "run_test_ocp_wtnx6", + % "run_test_sim_adj", + "run_test_sim_dae", + % "run_test_sim_forw", + "run_test_sim_hess", + "param_test", ]; for k = 1:length(test_names) diff --git a/examples/acados_matlab_octave/test/test_conl_cost.m b/examples/acados_matlab_octave/test/test_conl_cost.m new file mode 100644 index 0000000000..b8895448a0 --- /dev/null +++ b/examples/acados_matlab_octave/test/test_conl_cost.m @@ -0,0 +1,160 @@ +% +% 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.; + +% + +%% Test of CONVEX_OVER_NONLINEAR cost type in MATLAB interface +%% This test compares CONVEX_OVER_NONLINEAR with NONLINEAR_LS cost types +%% to verify they produce the same solution for a quadratic cost + +import casadi.* + +check_acados_requirements() + +%% Common problem setup +nx = 2; +nu = 1; +N = 20; +T = 1.0; +x0 = [1.0; 0.5]; +ny = nx + nu; +ny_e = nx; + +% Cost matrices (quadratic cost) +W = diag([1.0, 1.0, 0.01]); +W_e = diag([1.0, 1.0]); + +% References +yref = zeros(ny, 1); +yref_e = zeros(ny_e, 1); + +%% Solve with both cost formulations +fprintf('\n=== Testing CONVEX_OVER_NONLINEAR and NONLINEAR_LS cost formulations ===\n'); + +cost_types = {'CONVEX_OVER_NONLINEAR', 'NONLINEAR_LS'}; +x_sols = cell(2, 1); +u_sols = cell(2, 1); + +for i = 1:2 + cost_type = cost_types{i}; + fprintf('\n--- Solving with %s cost ---\n', cost_type); + + % Create model + x = SX.sym('x', nx); + u = SX.sym('u', nu); + f_expl = vertcat(x(2), u); + + model = AcadosModel(); + model.name = [lower(cost_type), '_test']; + model.x = x; + model.u = u; + model.f_expl_expr = f_expl; + + % Create OCP + ocp = AcadosOcp(); + ocp.model = model; + ocp.solver_options.tf = T; + ocp.solver_options.N_horizon = N; + + ocp.json_file = ['ocp' model.name '.json']; + + % Set cost type + ocp.cost.cost_type = cost_type; + ocp.cost.cost_type_e = cost_type; + + % Set cost expressions + ocp.model.cost_y_expr = vertcat(x, u); + ocp.model.cost_y_expr_e = x; + + ocp.cost.yref_e = yref_e; + ocp.cost.yref = yref; + + if strcmp(cost_type, 'CONVEX_OVER_NONLINEAR') + % CONVEX_OVER_NONLINEAR cost setup + r = SX.sym('r', ny); + r_e = SX.sym('r_e', ny_e); + ocp.model.cost_r_in_psi_expr = r; + ocp.model.cost_r_in_psi_expr_e = r_e; + ocp.model.cost_psi_expr = 0.5 * r' * W * r; + ocp.model.cost_psi_expr_e = 0.5 * r_e' * W_e * r_e; + else + % NONLINEAR_LS cost setup (equivalent to CONVEX_OVER_NONLINEAR with quadratic psi) + ocp.cost.W = W; + ocp.cost.W_e = W_e; + end + + % Set constraints + ocp.constraints.x0 = x0; + ocp.constraints.lbu = -2.0; + ocp.constraints.ubu = 2.0; + ocp.constraints.idxbu = 0; + + % Set solver options + ocp.solver_options.nlp_solver_type = 'SQP'; + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'; + ocp.solver_options.qp_solver_cond_N = N; + ocp.solver_options.nlp_solver_max_iter = 100; + ocp.solver_options.nlp_solver_tol_stat = 1e-6; + + % Create solver and solve + solver = AcadosOcpSolver(ocp); + solver.solve(); + status = solver.get('status'); + + solver.print_statistics(); + + if status ~= 0 + error(['%s solver returned status ', num2str(status)], cost_type); + end + + % Extract solution + x_sols{i} = solver.get('x'); + u_sols{i} = solver.get('u'); +end + +%% Compare solutions +fprintf('\n=== Comparing solutions ===\n'); +tol = 1e-9; + +x_diff = max(max(abs(x_sols{1} - x_sols{2}))); +u_diff = max(max(abs(u_sols{1} - u_sols{2}))); + +fprintf('Max state difference: %.2e\n', x_diff); +fprintf('Max control difference: %.2e\n', u_diff); + +if x_diff > tol + error(['State solutions differ by more than tolerance! Max diff: ', num2str(x_diff)]); +end + +if u_diff > tol + error(['Control solutions differ by more than tolerance! Max diff: ', num2str(u_diff)]); +end + +fprintf('\n=== All tests passed! ===\n'); +fprintf('CONVEX_OVER_NONLINEAR and NONLINEAR_LS produce equivalent solutions.\n'); diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index cd5dac930b..e3dc1b58d2 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -127,6 +127,44 @@ function make_consistent_cost_initial(self) error('setting nonlinear least square cost: need W_0, cost_y_expr_0, at least one missing.') end dims.ny_0 = ny; + elseif strcmp(cost.cost_type_0, 'CONVEX_OVER_NONLINEAR') + % MATLAB interface adaptation mirroring python _make_consistent_cost_initial + % Required model fields: cost_y_expr_0, cost_r_in_psi_expr_0, cost_psi_expr_0 + if isempty(model.cost_y_expr_0) + error('cost_y_expr_0 not provided for CONVEX_OVER_NONLINEAR initial cost.'); + end + % determine ny_0 from cost_y_expr_0 (casadi SX/MX => size); allow numeric vector too + try + ny = length(model.cost_y_expr_0); % works for CasADi MX/SX (returns number of elements) + catch + ny = numel(model.cost_y_expr_0); + end + if isempty(model.cost_r_in_psi_expr_0) + error('cost_r_in_psi_expr_0 not provided for CONVEX_OVER_NONLINEAR initial cost.'); + end + if length(model.cost_r_in_psi_expr_0) ~= ny + error('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_expr_0.'); + end + if isempty(model.cost_psi_expr_0) + error('cost_psi_expr_0 not provided for CONVEX_OVER_NONLINEAR initial cost.'); + end + if length(model.cost_psi_expr_0) ~= 1 + error('cost_psi_expr_0 must be scalar-valued.'); + end + if isempty(cost.yref_0) + warning(['yref_0 not defined provided.' 10 'Using zeros(ny_0,1) by default.']); + self.cost.yref_0 = zeros(ny,1); + end + if size(cost.yref_0,1) ~= ny + error('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_expr_0.'); + end + dims.ny_0 = ny; + % Hessian approximation check (mirror python): allow GAUSS_NEWTON or EXACT with exact_hess_cost == false + opts = self.solver_options; + if ~( (strcmp(opts.hessian_approx,'EXACT') && opts.exact_hess_cost == false) || strcmp(opts.hessian_approx,'GAUSS_NEWTON') ) + error(['With CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:' 10 ... + 'GAUSS_NEWTON or EXACT with exact_hess_cost == false.']); + end end end @@ -165,6 +203,34 @@ function make_consistent_cost_path(self) error('setting nonlinear least square cost: need W, cost_y_expr, at least one missing.') end dims.ny = ny; + elseif strcmp(cost.cost_type, 'CONVEX_OVER_NONLINEAR') + if isempty(model.cost_y_expr) + error('cost_y_expr not provided for CONVEX_OVER_NONLINEAR path cost.'); + end + try + ny = length(model.cost_y_expr); + catch + ny = numel(model.cost_y_expr); + end + if isempty(model.cost_r_in_psi_expr) || length(model.cost_r_in_psi_expr) ~= ny + error('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi_expr.'); + end + if isempty(model.cost_psi_expr) || length(model.cost_psi_expr) ~= 1 + error('cost_psi_expr not provided or not scalar-valued.'); + end + if isempty(cost.yref) + warning(['yref not defined provided.' 10 'Using zeros(ny,1) by default.']); + self.cost.yref = zeros(ny,1); + end + if size(cost.yref,1) ~= ny + error('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi_expr.'); + end + dims.ny = ny; + opts = self.solver_options; + if ~( (strcmp(opts.hessian_approx,'EXACT') && opts.exact_hess_cost == false) || strcmp(opts.hessian_approx,'GAUSS_NEWTON') ) + error(['With CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:' 10 ... + 'GAUSS_NEWTON or EXACT with exact_hess_cost == false.']); + end end end @@ -204,6 +270,34 @@ function make_consistent_cost_terminal(self) error('setting nonlinear least square cost: need W_e, cost_y_expr_e, at least one missing.') end dims.ny_e = ny_e; + elseif strcmp(cost.cost_type_e, 'CONVEX_OVER_NONLINEAR') + if isempty(model.cost_y_expr_e) + error('cost_y_expr_e not provided for CONVEX_OVER_NONLINEAR terminal cost.'); + end + try + ny_e = length(model.cost_y_expr_e); + catch + ny_e = numel(model.cost_y_expr_e); + end + if isempty(model.cost_r_in_psi_expr_e) || length(model.cost_r_in_psi_expr_e) ~= ny_e + error('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_expr_e.'); + end + if isempty(model.cost_psi_expr_e) || length(model.cost_psi_expr_e) ~= 1 + error('cost_psi_expr_e not provided or not scalar-valued.'); + end + if isempty(cost.yref_e) + warning(['yref_e not defined provided.' 10 'Using zeros(ny_e,1) by default.']); + self.cost.yref_e = zeros(ny_e,1); + end + if size(cost.yref_e,1) ~= ny_e + error('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_expr_e.'); + end + dims.ny_e = ny_e; + opts = self.solver_options; + if ~( (strcmp(opts.hessian_approx,'EXACT') && opts.exact_hess_cost == false) || strcmp(opts.hessian_approx,'GAUSS_NEWTON') ) + error(['With CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:' 10 ... + 'GAUSS_NEWTON or EXACT with exact_hess_cost == false.']); + end end end @@ -1475,7 +1569,7 @@ function make_consistent(self, is_mocp_phase) generate_c_code_nonlinear_least_squares(context, ocp.model, cost_dir, stage_types{i}); case 'CONVEX_OVER_NONLINEAR' - error("Convex-over-nonlinear cost is not implemented yet.") + generate_c_code_conl_cost(context, ocp.model, cost_dir, stage_types{i}); case 'EXTERNAL' generate_c_code_ext_cost(context, ocp.model, cost_dir, stage_types{i}); diff --git a/interfaces/acados_matlab_octave/generate_c_code_conl_cost.m b/interfaces/acados_matlab_octave/generate_c_code_conl_cost.m new file mode 100644 index 0000000000..b42d857212 --- /dev/null +++ b/interfaces/acados_matlab_octave/generate_c_code_conl_cost.m @@ -0,0 +1,153 @@ +% +% 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.; + +% + +function generate_c_code_conl_cost(context, model, target_dir, stage_type) + + import casadi.* + + %% load model + x = model.x; + u = model.u; + z = model.z; + p = model.p; + t = model.t; + + % check type + if isa(x(1), 'casadi.SX') + isSX = true; + else + isSX = false; + end + + if strcmp(stage_type, 'initial') + yref = model.cost_r_in_psi_expr_0; + y_expr = model.cost_y_expr_0; + outer_expr = model.cost_psi_expr_0; + res_expr = model.cost_r_in_psi_expr_0; + custom_hess = model.cost_conl_custom_outer_hess_0; + + suffix_name_fun = '_conl_cost_0_fun'; + suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess'; + + elseif strcmp(stage_type, 'path') + yref = model.cost_r_in_psi_expr; + y_expr = model.cost_y_expr; + outer_expr = model.cost_psi_expr; + res_expr = model.cost_r_in_psi_expr; + custom_hess = model.cost_conl_custom_outer_hess; + + suffix_name_fun = '_conl_cost_fun'; + suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess'; + + elseif strcmp(stage_type, 'terminal') + yref = model.cost_r_in_psi_expr_e; + y_expr = model.cost_y_expr_e; + outer_expr = model.cost_psi_expr_e; + res_expr = model.cost_r_in_psi_expr_e; + custom_hess = model.cost_conl_custom_outer_hess_e; + + suffix_name_fun = '_conl_cost_e_fun'; + suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess'; + + % create dummy u, z for terminal stage + if isSX + u = SX.sym('u', 0, 0); + z = SX.sym('z', 0, 0); + else + u = MX.sym('u', 0, 0); + z = MX.sym('z', 0, 0); + end + end + + % Check if required expressions are defined + if isempty(y_expr) + error(['cost_y_expr for stage ' stage_type ' is empty. Required for CONVEX_OVER_NONLINEAR cost.']); + end + if isempty(outer_expr) + error(['cost_psi_expr for stage ' stage_type ' is empty. Required for CONVEX_OVER_NONLINEAR cost.']); + end + if isempty(res_expr) + error(['cost_r_in_psi_expr for stage ' stage_type ' is empty. Required for CONVEX_OVER_NONLINEAR cost.']); + end + + % Set up function names + fun_name_cost_fun = [model.name suffix_name_fun]; + fun_name_cost_fun_jac_hess = [model.name suffix_name_fun_jac_hess]; + + % Set up functions to be exported + % inner_expr = y_expr - yref + inner_expr = y_expr - yref; + + % outer_loss_fun: psi(residual, t, p) = outer_expr + outer_loss_fun = Function('psi', {res_expr, t, p}, {outer_expr}); + + % cost_expr = outer_loss_fun(inner_expr, t, p) + cost_expr = outer_loss_fun(inner_expr, t, p); + + % outer_loss_grad_fun: gradient of outer loss w.r.t. residual + outer_grad = jacobian(outer_expr, res_expr).'; + outer_loss_grad_fun = Function('outer_loss_grad', {res_expr, t, p}, {outer_grad}); + + % Compute or use custom hessian + if isempty(custom_hess) + % Compute hessian of outer loss w.r.t. residual + [hess, ~] = hessian(outer_loss_fun(res_expr, t, p), res_expr); + else + hess = custom_hess; + end + + outer_hess_fun = Function('outer_hess', {res_expr, t, p}, {hess}); + outer_hess_expr = outer_hess_fun(inner_expr, t, p); + + % Check if hessian is diagonal + outer_hess_is_diag = outer_hess_expr.sparsity().is_diag(); + + % if residual dimension <= 4, do not exploit diagonal structure + ny = length(res_expr); + if ny <= 4 + outer_hess_is_diag = 0; + end + + % Jacobians of inner expression w.r.t. u, x, z + Jt_ux_expr = jacobian(inner_expr, vertcat(u, x)).'; + Jt_z_expr = jacobian(inner_expr, z).'; + + % Add functions to context + context.add_function_definition(fun_name_cost_fun, ... + {x, u, z, yref, t, p}, {cost_expr}, target_dir, 'cost'); + + context.add_function_definition(fun_name_cost_fun_jac_hess, ... + {x, u, z, yref, t, p}, ... + {cost_expr, outer_loss_grad_fun(inner_expr, t, p), Jt_ux_expr, Jt_z_expr, outer_hess_expr, outer_hess_is_diag}, ... + target_dir, 'cost'); + +end + From cb86df0811fdd236a9351368a6b491fece1c0d94 Mon Sep 17 00:00:00 2001 From: Josua Christoph Lindemann <107478604+ArgoJ@users.noreply.github.com> Date: Fri, 31 Oct 2025 10:53:37 +0100 Subject: [PATCH 164/164] ros2 control sequence publishing and test launch upgrade (#1673) A short upgrade that enables publishing of the optimized control sequence if needed. Also updated the test launch file, to directly compare values of the solver and not only looks at the parameters, subscribers, and publishers. Also made the test launch more stable without extra subprocesses but just one extra ros client. --- .../ros2/example_ros_minimal_ocp.py | 15 +- .../acados_template/acados_ocp.py | 17 +- .../acados_template/ros2/ocp_node.py | 15 + .../acados_template/ros2/utils.py | 2 +- .../ocp_interface_templates/CMakeLists.in.txt | 5 +- .../{ControlInput.in.msg => Control.in.msg} | 2 +- .../ControlSequence.in.msg | 8 + .../ocp_node_templates/node.in.cpp | 53 +++- .../ocp_node_templates/node.in.h | 27 +- .../ocp_node_templates/test.launch.in.py | 278 +++++++++++++----- 10 files changed, 311 insertions(+), 111 deletions(-) rename interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/{ControlInput.in.msg => Control.in.msg} (78%) create mode 100644 interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlSequence.in.msg diff --git a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py index 847880358e..a90c10d801 100644 --- a/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py +++ b/examples/acados_python/pendulum_on_cart/ros2/example_ros_minimal_ocp.py @@ -104,6 +104,7 @@ def create_minimal_ocp(export_dir: str, N: int = 20, Tf: float = 1.0, Fmax: floa ocp.ros_opts = AcadosOcpRosOptions() ocp.ros_opts.package_name = "pendulum_on_cart_ocp" ocp.ros_opts.generated_code_dir = export_dir + ocp.ros_opts.publish_control_sequence = True ocp.code_export_directory = str(os.path.join(export_dir, "c_generated_code")) return ocp @@ -125,17 +126,8 @@ def main(): # call SQP_RTI solver in the loop: tol = 1e-6 - SPLIT_RTI = True for i in range(20): - if SPLIT_RTI: - # preparation - ocp_solver.options_set("rti_phase", 1) - status = ocp_solver.solve() - # feedback - ocp_solver.options_set("rti_phase", 2) - status = ocp_solver.solve() - else: - status = ocp_solver.solve() + status = ocp_solver.solve() ocp_solver.print_statistics() residuals = ocp_solver.get_residuals(recompute=True) print("residuals after ", i, "SQP_RTI iterations:\n", residuals) @@ -151,6 +143,9 @@ def main(): simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") + expected_u_file = os.path.join(export_dir, 'expected_control_sequence.npy') + np.save(expected_u_file, simU) + ocp_solver.print_statistics() # plot diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 0f03e578ea..70b4ff4fb3 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1314,12 +1314,17 @@ def _get_ros_template_list(self) -> list: msg_dir = os.path.join(interface_dir, 'msg') template_file = os.path.join(ros_interface_dir, 'State.in.msg') template_list.append((template_file, 'State.msg', msg_dir, ros_template_glob)) - template_file = os.path.join(ros_interface_dir, 'References.in.msg') - template_list.append((template_file, 'References.msg', msg_dir, ros_template_glob)) - template_file = os.path.join(ros_interface_dir, 'Parameters.in.msg') - template_list.append((template_file, 'Parameters.msg', msg_dir, ros_template_glob)) - template_file = os.path.join(ros_interface_dir, 'ControlInput.in.msg') - template_list.append((template_file, 'ControlInput.msg', msg_dir, ros_template_glob)) + template_file = os.path.join(ros_interface_dir, 'Control.in.msg') + template_list.append((template_file, 'Control.msg', msg_dir, ros_template_glob)) + if self.dims.ny > 0 or self.dims.ny_0 > 0 or self.dims.ny_e > 0: + template_file = os.path.join(ros_interface_dir, 'References.in.msg') + template_list.append((template_file, 'References.msg', msg_dir, ros_template_glob)) + if self.ros_opts.publish_control_sequence: + template_file = os.path.join(ros_interface_dir, 'ControlSequence.in.msg') + template_list.append((template_file, 'ControlSequence.msg', msg_dir, ros_template_glob)) + if self.dims.np > 0: + template_file = os.path.join(ros_interface_dir, 'Parameters.in.msg') + template_list.append((template_file, 'Parameters.msg', msg_dir, ros_template_glob)) # --- Solver Package --- ros_pkg_dir = os.path.join('ocp_node_templates') diff --git a/interfaces/acados_template/acados_template/ros2/ocp_node.py b/interfaces/acados_template/acados_template/ros2/ocp_node.py index cf6fc55ddc..bdf215a6a0 100644 --- a/interfaces/acados_template/acados_template/ros2/ocp_node.py +++ b/interfaces/acados_template/acados_template/ros2/ocp_node.py @@ -38,11 +38,15 @@ def __init__(self): self.namespace: str = "" self.archtype: str = ArchType.NODE.value + # Topics self.__control_topic: str = "ocp_control" self.__state_topic: str = "ocp_state" self.__parameters_topic: str = "ocp_params" self.__reference_topic: str = "ocp_reference" + + # Other options self.__threads: int = 1 + self.__publish_control_sequence: bool = False @property def control_topic(self) -> str: @@ -64,6 +68,10 @@ def reference_topic(self) -> str: def threads(self) -> str: return self.__threads + @property + def publish_control_sequence(self) -> bool: + return self.__publish_control_sequence + @control_topic.setter def control_topic(self, value: str): if not isinstance(value, str): @@ -94,6 +102,12 @@ def threads(self, value: int): raise TypeError('Invalid threads value, expected int.\n') self.__threads = value + @publish_control_sequence.setter + def publish_control_sequence(self, value: bool): + if not isinstance(value, bool): + raise TypeError('Invalid publish_control_sequence value, expected bool.\n') + self.__publish_control_sequence = value + def to_dict(self) -> dict: return super().to_dict() | { "control_topic": self.control_topic, @@ -101,4 +115,5 @@ def to_dict(self) -> dict: "parameters_topic": self.parameters_topic, "reference_topic": self.reference_topic, "threads": self.threads, + "publish_control_sequence": self.publish_control_sequence, } \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2/utils.py b/interfaces/acados_template/acados_template/ros2/utils.py index bb5089b398..8ee8efb6d4 100644 --- a/interfaces/acados_template/acados_template/ros2/utils.py +++ b/interfaces/acados_template/acados_template/ros2/utils.py @@ -19,7 +19,7 @@ class AcadosRosBaseOptions: def __init__(self): self.__package_name: str = "acados_base" - self.__node_name: str = "acados_base_node" + self.__node_name: str = "" self.__namespace: str = "" self.__generated_code_dir: str = "ros_generated_code" self.__archtype: str = ArchType.NODE.value diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt index e22e6a7e7e..445e6c8527 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/CMakeLists.in.txt @@ -12,7 +12,10 @@ find_package(rosidl_default_generators REQUIRED) # --- INTERFACES --- set(MSG_FILES "msg/State.msg" - "msg/ControlInput.msg" + "msg/Control.msg" + {%- if ros_opts.publish_control_sequence %} + "msg/ControlSequence.msg" + {%- endif %} {%- if dims.ny > 0 or dims.ny_0 > 0 or dims.ny_e > 0 %} "msg/References.msg" {%- endif %} diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlInput.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/Control.in.msg similarity index 78% rename from interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlInput.in.msg rename to interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/Control.in.msg index dc776a312b..33d1f02329 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlInput.in.msg +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/Control.in.msg @@ -1,4 +1,4 @@ -# Generic state message +# Generic control message std_msgs/Header header # control vector diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlSequence.in.msg b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlSequence.in.msg new file mode 100644 index 0000000000..d6a3ec1c49 --- /dev/null +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_interface_templates/ControlSequence.in.msg @@ -0,0 +1,8 @@ +# Generic control trajectory message +std_msgs/Header header + +# control vector +Control[{{ solver_options.N_horizon }}] control_sequence + +# Status, 0 = OK +uint8 status \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp index f13503a71c..0b4f9fc990 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.cpp @@ -10,11 +10,13 @@ namespace {{ ros_opts.package_name }} {%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} {%- set references_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.reference_topic %} {%- set parameters_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.parameters_topic %} +{%- set control_sequence_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic ~ "_sequence" %} {%- else %} {%- set control_topic = "/" ~ ros_opts.control_topic %} {%- set state_topic = "/" ~ ros_opts.state_topic %} {%- set references_topic = "/" ~ ros_opts.reference_topic %} {%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} +{%- set control_sequence_topic = "/" ~ ros_opts.control_topic ~ "_sequence" %} {%- endif %} {%- set has_slack = dims.ns > 0 or dims.ns_0 > 0 or dims.ns_e > 0 %} {%- set use_multithreading = ros_opts.threads is defined and ros_opts.threads > 1 %} @@ -75,8 +77,12 @@ namespace {{ ros_opts.package_name }} {%- endif %} // --- Publisher --- - control_input_pub_ = this->create_publisher<{{ ros_opts.package_name }}_interface::msg::ControlInput>( + control_pub_ = this->create_publisher<{{ ros_opts.package_name }}_interface::msg::Control>( "{{ control_topic }}", 10); + {%- if ros_opts.publish_control_sequence %} + control_sequence_pub_ = this->create_publisher<{{ ros_opts.package_name }}_interface::msg::ControlSequence>( + "{{ control_sequence_topic }}", 10); + {%- endif %} // --- Init solver --- this->initialize_solver(); @@ -199,8 +205,16 @@ void {{ ClassName }}::control_loop() { void {{ ClassName }}::solver_status_behaviour(int status) { // publish u0 also if the solver failed - this->get_input(u0_.data(), 0); - this->publish_input(u0_, status); + this->get_control(u0_.data(), 0); + this->publish_control(u0_, status); + + {%- if ros_opts.publish_control_sequence %} + // publish full control sequence + for (size_t i = 0; i < {{ solver_options.N_horizon }}; ++i) { + this->get_control(u_seq_[i].data(), static_cast(i)); + } + this->publish_control_sequence(u_seq_, status); + {%- endif %} {%- if solver_options.nlp_solver_type == "SQP_RTI" %} // prepare for next iteration @@ -292,13 +306,32 @@ void {{ ClassName }}::parameters_callback(const {{ ros_opts.package_name }}_inte // --- ROS Publisher --- -void {{ ClassName }}::publish_input(const std::array& u0, int status) { - auto control_input = std::make_unique<{{ ros_opts.package_name }}_interface::msg::ControlInput>(); - control_input->header.stamp = this->get_clock()->now(); - control_input->status = status; - control_input->u = u0; - control_input_pub_->publish(std::move(control_input)); +void {{ ClassName }}::publish_control( + const std::array& u0, + int status +) { + auto control = std::make_unique<{{ ros_opts.package_name }}_interface::msg::Control>(); + control->header.stamp = this->get_clock()->now(); + control->status = status; + control->u = u0; + control_pub_->publish(std::move(control)); } +{%- if ros_opts.publish_control_sequence %} + +void {{ ClassName }}::publish_control_sequence( + const std::array, {{ solver_options.N_horizon }}>& u_sequence, + int status +) { + auto control_sequence = std::make_unique<{{ ros_opts.package_name }}_interface::msg::ControlSequence>(); + control_sequence->header.stamp = this->get_clock()->now(); + control_sequence->status = status; + + for (size_t i = 0; i < {{ solver_options.N_horizon }}; ++i) { + control_sequence->control_sequence[i].u = u_sequence[i]; + } + control_sequence_pub_->publish(std::move(control_sequence)); +} +{%- endif %} // --- ROS Parameter --- @@ -755,7 +788,7 @@ int {{ ClassName }}::ocp_solve() { // --- Acados Getter --- -void {{ ClassName }}::get_input(double* u, int stage) { +void {{ ClassName }}::get_control(double* u, int stage) { {%- if use_multithreading %} std::lock_guard lock(solver_mutex_); {%- endif %} diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h index 2f3b2c6b87..d0a6cd554b 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/node.in.h @@ -12,8 +12,11 @@ // ROS2 message includes #include "{{ ros_opts.package_name }}_interface/msg/state.hpp" -#include "{{ ros_opts.package_name }}_interface/msg/control_input.hpp" +#include "{{ ros_opts.package_name }}_interface/msg/control.hpp" #include "{{ ros_opts.package_name }}_interface/msg/references.hpp" +{%- if ros_opts.publish_control_sequence %} +#include "{{ ros_opts.package_name }}_interface/msg/control_sequence.hpp" +{%- endif %} {%- if dims.np > 0 %} #include "{{ ros_opts.package_name }}_interface/msg/parameters.hpp" {%- endif %} @@ -46,7 +49,10 @@ class {{ ClassName }} : public rclcpp::Node { {%- endif %} // --- ROS Publishers --- - rclcpp::Publisher<{{ ros_opts.package_name }}_interface::msg::ControlInput>::SharedPtr control_input_pub_; + rclcpp::Publisher<{{ ros_opts.package_name }}_interface::msg::Control>::SharedPtr control_pub_; + {%- if ros_opts.publish_control_sequence %} + rclcpp::Publisher<{{ ros_opts.package_name }}_interface::msg::ControlSequence>::SharedPtr control_sequence_pub_; + {%- endif %} // --- ROS Params and Timer rclcpp::TimerBase::SharedPtr control_timer_; @@ -77,6 +83,9 @@ class {{ ClassName }} : public rclcpp::Node { bool first_solve_{true}; {%- endif %} std::array u0_; + {%- if ros_opts.publish_control_sequence %} + std::array, {{ solver_options.N_horizon }}> u_seq_{}; + {%- endif %} std::array current_x_; {%- if dims.ny_0 > 0 %} std::array current_yref_0_; @@ -109,14 +118,22 @@ class {{ ClassName }} : public rclcpp::Node { {%- endif %} // --- ROS Publisher --- - void publish_input(const std::array& u0, int status); + void publish_control( + const std::array& u0, + int status); + {%- if ros_opts.publish_control_sequence %} + void publish_control_sequence( + const std::array, {{ solver_options.N_horizon }}>& u_sequence, + int status); + {%- endif %} // --- ROS Parameter --- void setup_parameter_handlers(); void declare_parameters(); void load_parameters(); void apply_all_parameters_to_solver(); - rcl_interfaces::msg::SetParametersResult on_parameter_update(const std::vector& params); + rcl_interfaces::msg::SetParametersResult on_parameter_update( + const std::vector& params); template void get_and_check_array_param( @@ -155,7 +172,7 @@ class {{ ClassName }} : public rclcpp::Node { int ocp_solve(); // --- Acados Getter --- - void get_input(double* u, int stage); + void get_control(double* u, int stage); void get_state(double* x, int stage); // --- Acados Setter --- diff --git a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py index db2de2950e..60c5c33806 100644 --- a/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py +++ b/interfaces/acados_template/acados_template/ros2_templates/ocp_node_templates/test.launch.in.py @@ -1,6 +1,5 @@ import re -from typing import Union -from unittest import result +import os import rclpy import unittest import launch @@ -8,24 +7,69 @@ import launch_testing import pytest import subprocess +import numpy as np + +from typing import Union +from unittest import result from launch_ros.actions import Node +from rclpy.parameter import Parameter, parameter_value_to_python +from rcl_interfaces.srv import GetParameters, SetParameters +from rclpy.publisher import Publisher -from {{ ros_opts.package_name }}_interface.msg import State, ControlInput, References +from {{ ros_opts.package_name }}_interface.msg import State, Control, References +{%- if ros_opts.publish_control_sequence -%} +, ControlSequence +{%- endif %} {%- if dims.np > 0 -%} , Parameters {%- endif %} {%- set ns = ros_opts.namespace | lower | trim(chars='/') | replace(from=" ", to="_") %} {%- if ns %} -{%- set control_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.control_topic %} -{%- set state_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.state_topic %} -{%- set references_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.reference_topic %} -{%- set parameters_topic = "/" ~ ros_opts.namespace ~ "/" ~ ros_opts.parameters_topic %} + {%- set topic_prefix = '/' ~ ns ~ '/' %} {%- else %} -{%- set control_topic = "/" ~ ros_opts.control_topic %} -{%- set state_topic = "/" ~ ros_opts.state_topic %} -{%- set references_topic = "/" ~ ros_opts.reference_topic %} -{%- set parameters_topic = "/" ~ ros_opts.parameters_topic %} + {%- set topic_prefix = '/' %} {%- endif %} +{%- set control_topic = topic_prefix ~ ros_opts.control_topic %} +{%- set state_topic = topic_prefix ~ ros_opts.state_topic %} +{%- set references_topic = topic_prefix ~ ros_opts.reference_topic %} +{%- set parameters_topic = topic_prefix ~ ros_opts.parameters_topic %} +{%- set control_sequence_topic = control_topic ~ '_sequence' %} + + +class AsyncParametersClient: + """Simple async parameter client.""" + def __init__(self, node, remote_node_name): + self.node = node + self.remote_node_name = remote_node_name + self._get_client = self.node.create_client( + GetParameters, + f'/{remote_node_name}/get_parameters') + self._set_client = self.node.create_client( + SetParameters, + f'/{remote_node_name}/set_parameters') + + def wait_for_service(self, timeout_sec=1.0): + ok1 = self._get_client.wait_for_service(timeout_sec=timeout_sec) + ok2 = self._set_client.wait_for_service(timeout_sec=timeout_sec) + return ok1 and ok2 + + def get_parameters(self, names): + request = GetParameters.Request() + request.names = names + return self._get_client.call_async(request) + + def set_parameters(self, params: list[tuple[str, object]]): + """Set parameters synchron via service (returns Future).""" + request = SetParameters.Request() + for name, value in params: + if isinstance(value, (list, tuple)): + value = [float(v) for v in value] + if isinstance(value, (int, float)): + value = float(value) + p = Parameter(name, value=value) + request.parameters.append(p.to_parameter_msg()) + return self._set_client.call_async(request) + @pytest.mark.launch_test def generate_test_description(): @@ -54,26 +98,23 @@ def tearDownClass(cls): def setUp(self): self.node = rclpy.create_node('generated_node_test') + self.param_client = AsyncParametersClient(self.node, '{{ ros_opts.node_name }}') def tearDown(self): self.node.destroy_node() def test_set_constraints(self, proc_info): - """ - Test if constraints compile-time declared default parameters. - """ + """Test if constraints compile-time declared default parameters.""" {%- for field, param in constraints %} {%- if param and ((field is starting_with('l')) or (field is starting_with('u'))) and ('bx_0' not in field) %} param_name = "constraints.{{ field }}" expected_value = [{{- param | join(sep=', ') -}}] - self.__check_parameter_set(param_name, expected_value) + self._check_parameter_set(param_name, expected_value) {%- endif %} {%- endfor %} def test_set_cost(self, proc_info): - """ - Test if cost compile-time declared default parameters. - """ + """Test if cost compile-time declared default parameters.""" # --- Weights --- {%- for field, param in cost %} {%- if param and (field is starting_with('W')) %} @@ -85,7 +126,7 @@ def test_set_cost(self, proc_info): {%- if not loop.last %}, {% endif -%} {%- endfor -%} ] - self.__check_parameter_set(param_name, expected_value) + self._check_parameter_set(param_name, expected_value) {%- endif %} {%- endfor %} {%- if has_slack %} @@ -96,96 +137,179 @@ def test_set_cost(self, proc_info): {%- if param and (field_l is starting_with('z')) %} param_name = "cost.{{ field }}" expected_value = [{{- param | join(sep=', ') -}}] - self.__check_parameter_set(param_name, expected_value) + self._check_parameter_set(param_name, expected_value) {%- endif %} {%- endfor %} {%- endif %} def test_set_solver_options(self, proc_info): - """ - Test if solver options compile-time declared default parameters. - """ - # --- Solver Options --- + """Test if solver options compile-time declared default parameters.""" param_name = "ts" expected_value = {{ solver_options.Tsim }} - self.__check_parameter_set(param_name, expected_value) + self._check_parameter_set(param_name, expected_value) def test_subscribing(self, proc_info): """Test if the node subscribes to all expected topics.""" - self.wait_for_subscription('{{ state_topic }}') - self.wait_for_subscription('{{ references_topic }}') + self._wait_for_subscription('{{ state_topic }}') + self._wait_for_subscription('{{ references_topic }}') {%- if dims.np > 0 %} - self.wait_for_subscription('{{ parameters_topic }}') + self._wait_for_subscription('{{ parameters_topic }}') {%- endif %} def test_publishing(self, proc_info): """Test if the node publishes to all expected topics.""" - self.wait_for_publisher('{{ control_topic }}') + self._wait_for_publisher('{{ control_topic }}') + {%- if ros_opts.publish_control_sequence %} + self._wait_for_publisher('{{ control_sequence_topic }}') + {%- endif %} + + {% if ros_opts.publish_control_sequence %} + def test_control_sequence_values(self, proc_info): + """ + Test if the node's published control sequence matches + the one pre-computed by the Python solver. + """ + expected_u_sequence = self._load_expected_sequence() + + self.received_control_sequence = None + sub = self.node.create_subscription( + ControlSequence, + '{{ control_sequence_topic }}', + self._control_sequence_callback, + 10 + ) + + pub = self.node.create_publisher(State, '{{ state_topic }}', 10) + self._publish_initial_state(pub) - def wait_for_subscription(self, topic: str, timeout: float = 2.0): + condition_met = self._wait_for_condition( + condition_check=lambda: self.received_control_sequence is not None, + timeout=10.0 + ) + + self.assertTrue( + condition_met, + "TEST FAILED: '{{ control_sequence_topic }}' message not received (Timeout)." + ) + self.assertEqual(self.received_control_sequence.status, 0, "Solver status was not successful (0).") + + expected_length = {{ solver_options.N_horizon }} + self.assertEqual(len(self.received_control_sequence.control_sequence), expected_length) + self.assertEqual(expected_u_sequence.shape[0], expected_length) + + for i, control_msg in enumerate(self.received_control_sequence.control_sequence): + expected_u = expected_u_sequence[i] + + self.assertEqual( + len(control_msg.u), + len(expected_u), + f"Control vector at step {i} has wrong dimension." + ) + + for j in range(len(expected_u)): + self.assertAlmostEqual( + control_msg.u[j], + expected_u[j], + places=2, + msg=(f"Value deviation at sequence[{i}].u[{j}]. " + f"Received: {control_msg.u[j]}, Expected: {expected_u[j]}") + ) + + def _control_sequence_callback(self, msg): + """Callback to store the received control sequence.""" + self.received_control_sequence = msg + + def _load_expected_sequence(self): + test_script_dir = os.path.dirname(os.path.realpath(__file__)) + expected_u_file = os.path.abspath( + os.path.join(test_script_dir, '..', '..', 'expected_control_sequence.npy') + ) + if not os.path.exists(expected_u_file): + self.skipTest(f"Expected control sequence file not found: {expected_u_file}") + return np.load(expected_u_file) + + def _publish_initial_state(self, publisher: Publisher, timeout_sec: float = 2.0): + state_msg = State() + {%- if constraints.has_x0 %} + state_msg.x = [float(v) for v in ({{ constraints.lbx_0 | join(sep=', ') }})] + {%- else %} + state_msg.x = [0.0] * {{ dims.nx }} + {%- endif %} + + end_time = time.time() + timeout_sec + while time.time() < end_time and publisher.get_subscription_count() < 1: + rclpy.spin_once(self.node, timeout_sec=0.1) + + if publisher.get_subscription_count() < 1: + self.fail(f"Publisher on Topic '{publisher.topic_name}' " + f"could not find a Subscriber (Timeout).") + + publisher.publish(state_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + {% endif %} + + def _wait_for_subscription(self, topic: str, timeout: float = 2.0): end_time = time.time() + timeout while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) subs = self.node.get_subscriptions_info_by_topic(topic) if subs: return True - time.sleep(0.1) self.fail(f"Node has NOT subscribed to '{topic}'.") - def wait_for_publisher(self, topic: str, timeout: float = 2.0): + def _wait_for_publisher(self, topic: str, timeout: float = 2.0): end_time = time.time() + timeout while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) pubs = self.node.get_publishers_info_by_topic(topic) if pubs: return True - time.sleep(0.1) self.fail(f"Node has NOT published to '{topic}'.") - def __check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): + def _wait_for_condition(self, condition_check: callable, timeout: float = 10.0) -> bool: + """Spin the node until a condition is met or a timeout occurs.""" + end_time = time.time() + timeout + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + if condition_check(): + return True + return False + + def _check_parameter_get(self, param_name: str, expected_value: Union[list[float], float]): """Run a subprocess command and return its output.""" - output = get_parameter(param_name) - numbers = [float(x) for x in re.findall(r"[-+]?\d*\.\d+|\d+", output)] + if not self.param_client.wait_for_service(timeout_sec=2.0): + self.fail(f"Parameter service for '{{ ros_opts.node_name }}' not available.") + + future = self.param_client.get_parameters([param_name]) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=2.0) + if not future.done(): + self.fail(f"Timeout while getting parameter {param_name}") + resp = future.result() + if not resp.values: + self.fail(f"Parameter {param_name} does not exist.") + actual_value = parameter_value_to_python(resp.values[0]) + if isinstance(expected_value, list): - self.assertListEqual(numbers, expected_value, f"Parameter {param_name} has the wrong value! Got {numbers}") + self.assertListEqual( + list(actual_value), expected_value, + f"Parameter {param_name} has the wrong value! Got {actual_value}") else: - self.assertEqual(numbers[0], expected_value, f"Parameter {param_name} has the wrong value! Got {numbers[0]}") + self.assertEqual( + actual_value, expected_value, + f"Parameter {param_name} has the wrong value! Got {actual_value}") - def __check_parameter_set(self, param_name: str, new_value: Union[list[float], float]): + def _check_parameter_set(self, param_name: str, new_value: Union[list[float], float]): """Run a subprocess command and return its output.""" - try: - set_parameter(param_name, new_value) - self.__check_parameter_get(param_name, new_value) - except subprocess.CalledProcessError as e: - self.fail(f"Failed to set parameter {param_name}.\n" - f"Exit-Code: {e.returncode}\n" - f"Stderr: {e.stderr}\n" - f"Stdout: {e.stdout}") - - -def get_parameter(param_name: str): - """Run a subprocess command and return its output.""" - cmd = ['ros2', 'param', 'get', '{{ ros_opts.node_name }}', param_name] - result = subprocess.run( - cmd, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - text=True, - check=True - ) - return result.stdout - -def set_parameter(param_name: str, value: Union[list[float], float]): - """Run a subprocess command to set a parameter.""" - if isinstance(value, list): - value_str = "[" + ",".join(map(str, value)) + "]" - else: - value_str = str(value) - - cmd = ['ros2', 'param', 'set', '{{ ros_opts.node_name }}', param_name, value_str] - result = subprocess.run( - cmd, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - text=True, - check=True - ) - return result.stderr \ No newline at end of file + if not self.param_client.wait_for_service(timeout_sec=2.0): + self.fail(f"Parameter service for '{{ ros_opts.node_name }}' not available.") + + future = self.param_client.set_parameters([(param_name, new_value)]) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) + if not future.done() or future.result() is None: + self.fail(f"Timeout while setting parameter {param_name}") + + results = future.result().results + if not results or not results[0].successful: + self.fail(f"Failed to set parameter {param_name}: {results[0].reason if results else 'unknown'}") + + self._check_parameter_get(param_name, new_value) \ No newline at end of file