From 76b7df6169eaa8f41630edca10c3ebd6da07ab14 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 12 Feb 2024 15:14:48 +0100 Subject: [PATCH 01/16] fix Matlab with MSVC (#1036) https://discourse.acados.org/t/bug-with-visual-studio-compiler-in-matlab-with-working-fix/1449 --- interfaces/acados_matlab_octave/acados_ocp.m | 2 +- .../compile_ocp_shared_lib.m | 20 +++++++++++++++++-- .../compile_sim_shared_lib.m | 20 +++++++++++++++++-- .../c_templates_tera/CMakeLists.in.txt | 7 +++++++ 4 files changed, 44 insertions(+), 5 deletions(-) diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index 115b675c54..19e5456889 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -204,7 +204,7 @@ % templated MEX return_dir = pwd(); - obj.code_gen_dir = obj.acados_ocp_nlp_json.code_export_directory; + obj.code_gen_dir = obj.acados_ocp_nlp_json.code_export_directory; cd(obj.code_gen_dir) mex_solver_name = sprintf('%s_mex_solver', obj.model_struct.name); 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 0c84b13e5d..19ab18e234 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 @@ -32,7 +32,6 @@ function compile_ocp_shared_lib(export_dir) return_dir = pwd; cd(export_dir); - %% build main file if isunix [ status, result ] = system('make shared_lib'); if status @@ -41,8 +40,25 @@ function compile_ocp_shared_lib(export_dir) 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 - [ status, result ] = system('cmake -G "MinGW Makefiles" -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); + 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',... 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 index 577573d9d2..d3ee59a739 100644 --- 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 @@ -32,7 +32,6 @@ function compile_sim_shared_lib(export_dir) return_dir = pwd; cd(export_dir); - %% build main file if isunix [ status, result ] = system('make sim_shared_lib'); if status @@ -41,8 +40,25 @@ function compile_sim_shared_lib(export_dir) 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 - [ 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 .'); + 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',... 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 9e12077501..03f82723bb 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 @@ -168,6 +168,13 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows") endif() +if(CMAKE_CXX_COMPILER_ID MATCHES "MSVC") + set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE {{ code_export_directory | replace(from="\", to="/") }}) + set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE {{ code_export_directory | replace(from="\", to="/") }}) + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE {{ code_export_directory | replace(from="\", to="/") }}) +endif() + + # object target names set(MODEL_OBJ model_{{ model.name }}) set(OCP_OBJ ocp_{{ model.name }}) From e84fa19cb12db0996dc29cb039121dc70e93a039 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 14 Feb 2024 13:36:22 +0100 Subject: [PATCH 02/16] Support slack penalties for initial stage in MEX interface (#1037) Support slack penalties for initial stage in MEX interface. Follow-up on https://discourse.acados.org/t/issue-in-setting-slack-variable-on-control-input/1456 The convention is as follows now: - bounds on u and general linear constraints are also enforced on the initial node - bounds on x and nonlinear constraints are fully split and have to be explicitly stated with `_0` correspondence to be enforced on the stage. --- interfaces/acados_matlab_octave/acados_ocp.m | 2 +- interfaces/acados_matlab_octave/acados_ocp_model.m | 8 ++++++++ .../set_up_acados_ocp_nlp_json.m | 13 +++++++++++++ .../matlab_templates/make_sfun_sim.in.m | 2 +- 4 files changed, 23 insertions(+), 2 deletions(-) diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index 19e5456889..cdb2cfb2ea 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -197,7 +197,7 @@ % generate templated solver if nargin < 3 - simulink_opts = get_acados_simulink_opts; + simulink_opts = get_acados_simulink_opts(); end obj.acados_ocp_nlp_json = set_up_acados_ocp_nlp_json(obj, simulink_opts); ocp_generate_c_code(obj); diff --git a/interfaces/acados_matlab_octave/acados_ocp_model.m b/interfaces/acados_matlab_octave/acados_ocp_model.m index 86ff456b58..b3a6d6ce28 100644 --- a/interfaces/acados_matlab_octave/acados_ocp_model.m +++ b/interfaces/acados_matlab_octave/acados_ocp_model.m @@ -188,6 +188,14 @@ obj.model_struct.cost_zu = value; elseif (strcmp(field, 'cost_zu_e')) obj.model_struct.cost_zu_e = value; + elseif (strcmp(field, 'cost_zl_0')) + obj.model_struct.cost_zl_0 = value; + elseif (strcmp(field, 'cost_zu_0')) + obj.model_struct.cost_zu_0 = value; + elseif (strcmp(field, 'cost_Zl_0')) + obj.model_struct.cost_Zl_0 = value; + elseif (strcmp(field, 'cost_Zu_0')) + obj.model_struct.cost_Zu_0 = value; else disp(['acados_ocp_model: set: wrong field: ', field]); keyboard; diff --git a/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m b/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m index 0284f5fbf1..f7b5f6edd1 100644 --- a/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m +++ b/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m @@ -485,6 +485,19 @@ ocp_json.cost.zu = model.cost_zu; end + if isfield(model, 'cost_Zl_0') + ocp_json.cost.Zl_0 = diag(model.cost_Zl_0); + end + if isfield(model, 'cost_Zu_0') + ocp_json.cost.Zu_0 = diag(model.cost_Zu_0); + end + if isfield(model, 'cost_zl_0') + ocp_json.cost.zl_0 = model.cost_zl_0; + end + if isfield(model, 'cost_zu_0') + ocp_json.cost.zu_0 = model.cost_zu_0; + end + if isfield(model, 'cost_Zl_e') ocp_json.cost.Zl_e = diag(model.cost_Zl_e); 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 96493a518a..2aa78f4bef 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 @@ -77,7 +77,7 @@ LIB_PATH = '{{ acados_lib_path }}'; -LIBS = '-lacados -lblasfeo -lhpipm'; +LIBS = '-lacados -lhpipm -lblasfeo'; try % eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... From 7cf7a3af67467038d868b95536e4a1623d1c3325 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 14 Feb 2024 13:39:29 +0100 Subject: [PATCH 03/16] Split condensing and regularization into preparation and feedback (#1034) Condensing modules now provide - `condense` - all condensing operations - `condense_rhs` - condensing the right hand side - `condense_lhs` - condensing the left hand side Similarly, regularization modules now provide - `regularize` - all operations - `regularize_rhs` - operations on the right hand side - `regularize_lhs` - operations on the left hand side Only if RTI is split into preparation and feedback, the `_rhs` / `_lhs` routines are called. Otherwise, operations are done all together. Addresses https://github.com/acados/acados/issues/1031 NOTE: using convexify with split RTI phase has now relatively overhead, see https://github.com/acados/acados/issues/1035 Splitting the computations for `convexify` more efficiently should be done in another PR. --- acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 1 - acados/ocp_nlp/ocp_nlp_reg_common.h | 6 +- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 331 +++++++++++++----- acados/ocp_nlp/ocp_nlp_reg_mirror.c | 19 +- acados/ocp_nlp/ocp_nlp_reg_noreg.c | 17 +- acados/ocp_nlp/ocp_nlp_reg_project.c | 17 +- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 22 +- acados/ocp_nlp/ocp_nlp_sqp.c | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 69 ++-- acados/ocp_qp/ocp_qp_common.h | 3 +- acados/ocp_qp/ocp_qp_full_condensing.c | 39 ++- acados/ocp_qp/ocp_qp_full_condensing.h | 4 + acados/ocp_qp/ocp_qp_partial_condensing.c | 34 +- acados/ocp_qp/ocp_qp_partial_condensing.h | 4 + acados/ocp_qp/ocp_qp_xcond_solver.c | 90 ++++- acados/ocp_qp/ocp_qp_xcond_solver.h | 9 +- docs/algorithm_overview/index.md | 2 +- .../ocp/example_sqp_rti_loop.py | 14 +- examples/c/regularization.c | 2 +- 19 files changed, 546 insertions(+), 139 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 527475259a..53485bc002 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -818,7 +818,6 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * config->sim_solver->evaluate(config->sim_solver, work->sim_in, work->sim_out, opts->sim_solver, mem->sim_solver, work->sim_solver); - // TODO transition functions for changing dimensions not yet implemented! // B blasfeo_pack_tran_dmat(nx1, nu, work->sim_out->S_forw + nx1 * nx, nx1, mem->BAbt, 0, 0); diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.h b/acados/ocp_nlp/ocp_nlp_reg_common.h index 15d15479df..de8c4259c2 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.h +++ b/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -48,7 +48,7 @@ extern "C" { /* dims */ -//typedef ocp_qp_dims ocp_nlp_reg_dims; +// same as qp_dims typedef struct { int *nx; @@ -95,7 +95,9 @@ typedef struct void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); /* functions */ - void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); + void (*regularize)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); + void (*regularize_lhs)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); + void (*regularize_rhs)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); } ocp_nlp_reg_config; diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index ae0ea3cf7f..5927ecc703 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -166,14 +166,14 @@ acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp { size += 2*blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq } - size += blasfeo_memsize_dmat(nuxM, nuxM); // tmp_RSQ + size += blasfeo_memsize_dmat(nuxM, nuxM); // tmp_RSQ // size += 2*blasfeo_memsize_dvec(nxM); // grad b2 // giaf's size += (2*(N+1)+N)*sizeof(struct blasfeo_dmat *); // RSQrq BAbt DCt size += (3*(N+1)+2*N)*sizeof(struct blasfeo_dvec *); // rq b ux pi lam - size += (N+1)*sizeof(int *); // idxb + size += (N+1)*sizeof(int *); // idxb return size; } @@ -264,8 +264,8 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, mem->lam = (struct blasfeo_dvec **) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dvec *); - mem->idxb = (int **) c_ptr; - c_ptr += (N+1)*sizeof(int *); + mem->idxb = (int **) c_ptr; + c_ptr += (N+1)*sizeof(int *); align_char_to(64, &c_ptr); @@ -540,10 +540,10 @@ void ocp_nlp_reg_convexify_memory_set(void *config_, ocp_nlp_reg_dims *dims, voi /************************************************ * functions ************************************************/ - -// NOTE this only considers the case of (dynamcs) equality constraints (no inequality constraints) +// Algorithm 6 from Verschueren2017 +// NOTE this only considers the case of (dynamics) equality constraints (no inequality constraints) // TODO inequality constraints case -void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +void ocp_nlp_reg_convexify_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_convexify_memory *mem = mem_; ocp_nlp_reg_convexify_opts *opts = opts_; @@ -554,38 +554,34 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di int *nu = dims->nu; int N = dims->N; -#if 0 - for(ii=0; ii<=N; ii++) - { - blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0); - blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); - } - return; -#endif - double delta = opts->delta; - // Algorithm 6 from Verschueren2017 - - blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); - + blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0); // TODO regularize R at last stage if needed !!! - // TODO fix for nu[N]>0 !!!!!!!!!! + // TODO fix for nu[N]>0 !!!!!!!!!! + + // delta_eye = delta * eye(nx[N]) blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0); blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0); + // Q_tilde = delta_eye blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0); + // Q_bar = RSQ[N] blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0); + // RSQ[N] = Q_tilde blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]); + // Q_bar -= Q_tilde blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0); + // make Q_bar symmetric blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); for (ii = N-1; ii >= 0; --ii) { + // add b in BAbt, rq in RSQrq blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0); blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0); - + // backup RSQrq -> original_RSQrq blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); // printf("----------------\n"); @@ -602,10 +598,13 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di // blasfeo_print_dmat(nx+nu, nx, &work->qp_in->BAbt[i], 0, 0); // TODO implement using cholesky + + // BAQ = BA * Q_bar blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0); + // rank nx[ii+1] update to RSQrq with BAQ blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); - blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); // printf("BAQ\n"); // blasfeo_print_dmat(nx+nu, nx, &BAQ, 0, 0); @@ -620,32 +619,23 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di if (needs_regularization) { - blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); - // TODO project only nu instead ??????????? - // TODO compute correction as a separate matrix, and apply to original_RSQrq too (TODO change this name then) + blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + // TODO project only nu instead ??????????? + // TODO compute correction as a separate matrix, and apply to original_RSQrq too (TODO change this name then) // acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); -#if 1 - blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); - acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); - blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); -#else - blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]); - acados_project(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); - blasfeo_pack_dmat(nu[ii], nu[ii], mem->reg_hess, nu[ii], mem->RSQrq[ii], 0, 0); -#endif -// blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]); -// acados_mirror(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); -// acados_project(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); -// blasfeo_pack_dmat(nu[ii], nu[ii], mem->reg_hess, nu[ii], mem->RSQrq[ii], 0, 0); - blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); - blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0); + blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); + acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); + blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); + + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0); } // printf("QSR_hat\n"); // blasfeo_print_dmat(nx+nu+1, nx+nu, &work->qp_in->RSQrq[i], 0, 0); - // backup Q + // backup Q blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); // R = L * L^T @@ -656,8 +646,8 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0); // Q = S^T * R^-1 * S + delta*I - blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0); - blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0); + blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0); // blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); @@ -690,9 +680,185 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di } +// TODO: implement proper split! Right now, we basically do everything twice. -// NOTE this only considers the case of (dynamcs) equality constraints (no inequality constraints) -// TODO inequality constraints case +void ocp_nlp_reg_convexify_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_convexify_memory *mem = mem_; + ocp_nlp_reg_convexify_opts *opts = opts_; + + int ii, jj; + + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + + double delta = opts->delta; + + // regularize() without updating mem->rq! + // blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); + + blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0); + + blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0); + blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0); + blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]); + blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0); + blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); + + for (ii = N-1; ii >= 0; --ii) + { + blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0); + // blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0); + + blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); + + blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0); + blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + + // blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + + blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->R, nu[ii]); + acados_eigen_decomposition(nu[ii], mem->R, mem->V, mem->d, mem->e); + + bool needs_regularization = false; + for (jj = 0; jj < nu[ii]; jj++) + if (mem->d[jj] < 1e-10) + needs_regularization = true; + + if (needs_regularization) + { + blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + + blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); + acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); + blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); + + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0); + } + + // backup Q + blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); + + // R = L * L^T + blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0); + // Q = S^T * L^-T + blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0); +// blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0); + + // Q = S^T * R^-1 * S + delta*I + blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0); +// blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + + blasfeo_dgead(nx[ii], nx[ii], -1.0, mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); + + // make symmetric + blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); + + } + + return; +} + + + +void ocp_nlp_reg_convexify_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_convexify_memory *mem = mem_; + ocp_nlp_reg_convexify_opts *opts = opts_; + + int ii, jj; + + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + + double delta = opts->delta; + + // start from original qp and do it all from scratch + for (ii = 0; ii<=N; ii++) + { + blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + } + + blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); + blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0); + + blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0); + blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0); + blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]); + blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0); + blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); + + for (ii = N-1; ii >= 0; --ii) + { + blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0); + blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0); + + // blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); + + blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0); + blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + + blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + + blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->R, nu[ii]); + acados_eigen_decomposition(nu[ii], mem->R, mem->V, mem->d, mem->e); + + bool needs_regularization = false; + for (jj = 0; jj < nu[ii]; jj++) + if (mem->d[jj] < 1e-10) + needs_regularization = true; + + if (needs_regularization) + { + blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + + blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); + acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); + blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); + + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0); + } + + // backup Q + blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); + + // R = L * L^T + blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0); + // Q = S^T * L^-T + blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0); +// blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0); + + // Q = S^T * R^-1 * S + delta*I + blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0); +// blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + + blasfeo_dgead(nx[ii], nx[ii], -1.0, mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); + + // make symmetric + blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); + + } +} + + + +// TODO: check if inequality constraints case is implemented correctly void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_convexify_memory *mem = mem_; @@ -707,58 +873,58 @@ void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims int *nbu = dims->nbu; int *ng = dims->ng; -// printf("\nRSQrq reg\n"); -// for(ii=0; ii<=N; ii++) -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); +// printf("\nRSQrq reg\n"); +// for(ii=0; ii<=N; ii++) +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); - // restore original hessian and gradient - for(ii=0; ii<=N; ii++) - { + // restore original hessian and gradient + for(ii=0; ii<=N; ii++) + { blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); - blasfeo_drowex(nu[ii]+nx[ii], 1.0, &mem->original_RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); - } + blasfeo_drowex(nu[ii]+nx[ii], 1.0, &mem->original_RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + } -// printf("\nRSQrq orig\n"); -// for(ii=0; ii<=N; ii++) -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); +// printf("\nRSQrq orig\n"); +// for(ii=0; ii<=N; ii++) +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); - // compute multipliers of equality constraints + // compute multipliers of equality constraints #if 0 blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0); for(ii=1; iiRSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0); blasfeo_dgemv_n(nx[ss], nx[ss+1], 1.0, mem->BAbt[ss], nu[ss], 0, mem->pi[ss], 0, 1.0, mem->pi[ss-1], 0, mem->pi[ss-1], 0); } #else - // last stage - blasfeo_dveccp(nx[N], mem->rq[N], nu[N], &mem->tmp_nuxM, nu[N]); - blasfeo_daxpy(nbu[N]+nbx[N]+ng[N], -1.0, mem->lam[N], 0, mem->lam[N], nbu[N]+nbx[N]+ng[N], &mem->tmp_nbgM, 0); - blasfeo_dvecad_sp(nbu[N]+nbx[N], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0); - // TODO avoid to multiply by R ??? - blasfeo_dsymv_l(nu[N]+nx[N], 1.0, mem->RSQrq[N], 0, 0, mem->ux[N], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); - blasfeo_dgemv_n(nx[N], ng[N], 1.0, mem->DCt[N], nu[N], 0, &mem->tmp_nbgM, nbu[N]+nbx[N], 1.0, &mem->tmp_nuxM, nu[N], &mem->tmp_nuxM, nu[N]); - blasfeo_dveccp(nx[N], &mem->tmp_nuxM, nu[N], mem->pi[N-1], 0); - - // middle stages - for(ii=0; iirq[N-1-ii], nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); - blasfeo_daxpy(nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], -1.0, mem->lam[N-1-ii], 0, mem->lam[N-1-ii], nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], &mem->tmp_nbgM, 0); - blasfeo_dvecad_sp(nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N-1-ii], &mem->tmp_nuxM, 0); - // TODO avoid to multiply by R ??? - blasfeo_dsymv_l(nu[N-1-ii]+nx[N-1-ii], 1.0, mem->RSQrq[N-1-ii], 0, 0, mem->ux[N-1-ii], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); - blasfeo_dgemv_n(nx[N-1-ii], nx[N-ii], 1.0, mem->BAbt[N-1-ii], nu[N-1-ii], 0, mem->pi[N-1-ii], 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); - blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); - blasfeo_dveccp(nx[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii], mem->pi[N-2-ii], 0); - } + // last stage + blasfeo_dveccp(nx[N], mem->rq[N], nu[N], &mem->tmp_nuxM, nu[N]); + blasfeo_daxpy(nbu[N]+nbx[N]+ng[N], -1.0, mem->lam[N], 0, mem->lam[N], nbu[N]+nbx[N]+ng[N], &mem->tmp_nbgM, 0); + blasfeo_dvecad_sp(nbu[N]+nbx[N], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0); + // TODO avoid to multiply by R ??? + blasfeo_dsymv_l(nu[N]+nx[N], 1.0, mem->RSQrq[N], 0, 0, mem->ux[N], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); + blasfeo_dgemv_n(nx[N], ng[N], 1.0, mem->DCt[N], nu[N], 0, &mem->tmp_nbgM, nbu[N]+nbx[N], 1.0, &mem->tmp_nuxM, nu[N], &mem->tmp_nuxM, nu[N]); + blasfeo_dveccp(nx[N], &mem->tmp_nuxM, nu[N], mem->pi[N-1], 0); + + // middle stages + for(ii=0; iirq[N-1-ii], nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_daxpy(nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], -1.0, mem->lam[N-1-ii], 0, mem->lam[N-1-ii], nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], &mem->tmp_nbgM, 0); + blasfeo_dvecad_sp(nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N-1-ii], &mem->tmp_nuxM, 0); + // TODO avoid to multiply by R ??? + blasfeo_dsymv_l(nu[N-1-ii]+nx[N-1-ii], 1.0, mem->RSQrq[N-1-ii], 0, 0, mem->ux[N-1-ii], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); + blasfeo_dgemv_n(nx[N-1-ii], nx[N-ii], 1.0, mem->BAbt[N-1-ii], nu[N-1-ii], 0, mem->pi[N-1-ii], 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_dveccp(nx[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii], mem->pi[N-2-ii], 0); + } #endif return; @@ -791,6 +957,9 @@ void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_pi_ptr = &ocp_nlp_reg_convexify_memory_set_pi_ptr; config->memory_set_lam_ptr = &ocp_nlp_reg_convexify_memory_set_lam_ptr; // functions - config->regularize_hessian = &ocp_nlp_reg_convexify_regularize_hessian; + // TODO: fix split + config->regularize = &ocp_nlp_reg_convexify_regularize; + config->regularize_lhs = &ocp_nlp_reg_convexify_regularize_lhs; + config->regularize_rhs = &ocp_nlp_reg_convexify_regularize_rhs; config->correct_dual_sol = &ocp_nlp_reg_convexify_correct_dual_sol; } diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.c b/acados/ocp_nlp/ocp_nlp_reg_mirror.c index 468a143e11..177ebef208 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_mirror.c +++ b/acados/ocp_nlp/ocp_nlp_reg_mirror.c @@ -267,7 +267,7 @@ void ocp_nlp_reg_mirror_memory_set(void *config_, ocp_nlp_reg_dims *dims, void * * functions ************************************************/ -void ocp_nlp_reg_mirror_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +void ocp_nlp_reg_mirror_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_mirror_memory *mem = (ocp_nlp_reg_mirror_memory *) mem_; ocp_nlp_reg_mirror_opts *opts = opts_; @@ -292,6 +292,19 @@ void ocp_nlp_reg_mirror_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, +void ocp_nlp_reg_mirror_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_mirror_regularize(config, dims, opts_, mem_); +} + + + +void ocp_nlp_reg_mirror_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + void ocp_nlp_reg_mirror_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { return; @@ -324,6 +337,8 @@ void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_pi_ptr = &ocp_nlp_reg_mirror_memory_set_pi_ptr; config->memory_set_lam_ptr = &ocp_nlp_reg_mirror_memory_set_lam_ptr; // functions - config->regularize_hessian = &ocp_nlp_reg_mirror_regularize_hessian; + config->regularize = &ocp_nlp_reg_mirror_regularize; + config->regularize_rhs = &ocp_nlp_reg_mirror_regularize_rhs; + config->regularize_lhs = &ocp_nlp_reg_mirror_regularize_lhs; config->correct_dual_sol = &ocp_nlp_reg_mirror_correct_dual_sol; } diff --git a/acados/ocp_nlp/ocp_nlp_reg_noreg.c b/acados/ocp_nlp/ocp_nlp_reg_noreg.c index 0ad63f2f8d..cc4a749ea3 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_noreg.c +++ b/acados/ocp_nlp/ocp_nlp_reg_noreg.c @@ -180,12 +180,23 @@ void ocp_nlp_reg_noreg_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *m * functions ************************************************/ -void ocp_nlp_reg_noreg_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +void ocp_nlp_reg_noreg_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { return; } +void ocp_nlp_reg_noreg_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + +void ocp_nlp_reg_noreg_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + void ocp_nlp_reg_noreg_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { return; @@ -218,7 +229,9 @@ void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_pi_ptr = &ocp_nlp_reg_noreg_memory_set_pi_ptr; config->memory_set_lam_ptr = &ocp_nlp_reg_noreg_memory_set_lam_ptr; // functions - config->regularize_hessian = &ocp_nlp_reg_noreg_regularize_hessian; + config->regularize = &ocp_nlp_reg_noreg_regularize; + config->regularize_lhs = &ocp_nlp_reg_noreg_regularize_lhs; + config->regularize_rhs = &ocp_nlp_reg_noreg_regularize_rhs; config->correct_dual_sol = &ocp_nlp_reg_noreg_correct_dual_sol; } diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.c b/acados/ocp_nlp/ocp_nlp_reg_project.c index 5ae248abc0..34b687624e 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project.c @@ -267,7 +267,7 @@ void ocp_nlp_reg_project_memory_set(void *config_, ocp_nlp_reg_dims *dims, void * functions ************************************************/ -void ocp_nlp_reg_project_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +void ocp_nlp_reg_project_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_project_memory *mem = (ocp_nlp_reg_project_memory *) mem_; ocp_nlp_reg_project_opts *opts = opts_; @@ -290,6 +290,17 @@ void ocp_nlp_reg_project_regularize_hessian(void *config, ocp_nlp_reg_dims *dims } +void ocp_nlp_reg_project_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_project_regularize(config, dims, opts_, mem_); +} + + +void ocp_nlp_reg_project_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + void ocp_nlp_reg_project_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { @@ -323,7 +334,9 @@ void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_pi_ptr = &ocp_nlp_reg_project_memory_set_pi_ptr; config->memory_set_lam_ptr = &ocp_nlp_reg_project_memory_set_lam_ptr; // functions - config->regularize_hessian = &ocp_nlp_reg_project_regularize_hessian; + config->regularize = &ocp_nlp_reg_project_regularize; + config->regularize_rhs = &ocp_nlp_reg_project_regularize_rhs; + config->regularize_lhs = &ocp_nlp_reg_project_regularize_lhs; config->correct_dual_sol = &ocp_nlp_reg_project_correct_dual_sol; } diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index 3073e6036d..8cb03169ce 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -329,7 +329,7 @@ void ocp_nlp_reg_project_reduc_hess_memory_set(void *config_, ocp_nlp_reg_dims * * functions ************************************************/ -void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +void ocp_nlp_reg_project_reduc_hess_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) mem_; ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; @@ -445,7 +445,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg } } } - + pivot = BLASFEO_DMATEL(L3, jj, jj); if ((pivotmin_pivot) & (pivot>-opts->min_pivot)) { @@ -466,7 +466,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg } } - // apply shur to P + // apply schur to P blasfeo_dgecp(nx[ss], nx[ss], L, nu[ss], nu[ss], P, 0, 0); if(do_reg) // if(0) @@ -521,6 +521,18 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg } +void ocp_nlp_reg_project_reduc_hess_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_project_reduc_hess_regularize(config, dims, opts_, mem_); +} + +void ocp_nlp_reg_project_reduc_hess_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + + void ocp_nlp_reg_project_reduc_hess_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { @@ -554,7 +566,9 @@ void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config config->memory_set_pi_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr; config->memory_set_lam_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_lam_ptr; // functions - config->regularize_hessian = &ocp_nlp_reg_project_reduc_hess_regularize_hessian; + config->regularize = &ocp_nlp_reg_project_reduc_hess_regularize; + config->regularize_rhs = &ocp_nlp_reg_project_reduc_hess_regularize_rhs; + config->regularize_lhs = &ocp_nlp_reg_project_reduc_hess_regularize_lhs; config->correct_dual_sol = &ocp_nlp_reg_project_reduc_hess_correct_dual_sol; } diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index a3f1bd5682..b2f9d6d6ef 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -591,7 +591,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // regularize Hessian acados_tic(&timer1); - config->regularize->regularize_hessian(config->regularize, dims->regularize, + config->regularize->regularize(config->regularize, dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); mem->time_reg += acados_toc(&timer1); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index e3902416f2..c10bcbd8e1 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -430,7 +430,7 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim acados_timer timer1; ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - // ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; + ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; ocp_nlp_workspace *nlp_work = work->nlp_work; @@ -450,13 +450,26 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim ocp_nlp_initialize_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); /* SQP body */ - // linearizate NLP and update QP matrices + // linearize NLP and update QP matrices acados_tic(&timer1); ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); mem->time_lin += acados_toc(&timer1); + + if (opts->rti_phase == 1) + { + // regularize Hessian + acados_tic(&timer1); + config->regularize->regularize_lhs(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + // condense lhs + qp_solver->condense_lhs(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + } #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); @@ -490,11 +503,22 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); - // regularize Hessian - acados_tic(&timer1); - config->regularize->regularize_hessian(config->regularize, - dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); - mem->time_reg += acados_toc(&timer1); + if (opts->rti_phase == 2) + { + // finish regularization + acados_tic(&timer1); + config->regularize->regularize_rhs(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + } + else + { + // full regularization + acados_tic(&timer1); + config->regularize->regularize(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + } if (nlp_opts->print_level > 0) { printf("\n------- qp_in --------\n"); @@ -510,9 +534,18 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * // solve qp acados_tic(&timer1); - qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, - nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, - nlp_mem->qp_solver_mem, nlp_work->qp_work); + if (opts->rti_phase == 0) + { + qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + } + else // just feedback + { + qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + } mem->time_qp_sol += acados_toc(&timer1); @@ -527,7 +560,6 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); mem->time_reg += acados_toc(&timer1); - // TODO move into QP solver memory ??? qp_info *qp_info_; ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; @@ -539,14 +571,8 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * work->qp_res, work->qp_res_ws); ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+2)); -// printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, -// inf_norm_qp_res[0], inf_norm_qp_res[1], -// inf_norm_qp_res[2], inf_norm_qp_res[3]); } - // printf("\n------- qp_out (sqp iter %d) ---------\n", sqp_iter); - // print_ocp_qp_out(nlp_mem->qp_out); - // exit(1); // save statistics mem->stat[mem->stat_n*1+0] = qp_status; @@ -578,12 +604,6 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * // update variables ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); - // ocp_nlp_dims_print(nlp_out->dims); - // ocp_nlp_out_print(nlp_out); - // exit(1); - - // print_ocp_qp_in(mem->qp_in); - mem->status = ACADOS_SUCCESS; } @@ -604,8 +624,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_sqp_rti_workspace *work = work_; ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); - ocp_nlp_sqp_rti_opts *nlp_opts = opts_; - int rti_phase = nlp_opts->rti_phase; + int rti_phase = opts->rti_phase; switch(rti_phase) { diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index 5aeb33c28a..22bd29f5ba 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -94,7 +94,8 @@ typedef struct void (*memory_get)(void *config, void *mem, const char *field, void* value); acados_size_t (*workspace_calculate_size)(void *dims, void *opts); int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); - int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*condense_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*condense_lhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); } ocp_qp_xcond_config; diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index c8bc149cde..e012848104 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -465,6 +465,7 @@ int ocp_qp_full_condensing(void *qp_in_, void *fcond_qp_in_, void *opts_, void * //d_ocp_qp_print(mem->red_qp->dim, mem->red_qp); //exit(1); + // TODO: remove this flag and instead call the different functions // convert to dense qp structure if (opts->cond_hess == 0) { @@ -484,8 +485,36 @@ int ocp_qp_full_condensing(void *qp_in_, void *fcond_qp_in_, void *opts_, void * } +int ocp_qp_full_condensing_condense_lhs(void *qp_in_, void *fcond_qp_in_, void *opts_, void *mem_, void *work_) +{ + ocp_qp_in *qp_in = qp_in_; + dense_qp_in *fcond_qp_in = fcond_qp_in_; + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + acados_timer timer; + + // save pointer to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + + // start timer + acados_tic(&timer); + + // reduce eq constr DOF + d_ocp_qp_reduce_eq_dof_lhs(qp_in, mem->red_qp, opts->hpipm_red_opts, mem->hpipm_red_work); + + // condense Hessian + d_cond_qp_cond_lhs(mem->red_qp, fcond_qp_in, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + // stop timer + mem->time_qp_xcond = acados_toc(&timer); + + return ACADOS_SUCCESS; +} + -int ocp_qp_full_condensing_rhs(void *qp_in_, void *fcond_qp_in_, void *opts_, void *mem_, void *work_) + +int ocp_qp_full_condensing_condense_rhs(void *qp_in_, void *fcond_qp_in_, void *opts_, void *mem_, void *work_) { ocp_qp_in *qp_in = qp_in_; dense_qp_in *fcond_qp_in = fcond_qp_in_; @@ -501,19 +530,20 @@ int ocp_qp_full_condensing_rhs(void *qp_in_, void *fcond_qp_in_, void *opts_, vo mem->ptr_qp_in = qp_in; // 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_reduce_eq_dof_rhs(qp_in, mem->red_qp, opts->hpipm_red_opts, mem->hpipm_red_work); // condense gradient only d_cond_qp_cond_rhs(mem->red_qp, fcond_qp_in, opts->hpipm_cond_opts, mem->hpipm_cond_work); // stop timer - mem->time_qp_xcond = acados_toc(&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) { dense_qp_out *fcond_qp_out = fcond_qp_out_; @@ -568,7 +598,8 @@ void ocp_qp_full_condensing_config_initialize_default(void *config_) config->memory_get = &ocp_qp_full_condensing_memory_get; config->workspace_calculate_size = &ocp_qp_full_condensing_workspace_calculate_size; config->condensing = &ocp_qp_full_condensing; - config->condensing_rhs = &ocp_qp_full_condensing_rhs; + config->condense_rhs = &ocp_qp_full_condensing_condense_rhs; + config->condense_lhs = &ocp_qp_full_condensing_condense_lhs; config->expansion = &ocp_qp_full_expansion; return; diff --git a/acados/ocp_qp/ocp_qp_full_condensing.h b/acados/ocp_qp/ocp_qp_full_condensing.h index d23e658b48..39ca81c09a 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/acados/ocp_qp/ocp_qp_full_condensing.h @@ -103,6 +103,10 @@ acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void * // int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work); // +int ocp_qp_full_condensing_condense_rhs(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_full_condensing_condense_lhs(void *in, void *out, void *opts, void *mem, void *work); +// int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work); // void ocp_qp_full_condensing_config_initialize_default(void *config_); diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index ac8276a3a0..b0649c0b4e 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -483,7 +483,6 @@ int ocp_qp_partial_condensing(void *qp_in_, void *pcond_qp_in_, void *opts_, voi //exit(1); // convert to partially condensed qp structure - // TODO only if N2red_qp, pcond_qp_in, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); // stop timer @@ -492,9 +491,34 @@ int ocp_qp_partial_condensing(void *qp_in_, void *pcond_qp_in_, void *opts_, voi return ACADOS_SUCCESS; } +int ocp_qp_partial_condensing_condense_lhs(void *qp_in_, void *pcond_qp_in_, void *opts_, void *mem_, void *work) +{ + ocp_qp_in *qp_in = qp_in_; + ocp_qp_in *pcond_qp_in = pcond_qp_in_; + ocp_qp_partial_condensing_opts *opts = opts_; + ocp_qp_partial_condensing_memory *mem = mem_; + acados_timer timer; + + assert(opts->N2 == opts->N2_bkp); + + // save pointers to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + mem->ptr_pcond_qp_in = pcond_qp_in; -int ocp_qp_partial_condensing_rhs(void *qp_in_, void *pcond_qp_in_, void *opts_, void *mem_, void *work) + acados_tic(&timer); + + d_ocp_qp_reduce_eq_dof_lhs(qp_in, mem->red_qp, opts->hpipm_red_opts, mem->hpipm_red_work); + d_part_cond_qp_cond_lhs(mem->red_qp, pcond_qp_in, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + + mem->time_qp_xcond = acados_toc(&timer); + + return ACADOS_SUCCESS; +} + + + +int ocp_qp_partial_condensing_condense_rhs(void *qp_in_, void *pcond_qp_in_, void *opts_, void *mem_, void *work) { ocp_qp_in *qp_in = qp_in_; ocp_qp_in *pcond_qp_in = pcond_qp_in_; @@ -513,10 +537,9 @@ int ocp_qp_partial_condensing_rhs(void *qp_in_, void *pcond_qp_in_, void *opts_, mem->ptr_pcond_qp_in = pcond_qp_in; // 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_reduce_eq_dof_rhs(qp_in, mem->red_qp, opts->hpipm_red_opts, mem->hpipm_red_work); // convert to partially condensed qp structure - // TODO only if N2red_qp, pcond_qp_in, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); // stop timer @@ -574,7 +597,8 @@ void ocp_qp_partial_condensing_config_initialize_default(void *config_) config->memory_get = &ocp_qp_partial_condensing_memory_get; config->workspace_calculate_size = &ocp_qp_partial_condensing_workspace_calculate_size; config->condensing = &ocp_qp_partial_condensing; - config->condensing_rhs = &ocp_qp_partial_condensing_rhs; + config->condense_lhs = &ocp_qp_partial_condensing_condense_lhs; + config->condense_rhs = &ocp_qp_partial_condensing_condense_rhs; config->expansion = &ocp_qp_partial_expansion; return; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.h b/acados/ocp_qp/ocp_qp_partial_condensing.h index 844f6048fe..16faa14c1d 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -107,6 +107,10 @@ acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, voi // int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work); // +int ocp_qp_partial_condensing_condense_lhs(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_partial_condensing_condense_rhs(void *in, void *out, void *opts, void *mem, void *work); +// int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work); // void ocp_qp_partial_condensing_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 ed5b29f385..e8c1837d89 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -485,7 +485,7 @@ static void cast_workspace(void *config_, ocp_qp_xcond_solver_dims *dims, * functions ************************************************/ -int ocp_qp_xcond_solver(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, +int ocp_qp_xcond_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_) { ocp_qp_xcond_solver_config *config = config_; @@ -535,6 +535,88 @@ int ocp_qp_xcond_solver(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in +int ocp_qp_xcond_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_) +{ + ocp_qp_xcond_solver_config *config = config_; + // qp_solver_config *qp_solver = config->qp_solver; + ocp_qp_xcond_config *xcond = config->xcond; + + qp_info *info = (qp_info *) qp_out->misc; + acados_timer tot_timer, cond_timer; + acados_tic(&tot_timer); + + // cast data structures + ocp_qp_xcond_solver_opts *opts = opts_; + ocp_qp_xcond_solver_memory *memory = mem_; + ocp_qp_xcond_solver_workspace *work = work_; + + // cast workspace + cast_workspace(config_, dims, opts, memory, work); + + int solver_status = ACADOS_SUCCESS; + + // condensing + acados_tic(&cond_timer); + xcond->condense_lhs(qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + info->condensing_time = acados_toc(&cond_timer); + + info->total_time = acados_toc(&tot_timer); + + return solver_status; +} + + +int ocp_qp_xcond_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_) +{ + ocp_qp_xcond_solver_config *config = config_; + qp_solver_config *qp_solver = config->qp_solver; + ocp_qp_xcond_config *xcond = config->xcond; + + qp_info *info = (qp_info *) qp_out->misc; + acados_timer tot_timer, cond_timer; + acados_tic(&tot_timer); + + // cast data structures + ocp_qp_xcond_solver_opts *opts = opts_; + ocp_qp_xcond_solver_memory *memory = mem_; + ocp_qp_xcond_solver_workspace *work = work_; + + // cast workspace + cast_workspace(config_, dims, opts, memory, work); + + int solver_status = ACADOS_SUCCESS; + + // condensing + acados_tic(&cond_timer); + xcond->condense_rhs(qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + info->condensing_time += acados_toc(&cond_timer); + + // solve qp + solver_status = qp_solver->evaluate(qp_solver, memory->xcond_qp_in, memory->xcond_qp_out, + opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); + + // expansion + acados_tic(&cond_timer); + xcond->expansion(memory->xcond_qp_out, qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + info->condensing_time += acados_toc(&cond_timer); + + // output qp info + qp_info *info_mem; + xcond->memory_get(xcond, memory->xcond_memory, "qp_out_info", &info_mem); + + info->total_time += acados_toc(&tot_timer); + info->solve_QP_time = info_mem->solve_QP_time; + info->interface_time = info_mem->interface_time; + info->num_iter = info_mem->num_iter; + info->t_computed = info_mem->t_computed; + + return solver_status; +} + + + 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 *opts_, void *mem_, void *work_) { @@ -557,7 +639,7 @@ void ocp_qp_xcond_solver_eval_sens(void *config_, ocp_qp_xcond_solver_dims *dims // condensing // acados_tic(&cond_timer); - xcond->condensing_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->condense_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); // info->condensing_time = acados_toc(&cond_timer); // qp evaluate sensitivity @@ -603,7 +685,9 @@ void ocp_qp_xcond_solver_config_initialize_default(void *config_) config->solver_get = &ocp_qp_xcond_solver_get; config->memory_reset = &ocp_qp_xcond_solver_memory_reset; // TODO: unused? config->workspace_calculate_size = &ocp_qp_xcond_solver_workspace_calculate_size; - config->evaluate = &ocp_qp_xcond_solver; + 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; return; diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.h b/acados/ocp_qp/ocp_qp_xcond_solver.h index 393ff24703..da4bf9aeaa 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.h +++ b/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -94,6 +94,8 @@ typedef struct void (*memory_reset)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); 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); qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver ocp_qp_xcond_config *xcond; @@ -139,7 +141,12 @@ acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_ /* config */ // -int ocp_qp_xcond_solver(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 ocp_qp_xcond_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_); + +int ocp_qp_xcond_cond_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 ocp_qp_xcond_cond_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 ocp_qp_xcond_solver_config_initialize_default(void *config_); diff --git a/docs/algorithm_overview/index.md b/docs/algorithm_overview/index.md index db192c13ee..1ab8261c22 100644 --- a/docs/algorithm_overview/index.md +++ b/docs/algorithm_overview/index.md @@ -23,7 +23,7 @@ The following steps are carried out: - 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`) -- call the regularization module (`regularize_hessian`, see [`regularize_method`](https://docs.acados.org/python_interface/index.html?highlight=regularize#acados_template.acados_ocp_options.AcadosOcpOptions.regularize_method)) +- 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)) \ No newline at end of file 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 b19910adfb..3be5b357af 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 @@ -109,12 +109,20 @@ def main(): simX = np.ndarray((N+1, nx)) simU = np.ndarray((N, nu)) - # call SQP_RTI solver in the loop: tol = 1e-6 + SPLIT_RTI = True for i in range(20): - status = ocp_solver.solve() + 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.custom_update(np.array([])) ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") residuals = ocp_solver.get_residuals() @@ -137,7 +145,7 @@ def main(): cost = ocp_solver.get_cost() print("cost function value of solution = ", cost) - PRINT_QP = True + PRINT_QP = False range_without_terminal = [0, 1, N-2, N-1] range_with_terminal = [0, 1, N-1, N] diff --git a/examples/c/regularization.c b/examples/c/regularization.c index 7b3d7d7c8f..4e2bfa17ee 100644 --- a/examples/c/regularization.c +++ b/examples/c/regularization.c @@ -136,7 +136,7 @@ int main() blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], RSQrq+ii, 0, 0); // regularization - config->regularize_hessian(config, dims, opts, memory); + config->regularize(config, dims, opts, memory); printf("\nafter regularization\n\n"); for(ii=0; ii<=N; ii++) From 37e17d31890ab54e5a855f1fe787fbf2f5d43bdb Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 16 Feb 2024 11:19:54 +0100 Subject: [PATCH 04/16] Work on core for speedups (#888) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - **Cast highest level workspace (SQP/SQP-RTI) only in precompute for more speed**: This still allows to reuse workspace across modules. This can only break if one tries to reuse the acados workspace memory from outside. - **Add CasADi sparsity detection** that is called when creating an external function. The CasADi sparsity struct in the generated functions contains an `int` which always say inputs are sparse, often this is not the case. Thus, we check now for speed! - Added `sim_in_assign_and_advance`, this can be called to avoid calling `calculate_size` twice. Doing this everywhere is rather tedious and does not save very much. - **Breaking**: precompute is now required! On a small benchmark of an RTI with ERK and the cartpole this gives me >20% speedup, from 78μs to 62μs. --- acados/ocp_nlp/ocp_nlp_common.c | 4 +- acados/ocp_nlp/ocp_nlp_common.h | 1 + acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 53 +++--- acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 22 +-- acados/ocp_nlp/ocp_nlp_dynamics_cont.h | 2 + acados/ocp_nlp/ocp_nlp_sqp.c | 7 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 8 +- acados/ocp_qp/ocp_qp_xcond_solver.c | 7 +- acados/sim/sim_common.c | 36 ++++ acados/sim/sim_common.h | 2 + acados/sim/sim_erk_integrator.c | 116 ++++++------ acados/sim/sim_erk_integrator.h | 12 +- acados/utils/external_function_generic.c | 216 +++++++++++++++-------- acados/utils/external_function_generic.h | 4 + examples/c/simple_dae_example.c | 1 + test/ocp_nlp/test_chain.cpp | 4 +- 16 files changed, 301 insertions(+), 194 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 3deb79a187..0b0df6e70a 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1881,7 +1881,6 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } else { - // qp solver work->qp_work = (void *) c_ptr; c_ptr += qp_solver->workspace_calculate_size(qp_solver, dims->qp_solver, @@ -1910,7 +1909,7 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } - assert((char *) work + ocp_nlp_workspace_calculate_size(config, dims, opts) >= c_ptr); + assert((char *) work + mem->workspace_size >= c_ptr); return work; } @@ -1953,6 +1952,7 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->dynamics[i]->memory_set_RSQrq_ptr(nlp_mem->qp_in->RSQrq+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_dzduxt_ptr(nlp_mem->dzduxt+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_sim_guess_ptr(nlp_mem->sim_guess+i, nlp_mem->set_sim_guess+i, nlp_mem->dynamics[i]); + // NOTE: no z at terminal stage, since dynamics modules dont compute it. config->dynamics[i]->memory_set_z_alg_ptr(nlp_mem->z_alg+i, nlp_mem->dynamics[i]); } diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 9482413773..5474e8f0b3 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -354,6 +354,7 @@ typedef struct ocp_nlp_memory bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables struct blasfeo_dvec *sim_guess; + acados_size_t workspace_size; } ocp_nlp_memory; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index c5c72bb21d..dc97369ecd 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -1192,12 +1192,18 @@ acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config_, vo size += sizeof(ocp_nlp_constraints_bgh_workspace); - size += 1 * blasfeo_memsize_dmat(nu+nx, nu+nx); // tmp_nv_nv - size += 1 * blasfeo_memsize_dmat(nz, nh); // tmp_nz_nh - size += 1 * blasfeo_memsize_dmat(nz, nx+nu); // tmp_nz_nv - size += 1 * blasfeo_memsize_dmat(nx+nu, nh); // tmp_nv_nh - size += 1 * blasfeo_memsize_dmat(nz, nz); // hess_z - size += 1 * blasfeo_memsize_dvec(nh); // tmp_nh + if (nz > 0) + { + size += 1 * blasfeo_memsize_dmat(nz, nh); // tmp_nz_nh + size += 1 * blasfeo_memsize_dmat(nz, nz); // hess_z + size += 1 * blasfeo_memsize_dmat(nz, nx+nu); // tmp_nz_nv + } + if (nh > 0) + { + size += 1 * blasfeo_memsize_dmat(nu+nx, nu+nx); // tmp_nv_nv + size += 1 * blasfeo_memsize_dmat(nx+nu, nh); // tmp_nv_nh + size += 1 * blasfeo_memsize_dvec(nh); // tmp_nh + } size += 1 * blasfeo_memsize_dvec(nb+ng+nh+ns); // tmp_ni size += 1 * 64; // blasfeo_mem align @@ -1227,23 +1233,24 @@ static void ocp_nlp_constraints_bgh_cast_workspace(void *config_, void *dims_, v // blasfeo_mem align align_char_to(64, &c_ptr); - // tmp_nv_nv - assign_and_advance_blasfeo_dmat_mem(nu+nx, nu+nx, &work->tmp_nv_nv, &c_ptr); - - // tmp_nz_nh - assign_and_advance_blasfeo_dmat_mem(nz, nh, &work->tmp_nz_nh, &c_ptr); - - // tmp_nv_nh - assign_and_advance_blasfeo_dmat_mem(nx + nu, nh, &work->tmp_nv_nh, &c_ptr); - - // hess_z - assign_and_advance_blasfeo_dmat_mem(nz, nz, &work->hess_z, &c_ptr); - - // tmp_nz_nv - assign_and_advance_blasfeo_dmat_mem(nz, nx+nu, &work->tmp_nz_nv, &c_ptr); - - // tmp_nh - assign_and_advance_blasfeo_dvec_mem(nh, &work->tmp_nh, &c_ptr); + if (nz > 0) + { + // tmp_nz_nh + assign_and_advance_blasfeo_dmat_mem(nz, nh, &work->tmp_nz_nh, &c_ptr); + // hess_z + assign_and_advance_blasfeo_dmat_mem(nz, nz, &work->hess_z, &c_ptr); + // tmp_nz_nv + assign_and_advance_blasfeo_dmat_mem(nz, nx+nu, &work->tmp_nz_nv, &c_ptr); + } + if (nh > 0) + { + // tmp_nv_nv + assign_and_advance_blasfeo_dmat_mem(nu+nx, nu+nx, &work->tmp_nv_nv, &c_ptr); + // tmp_nv_nh + assign_and_advance_blasfeo_dmat_mem(nx + nu, nh, &work->tmp_nv_nh, &c_ptr); + // tmp_nh + assign_and_advance_blasfeo_dvec_mem(nh, &work->tmp_nh, &c_ptr); + } // tmp_ni assign_and_advance_blasfeo_dvec_mem(nb+ng+nh+ns, &work->tmp_ni, &c_ptr); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 53485bc002..777939673f 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -635,8 +635,9 @@ acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config_, void static void ocp_nlp_dynamics_cont_cast_workspace(void *config_, void *dims_, void *opts_, - void *work_) + void *work_, void *mem_) { + ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_config *config = config_; ocp_nlp_dynamics_cont_dims *dims = dims_; ocp_nlp_dynamics_cont_opts *opts = opts_; @@ -650,8 +651,8 @@ static void ocp_nlp_dynamics_cont_cast_workspace(void *config_, void *dims_, voi int nu = dims->nu; // sim in - work->sim_in = sim_in_assign(config->sim_solver, dims->sim, c_ptr); - c_ptr += sim_in_calculate_size(config->sim_solver, dims->sim); + sim_in_assign_and_advance(config->sim_solver, dims->sim, &work->sim_in, &c_ptr); + // sim out work->sim_out = sim_out_assign(config->sim_solver, dims->sim, c_ptr); c_ptr += sim_out_calculate_size(config->sim_solver, dims->sim); @@ -665,7 +666,7 @@ static void ocp_nlp_dynamics_cont_cast_workspace(void *config_, void *dims_, voi // hess assign_and_advance_blasfeo_dmat_mem(nu+nx, nu+nx, &work->hess, &c_ptr); - assert((char *) work + ocp_nlp_dynamics_cont_workspace_calculate_size(config, dims, opts) >= c_ptr); + assert((char *) work + mem->workspace_size >= c_ptr); return; } @@ -764,7 +765,7 @@ void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims_, void *model_, void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void *model_, void *opts_, void *mem_, void *work_) { - ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_); + ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); ocp_nlp_dynamics_config *config = config_; ocp_nlp_dynamics_cont_dims *dims = dims_; @@ -896,7 +897,7 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *mem_, void *work_) { - ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_); + ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); ocp_nlp_dynamics_config *config = config_; ocp_nlp_dynamics_cont_dims *dims = dims_; @@ -968,13 +969,14 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims_, void *model_, void *opts_, void *mem_, void *work_) { - ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_); - + ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_config *config = config_; + mem->workspace_size = ocp_nlp_dynamics_cont_workspace_calculate_size(config, dims_, opts_); + ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); + // ocp_nlp_dynamics_cont_dims *dims = dims_; ocp_nlp_dynamics_cont_opts *opts = opts_; ocp_nlp_dynamics_cont_workspace *work = work_; - ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_cont_model *model = model_; work->sim_in->model = model->sim_model; work->sim_in->T = model->T; @@ -982,7 +984,7 @@ int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims_, void *model_, v // call integrator int status = config->sim_solver->precompute(config->sim_solver, work->sim_in, work->sim_out, opts->sim_solver, mem->sim_solver, work->sim_solver); - + config->sim_solver->memory_set_to_zero(config->sim_solver, work->sim_in->dims, opts->sim_solver, mem->sim_solver, "guesses"); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 3afdc9f4ed..186c19de96 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -121,6 +121,8 @@ typedef struct // struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed void *sim_solver; // sim solver memory + acados_size_t workspace_size; + } ocp_nlp_dynamics_cont_memory; // diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index b2f9d6d6ef..1b25dfed94 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -468,7 +468,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_res *nlp_res = nlp_mem->nlp_res; ocp_nlp_sqp_workspace *work = work_; - ocp_nlp_sqp_cast_workspace(config, dims, opts, mem, work); + // ocp_nlp_sqp_cast_workspace(config, dims, opts, mem, work); ocp_nlp_workspace *nlp_work = work->nlp_work; ocp_qp_in *qp_in = nlp_mem->qp_in; @@ -498,7 +498,6 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // set number of threads omp_set_num_threads(opts->nlp_opts->num_threads); #endif - ocp_nlp_alias_memory_to_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); // if (opts->initialize_t_slacks > 0) @@ -1017,6 +1016,8 @@ int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_ ocp_nlp_out *nlp_out = nlp_out_; ocp_nlp_memory *nlp_mem = mem->nlp_mem; + nlp_mem->workspace_size = ocp_nlp_workspace_calculate_size(config, dims, opts->nlp_opts); + ocp_nlp_sqp_workspace *work = work_; ocp_nlp_sqp_cast_workspace(config, dims, opts, mem, work); ocp_nlp_workspace *nlp_work = work->nlp_work; @@ -1040,7 +1041,7 @@ void ocp_nlp_sqp_eval_param_sens(void *config_, void *dims_, void *opts_, void * ocp_nlp_out *sens_nlp_out = sens_nlp_out_; ocp_nlp_sqp_workspace *work = work_; - ocp_nlp_sqp_cast_workspace(config, dims, opts, mem, work); + // ocp_nlp_sqp_cast_workspace(config, dims, opts, mem, work); ocp_nlp_workspace *nlp_work = work->nlp_work; d_ocp_qp_copy_all(nlp_mem->qp_in, work->tmp_qp_in); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index c10bcbd8e1..b3037025ad 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -444,8 +444,6 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim omp_set_num_threads(opts->nlp_opts->num_threads); #endif - ocp_nlp_alias_memory_to_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); - // initialize QP ocp_nlp_initialize_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); @@ -622,7 +620,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_in *nlp_in = nlp_in_; ocp_nlp_out *nlp_out = nlp_out_; ocp_nlp_sqp_rti_workspace *work = work_; - ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); + // ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); int rti_phase = opts->rti_phase; @@ -682,6 +680,8 @@ int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_, void *nlp_in_, ocp_nlp_out *nlp_out = nlp_out_; ocp_nlp_memory *nlp_mem = mem->nlp_mem; + nlp_mem->workspace_size = ocp_nlp_workspace_calculate_size(config, dims, opts->nlp_opts); + ocp_nlp_sqp_rti_workspace *work = work_; ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); ocp_nlp_workspace *nlp_work = work->nlp_work; @@ -706,7 +706,7 @@ void ocp_nlp_sqp_rti_eval_param_sens(void *config_, void *dims_, void *opts_, ocp_nlp_out *sens_nlp_out = sens_nlp_out_; ocp_nlp_sqp_rti_workspace *work = work_; - ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); + // ocp_nlp_sqp_rti_cast_workspace(config, dims, opts, mem, work); ocp_nlp_workspace *nlp_work = work->nlp_work; d_ocp_qp_copy_all(nlp_mem->qp_in, work->tmp_qp_in); diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.c b/acados/ocp_qp/ocp_qp_xcond_solver.c index e8c1837d89..6f3cf51cfd 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -300,7 +300,7 @@ acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config_, ocp_qp_xc acados_size_t size = 0; size += sizeof(ocp_qp_xcond_solver_memory); - // set up dimesions of partially condensed qp + // set up dimensions of partially condensed qp void *xcond_qp_dims; xcond->dims_get(xcond, dims->xcond_dims, "xcond_dims", &xcond_qp_dims); @@ -324,7 +324,7 @@ void *ocp_qp_xcond_solver_memory_assign(void *config_, ocp_qp_xcond_solver_dims char *c_ptr = (char *) raw_memory; - // set up dimesions of partially condensed qp + // set up dimensions of partially condensed qp void *xcond_qp_dims; xcond->dims_get(xcond, dims->xcond_dims, "xcond_dims", &xcond_qp_dims); @@ -442,7 +442,7 @@ acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config_, ocp_qp // size += xcond->workspace_calculate_size(dims->orig_dims, opts->xcond_opts); size += xcond->workspace_calculate_size(dims->xcond_dims, opts->xcond_opts); - // set up dimesions of condensed qp + // set up dimensions of condensed qp void *xcond_qp_dims; xcond->dims_get(xcond, dims->xcond_dims, "xcond_dims", &xcond_qp_dims); @@ -462,7 +462,6 @@ static void cast_workspace(void *config_, ocp_qp_xcond_solver_dims *dims, qp_solver_config *qp_solver = config->qp_solver; ocp_qp_xcond_config *xcond = config->xcond; - // set up dimesions of condensed qp void *xcond_qp_dims; xcond->dims_get(xcond, dims->xcond_dims, "xcond_dims", &xcond_qp_dims); diff --git a/acados/sim/sim_common.c b/acados/sim/sim_common.c index a751f0763a..1f94c1049f 100644 --- a/acados/sim/sim_common.c +++ b/acados/sim/sim_common.c @@ -144,6 +144,42 @@ sim_in *sim_in_assign(void *config_, void *dims, void *raw_memory) } +void sim_in_assign_and_advance(void *config_, void *dims, sim_in **sim_in_p, char **c_ptr) +{ + sim_config *config = config_; + + // assign structure itself + *sim_in_p = (sim_in *) *c_ptr; + *c_ptr += sizeof(sim_in); + // shorthand + sim_in *in = (sim_in *) *sim_in_p; + + // align + align_char_to(8, &*c_ptr); + + // assign substructures + in->model = config->model_assign(config, dims, *c_ptr); + *c_ptr += config->model_calculate_size(config, dims); + + // set pointers and dimensions, defaults + in->dims = dims; + int nx, nu, nz; + config->dims_get(config_, dims, "nx", &nx); + config->dims_get(config_, dims, "nu", &nu); + config->dims_get(config_, dims, "nz", &nz); + int NF = nx + nu; + in->identity_seed = false; + + // align + align_char_to(8, c_ptr); + + // assign doubles + assign_and_advance_double(nx, &in->x, c_ptr); + assign_and_advance_double(nu, &in->u, c_ptr); + assign_and_advance_double(nx * NF, &in->S_forw, c_ptr); + assign_and_advance_double(NF, &in->S_adj, c_ptr); +} + int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value) { diff --git a/acados/sim/sim_common.h b/acados/sim/sim_common.h index e351d66f24..6826cc192b 100644 --- a/acados/sim/sim_common.h +++ b/acados/sim/sim_common.h @@ -205,6 +205,8 @@ acados_size_t sim_in_calculate_size(void *config, void *dims); // sim_in *sim_in_assign(void *config, void *dims, void *raw_memory); // +void sim_in_assign_and_advance(void *config, void *dims, sim_in **sim_in_p, char **c_ptr); +// int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value); /* out */ diff --git a/acados/sim/sim_erk_integrator.c b/acados/sim/sim_erk_integrator.c index 8aa023da7d..bab12506c9 100644 --- a/acados/sim/sim_erk_integrator.c +++ b/acados/sim/sim_erk_integrator.c @@ -440,10 +440,11 @@ acados_size_t sim_erk_workspace_calculate_size(void *config_, void *dims_, void -static void *sim_erk_cast_workspace(void *config_, void *dims_, void *opts_, void *raw_memory) +static void *sim_erk_cast_workspace(void *config_, void *dims_, void *opts_, void *raw_memory, void *mem_) { sim_opts *opts = opts_; sim_erk_dims *dims = (sim_erk_dims *) dims_; + sim_erk_memory *mem = mem_; int ns = opts->ns; @@ -478,7 +479,6 @@ static void *sim_erk_cast_workspace(void *config_, void *dims_, void *opts_, voi //assign_and_advance_double((num_steps + 1) * nX, &workspace->out_forw_traj, &c_ptr); work->out_forw_traj = d_ptr; d_ptr += (num_steps+1)*nX; - } else { @@ -518,7 +518,7 @@ static void *sim_erk_cast_workspace(void *config_, void *dims_, void *opts_, voi // update c_ptr c_ptr = (char *) d_ptr; - assert((char *) raw_memory + sim_erk_workspace_calculate_size(config_, dims, opts_) >= c_ptr); + assert((char *) raw_memory + mem->workspace_size >= c_ptr); return (void *) work; } @@ -530,6 +530,8 @@ static void *sim_erk_cast_workspace(void *config_, void *dims_, void *opts_, voi int sim_erk_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_) { + sim_erk_memory *mem = mem_; + mem->workspace_size = sim_erk_workspace_calculate_size(config_, in->dims, opts_); return ACADOS_SUCCESS; } @@ -555,7 +557,7 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo void *dims_ = in->dims; sim_erk_dims *dims = (sim_erk_dims *) dims_; - sim_erk_workspace *work = sim_erk_cast_workspace(config, dims, opts, work_); + sim_erk_workspace *work = sim_erk_cast_workspace(config, dims, opts, work_, mem_); int i, j, s, istep; double a = 0, b = 0; // temp values of A_mat and b_vec @@ -616,6 +618,39 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo ext_fun_arg_t ext_fun_type_out[3]; void *ext_fun_out[3]; + ext_fun_arg_t expl_vde_type_in[4]; + void *expl_vde_in[4]; + ext_fun_arg_t expl_vde_type_out[3]; + void *expl_vde_out[3]; + + int nx_squared_plus_nx = nx * nx + nx; + int nx_times_nu = nx * nu; + + if (opts->sens_forw) + { // simulation + forward sensitivities + expl_vde_type_in[0] = COLMAJ; + expl_vde_in[0] = rhs_forw_in; // x: nx + expl_vde_type_in[1] = COLMAJ; + expl_vde_in[1] = rhs_forw_in + nx; // Sx: nx*nx + expl_vde_type_in[2] = COLMAJ; + expl_vde_in[2] = rhs_forw_in + nx_squared_plus_nx; // Su: nx*nu + expl_vde_type_in[3] = COLMAJ; + expl_vde_in[3] = rhs_forw_in + nx_squared_plus_nx + nx_times_nu; // u: nu + + expl_vde_type_out[0] = COLMAJ; + expl_vde_type_out[1] = COLMAJ; + expl_vde_type_out[2] = COLMAJ; + } + else + { + expl_vde_type_in[0] = COLMAJ; + expl_vde_in[0] = rhs_forw_in; // x: nx + expl_vde_type_in[1] = COLMAJ; + expl_vde_in[1] = rhs_forw_in + nx; // u: nu + + expl_vde_type_out[0] = COLMAJ; + } + erk_model *model = in->model; double timing_ad = 0.0; @@ -660,43 +695,23 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo acados_tic(&timer_ad); if (opts->sens_forw) { // simulation + forward sensitivities - ext_fun_type_in[0] = COLMAJ; - ext_fun_in[0] = rhs_forw_in + 0; // x: nx - ext_fun_type_in[1] = COLMAJ; - ext_fun_in[1] = rhs_forw_in + nx; // Sx: nx*nx - ext_fun_type_in[2] = COLMAJ; - ext_fun_in[2] = rhs_forw_in + nx + nx * nx; // Su: nx*nu - ext_fun_type_in[3] = COLMAJ; - ext_fun_in[3] = rhs_forw_in + nx + nx * nx + nx * nu; // u: nu - - ext_fun_type_out[0] = COLMAJ; - ext_fun_out[0] = K_traj + s * nX + 0; // fun: nx - ext_fun_type_out[1] = COLMAJ; - ext_fun_out[1] = K_traj + s * nX + nx; // Sx: nx*nx - ext_fun_type_out[2] = COLMAJ; - ext_fun_out[2] = K_traj + s * nX + nx + nx * nx; // Su: nx*nu - // forward VDE evaluation - model->expl_vde_for->evaluate(model->expl_vde_for, ext_fun_type_in, ext_fun_in, - ext_fun_type_out, ext_fun_out); + expl_vde_out[0] = K_traj + s * nX; // fun: nx + expl_vde_out[1] = K_traj + s * nX + nx; // Sx: nx*nx + expl_vde_out[2] = K_traj + s * nX + nx_squared_plus_nx; // Su: nx*nu + model->expl_vde_for->evaluate(model->expl_vde_for, expl_vde_type_in, expl_vde_in, + expl_vde_type_out, expl_vde_out); } else { // simulation only - ext_fun_type_in[0] = COLMAJ; - ext_fun_in[0] = rhs_forw_in + 0; // x: nx - ext_fun_type_in[1] = COLMAJ; - ext_fun_in[1] = rhs_forw_in + nx; // u: nu - - ext_fun_type_out[0] = COLMAJ; - ext_fun_out[0] = K_traj + s * nX + 0; // fun: nx - if (model->expl_ode_fun == 0) { printf("sim ERK: expl_ode_fun is not provided. Exiting.\n"); exit(1); } - model->expl_ode_fun->evaluate(model->expl_ode_fun, ext_fun_type_in, ext_fun_in, - ext_fun_type_out, ext_fun_out); // ODE evaluation + expl_vde_out[0] = K_traj + s * nX; // fun: nx + model->expl_ode_fun->evaluate(model->expl_ode_fun, expl_vde_type_in, expl_vde_in, + expl_vde_type_out, expl_vde_out); // ODE evaluation } timing_ad += acados_toc(&timer_ad); } @@ -737,8 +752,6 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo adj_tmp[nx + nu + i] = 0.0; } - // printf("\nnForw=%d nAdj=%d\n", nForw, nAdj); - for (i = 0; i < nu; i++) rhs_adj_in[nForw + nx + i] = u[i]; @@ -783,20 +796,12 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo } } - // TODO(oj): fix this whole file or write from scratch, not really readable :/ acados_tic(&timer_ad); if (!opts->sens_hess) { ext_fun_type_in[0] = COLMAJ; ext_fun_in[0] = rhs_adj_in + 0; // x: nx -// ext_fun_type_in[1] = COLMAJ; -// ext_fun_in[1] = rhs_adj_in + nx; // Sx: nx*nx -// ext_fun_type_in[2] = COLMAJ; -// ext_fun_in[2] = rhs_adj_in + nx + nx * nx; // Su: nx*nu -// ext_fun_type_in[3] = COLMAJ; -// ext_fun_in[3] = rhs_adj_in + nx + nx * nx + nx * nu; // lam: nx -// ext_fun_type_in[4] = COLMAJ; -// ext_fun_in[4] = rhs_adj_in + nx + nx * nx + nx * nu + nx; // u: nu + ext_fun_type_in[1] = COLMAJ; ext_fun_in[1] = rhs_adj_in + nx; // lam: nx ext_fun_type_in[2] = COLMAJ; @@ -804,8 +809,6 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo ext_fun_type_out[0] = COLMAJ; ext_fun_out[0] = adj_traj + s * nAdj + 0; // adj: nx+nu -// ext_fun_type_out[1] = COLMAJ; -// ext_fun_out[1] = adj_traj + s * nAdj + nx + nu; // hess: (nx+nu)*(nx+nu) if (model->expl_vde_adj == 0) { @@ -823,31 +826,19 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo ext_fun_type_in[1] = COLMAJ; ext_fun_in[1] = rhs_adj_in + nx; // Sx: nx*nx ext_fun_type_in[2] = COLMAJ; - ext_fun_in[2] = rhs_adj_in + nx + nx * nx; // Su: nx*nu + ext_fun_in[2] = rhs_adj_in + nx_squared_plus_nx; // Su: nx*nu ext_fun_type_in[3] = COLMAJ; - ext_fun_in[3] = rhs_adj_in + nx + nx * nx + nx * nu; // lam: nx + ext_fun_in[3] = rhs_adj_in + nx_squared_plus_nx + nx_times_nu; // lam: nx ext_fun_type_in[4] = COLMAJ; - ext_fun_in[4] = rhs_adj_in + nx + nx * nx + nx * nu + nx; // u: nu + ext_fun_in[4] = rhs_adj_in + nx_squared_plus_nx + nx_times_nu + nx; // u: nu ext_fun_type_out[0] = COLMAJ; ext_fun_out[0] = adj_traj + s * nAdj + 0; // adj: nx+nu ext_fun_type_out[1] = COLMAJ; ext_fun_out[1] = adj_traj + s * nAdj + nx + nu; // hess: (nx+nu)*(nx+nu) -//printf("\nin\n"); -//d_print_mat(1, nx, ext_fun_in[0], 1); -//d_print_mat(nx, nx, ext_fun_in[1], nx); -//d_print_mat(nx, nu, ext_fun_in[2], nx); -//d_print_mat(1, nx, ext_fun_in[3], 1); -//d_print_mat(1, nu, ext_fun_in[4], 1); - model->expl_ode_hes->evaluate(model->expl_ode_hes, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); - -//printf("\nout\n"); -//d_print_mat(1, nx+nu, ext_fun_out[0], 1); -//d_print_mat(1, (nx+nu)*(nu+nx+1)/2, ext_fun_out[1], nu+nx); - } timing_ad += acados_toc(&timer_ad); } @@ -872,8 +863,8 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo for (int i = j; i < nx + nu; i++) { S_hess_out[i + (nf) *j] = adj_tmp[nx + nu + count_upper]; - S_hess_out[j + (nf) *i] = adj_tmp[nx + nu + count_upper]; // copy to upper part + S_hess_out[j + (nf) *i] = adj_tmp[nx + nu + count_upper]; count_upper++; } } @@ -889,8 +880,7 @@ int sim_erk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo mem->time_ad = out->info->ADtime; mem->time_la = out->info->LAtime; - // return - return 0; // success + return 0; } diff --git a/acados/sim/sim_erk_integrator.h b/acados/sim/sim_erk_integrator.h index fd46cb4d99..b45a2571d1 100644 --- a/acados/sim/sim_erk_integrator.h +++ b/acados/sim/sim_erk_integrator.h @@ -68,19 +68,19 @@ typedef struct typedef struct { - // memory - double time_sim; - double time_ad; - double time_la; + // memory + double time_sim; + double time_ad; + double time_la; + acados_size_t workspace_size; - // workspace structs } sim_erk_memory; typedef struct { - // workspace mem + // workspace mem double *rhs_forw_in; // x + S + p double *K_traj; // (stages*nX) or (steps*stages*nX) for adj diff --git a/acados/utils/external_function_generic.c b/acados/utils/external_function_generic.c index 53a4525873..48c5364e64 100644 --- a/acados/utils/external_function_generic.c +++ b/acados/utils/external_function_generic.c @@ -122,8 +122,6 @@ void external_function_param_generic_assign(external_function_param_generic *fun void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out) { - // TODO somehow check on types ????? - // cast into external generic function external_function_param_generic *fun = self; @@ -192,24 +190,74 @@ static int casadi_nnz(const int *sparsity) } +static int casadi_is_dense(const int *sparsity) +{ + // returns true if casadi sparsity goes through all elements of the matrix as it would for a dense matrix; + // otherwise false. + int idx, jj; + if (sparsity != NULL) + { + const int nrow = sparsity[0]; + const int ncol = sparsity[1]; + const int dense = sparsity[2]; + if ((nrow<=0 )| (ncol<=0)) + return 1; + if (dense) + { + return 1; + } + else + { + // check for casadi fake sparse + const int *idxcol = sparsity + 2; + const int *row = sparsity + ncol + 3; + for (jj = 0; jj < ncol; jj++) + { + if (idxcol[jj + 1] - idxcol[jj] != nrow) + { + // printf("casadi_is_dense: REAL sparse!\n"); + // printf("idxco[jj+1] = %d idxcol[jj] %d nrow %d\n", idxcol[jj + 1], idxcol[jj], nrow); + return 0; + } + for (idx = idxcol[jj]; idx != idxcol[jj + 1]; idx++) + { + if (row[idx]+jj*ncol != idx) + { + // printf("casadi_is_dense: REAL sparse!\n"); + // printf("row[idx] %d != idx = %d, nrow %d ncol %d, jj %d\n", row[idx], idx, nrow, ncol, jj); + return 0; + } + } + } + } + } + // printf("casadi_is_dense: real DENSE!\n"); + + return 1; +} + -static void d_cvt_casadi_to_colmaj(double *in, int *sparsity_in, double *out) + + + +static void d_cvt_casadi_to_colmaj(double *in, int *sparsity_in, double *out, int is_dense) { int ii, jj, idx; int nrow = sparsity_in[0]; int ncol = sparsity_in[1]; - int dense = sparsity_in[2]; if ((nrow<=0 )| (ncol<=0)) return; - if (dense) + if (is_dense) { + // printf("d_cvt_casadi_to_colmaj: dense\n"); for (ii = 0; ii < ncol * nrow; ii++) out[ii] = in[ii]; } else { + // printf("d_cvt_casadi_to_colmaj: sparse\n"); double *ptr = in; int *idxcol = sparsity_in + 2; int *row = sparsity_in + ncol + 3; @@ -232,23 +280,24 @@ static void d_cvt_casadi_to_colmaj(double *in, int *sparsity_in, double *out) -static void d_cvt_colmaj_to_casadi(double *in, double *out, int *sparsity_out) +static void d_cvt_colmaj_to_casadi(double *in, double *out, int *sparsity_out, int is_dense) { int ii, jj, idx; int nrow = sparsity_out[0]; int ncol = sparsity_out[1]; - int dense = sparsity_out[2]; if ((nrow<=0 )| (ncol<=0)) return; - if (dense) + if (is_dense) { + // printf("d_cvt_colmaj_to_casadi: dense\n"); for (ii = 0; ii < ncol * nrow; ii++) out[ii] = in[ii]; } else { + // printf("d_cvt_colmaj_to_casadi: sparse\n"); double *ptr = out; int *idxcol = sparsity_out + 2; int *row = sparsity_out + ncol + 3; @@ -268,19 +317,17 @@ static void d_cvt_colmaj_to_casadi(double *in, double *out, int *sparsity_out) -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_casadi_to_dmat(double *in, int *sparsity_in, struct blasfeo_dmat *out) +static void d_cvt_casadi_to_dmat(double *in, int *sparsity_in, struct blasfeo_dmat *out, int is_dense) { int jj, idx; int nrow = sparsity_in[0]; int ncol = sparsity_in[1]; - int dense = sparsity_in[2]; if ((nrow<=0 )| (ncol<=0)) return; - if (dense) + if (is_dense) { blasfeo_pack_dmat(nrow, ncol, in, nrow, out, 0, 0); } @@ -307,19 +354,17 @@ static void d_cvt_casadi_to_dmat(double *in, int *sparsity_in, struct blasfeo_dm -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_dmat_to_casadi(struct blasfeo_dmat *in, double *out, int *sparsity_out) +static void d_cvt_dmat_to_casadi(struct blasfeo_dmat *in, double *out, int *sparsity_out, int is_dense) { int jj, idx; int nrow = sparsity_out[0]; int ncol = sparsity_out[1]; - int dense = sparsity_out[2]; if ((nrow<=0 )| (ncol<=0)) return; - if (dense) + if (is_dense) { blasfeo_unpack_dmat(nrow, ncol, in, 0, 0, out, nrow); } @@ -345,20 +390,18 @@ static void d_cvt_dmat_to_casadi(struct blasfeo_dmat *in, double *out, int *spar // column vector: assume sparsity_in[1] = 1 !!! or empty vector; -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_casadi_to_dvec(double *in, int *sparsity_in, struct blasfeo_dvec *out) +static void d_cvt_casadi_to_dvec(double *in, int *sparsity_in, struct blasfeo_dvec *out, int is_dense) { int idx; assert((sparsity_in[1] == 1) | (sparsity_in[0] == 0) | (sparsity_in[1] == 0)); int n = sparsity_in[0]; - int dense = sparsity_in[2]; if (n<=0) return; - if (dense) + if (is_dense) { blasfeo_pack_dvec(n, in, 1, out, 0); } @@ -383,20 +426,18 @@ static void d_cvt_casadi_to_dvec(double *in, int *sparsity_in, struct blasfeo_dv // column vector: assume sparsity_in[1] = 1 !!! or empty vector; -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_dvec_to_casadi(struct blasfeo_dvec *in, double *out, int *sparsity_out) +static void d_cvt_dvec_to_casadi(struct blasfeo_dvec *in, double *out, int *sparsity_out, int is_dense) { int idx; assert((sparsity_out[1] == 1) | (sparsity_out[0] == 0) | (sparsity_out[1] == 0)); int n = sparsity_out[0]; - int dense = sparsity_out[2]; if (n<=0) return; - if (dense) + if (is_dense) { blasfeo_unpack_dvec(n, in, 0, out, 1); } @@ -418,13 +459,12 @@ static void d_cvt_dvec_to_casadi(struct blasfeo_dvec *in, double *out, int *spar -static void d_cvt_casadi_to_colmaj_args(double *in, int *sparsity_in, struct colmaj_args *out) +static void d_cvt_casadi_to_colmaj_args(double *in, int *sparsity_in, struct colmaj_args *out, int is_dense) { int ii, jj, idx; int nrow = sparsity_in[0]; int ncol = sparsity_in[1]; - int dense = sparsity_in[2]; if ((nrow<=0 )| (ncol<=0)) return; @@ -432,7 +472,7 @@ static void d_cvt_casadi_to_colmaj_args(double *in, int *sparsity_in, struct col double *A = out->A; int lda = out->lda; - if (dense) + if (is_dense) { for (ii = 0; ii < ncol; ii++) for (jj = 0; jj < nrow; jj++) A[ii + jj * lda] = in[ii + ncol * jj]; @@ -461,13 +501,12 @@ static void d_cvt_casadi_to_colmaj_args(double *in, int *sparsity_in, struct col -static void d_cvt_colmaj_args_to_casadi(struct colmaj_args *in, double *out, int *sparsity_out) +static void d_cvt_colmaj_args_to_casadi(struct colmaj_args *in, double *out, int *sparsity_out, int is_dense) { int ii, jj, idx; int nrow = sparsity_out[0]; int ncol = sparsity_out[1]; - int dense = sparsity_out[2]; if ((nrow<=0 )| (ncol<=0)) return; @@ -475,7 +514,7 @@ static void d_cvt_colmaj_args_to_casadi(struct colmaj_args *in, double *out, int double *A = in->A; int lda = in->lda; - if (dense) + if (is_dense) { for (ii = 0; ii < ncol; ii++) for (jj = 0; jj < nrow; jj++) out[ii + ncol * jj] = A[ii + jj * lda]; @@ -501,14 +540,12 @@ static void d_cvt_colmaj_args_to_casadi(struct colmaj_args *in, double *out, int -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_casadi_to_dmat_args(double *in, int *sparsity_in, struct blasfeo_dmat_args *out) +static void d_cvt_casadi_to_dmat_args(double *in, int *sparsity_in, struct blasfeo_dmat_args *out, int is_dense) { int jj, idx; int nrow = sparsity_in[0]; int ncol = sparsity_in[1]; - int dense = sparsity_in[2]; if ((nrow<=0 )| (ncol<=0)) return; @@ -517,7 +554,7 @@ static void d_cvt_casadi_to_dmat_args(double *in, int *sparsity_in, struct blasf int ai = out->ai; int aj = out->aj; - if (dense) + if (is_dense) { blasfeo_pack_dmat(nrow, ncol, in, nrow, A, ai, aj); } @@ -544,14 +581,12 @@ static void d_cvt_casadi_to_dmat_args(double *in, int *sparsity_in, struct blasf -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_dmat_args_to_casadi(struct blasfeo_dmat_args *in, double *out, int *sparsity_out) +static void d_cvt_dmat_args_to_casadi(struct blasfeo_dmat_args *in, double *out, int *sparsity_out, int is_dense) { int jj, idx; int nrow = sparsity_out[0]; int ncol = sparsity_out[1]; - int dense = sparsity_out[2]; if ((nrow<=0 )| (ncol<=0)) return; @@ -560,7 +595,7 @@ static void d_cvt_dmat_args_to_casadi(struct blasfeo_dmat_args *in, double *out, int ai = in->ai; int aj = in->aj; - if (dense) + if (is_dense) { blasfeo_unpack_dmat(nrow, ncol, A, ai, aj, out, nrow); } @@ -586,15 +621,13 @@ static void d_cvt_dmat_args_to_casadi(struct blasfeo_dmat_args *in, double *out, // column vector: assume sparsity_in[1] = 1 !!! or empty vector; -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_casadi_to_dvec_args(double *in, int *sparsity_in, struct blasfeo_dvec_args *out) +static void d_cvt_casadi_to_dvec_args(double *in, int *sparsity_in, struct blasfeo_dvec_args *out, int is_dense) { int idx; assert((sparsity_in[1] == 1) | (sparsity_in[0] == 0) | (sparsity_in[1] == 0)); int n = sparsity_in[0]; - int dense = sparsity_in[2]; if (n<=0) return; @@ -602,7 +635,7 @@ static void d_cvt_casadi_to_dvec_args(double *in, int *sparsity_in, struct blasf struct blasfeo_dvec *x = out->x; int xi = out->xi; - if (dense) + if (is_dense) { blasfeo_pack_dvec(n, in, 1, x, xi); } @@ -627,15 +660,13 @@ static void d_cvt_casadi_to_dvec_args(double *in, int *sparsity_in, struct blasf // column vector: assume sparsity_in[1] = 1 !!! or empty vector; -// TODO(all): detect if dense from number of elements per column !!! -static void d_cvt_dvec_args_to_casadi(struct blasfeo_dvec_args *in, double *out, int *sparsity_out) +static void d_cvt_dvec_args_to_casadi(struct blasfeo_dvec_args *in, double *out, int *sparsity_out, int is_dense) { int idx; assert((sparsity_out[1] == 1) | (sparsity_out[0] == 0) | (sparsity_out[1] == 0)); int n = sparsity_out[0]; - int dense = sparsity_out[2]; if (n<=0) return; @@ -643,7 +674,7 @@ static void d_cvt_dvec_args_to_casadi(struct blasfeo_dvec_args *in, double *out, struct blasfeo_dvec *x = in->x; int xi = in->xi; - if (dense) + if (is_dense) { blasfeo_unpack_dvec(n, x, xi, out, 1); } @@ -754,8 +785,8 @@ acados_size_t external_function_casadi_calculate_size(external_function_casadi * size += fun->res_num * sizeof(double *); // res // ints - size += fun->args_num * sizeof(int); // args_size - size += fun->res_num * sizeof(int); // res_size + size += 2 * fun->args_num * sizeof(int); // args_size, args_dense + size += 2 * fun->res_num * sizeof(int); // res_size, res_dense size += fun->iw_size * sizeof(int); // iw // doubles @@ -794,14 +825,22 @@ void external_function_casadi_assign(external_function_casadi *fun, void *raw_me // res assign_and_advance_double_ptrs(fun->res_num, &fun->res, &c_ptr); - // args_size + // args_size, args_dense assign_and_advance_int(fun->args_num, &fun->args_size, &c_ptr); + assign_and_advance_int(fun->args_num, &fun->args_dense, &c_ptr); for (ii = 0; ii < fun->args_num; ii++) + { fun->args_size[ii] = casadi_nnz(fun->casadi_sparsity_in(ii)); - // res_size + fun->args_dense[ii] = casadi_is_dense(fun->casadi_sparsity_in(ii)); + } + // res_size, res_dense assign_and_advance_int(fun->res_num, &fun->res_size, &c_ptr); + assign_and_advance_int(fun->res_num, &fun->res_dense, &c_ptr); for (ii = 0; ii < fun->res_num; ii++) + { fun->res_size[ii] = casadi_nnz(fun->casadi_sparsity_out(ii)); + fun->res_dense[ii] = casadi_is_dense(fun->casadi_sparsity_out(ii)); + } // iw assign_and_advance_int(fun->iw_size, &fun->iw, &c_ptr); @@ -840,32 +879,38 @@ void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void * { case COLMAJ: d_cvt_colmaj_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case BLASFEO_DMAT: d_cvt_dmat_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case BLASFEO_DVEC: d_cvt_dvec_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case COLMAJ_ARGS: d_cvt_colmaj_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case BLASFEO_DMAT_ARGS: d_cvt_dmat_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case BLASFEO_DVEC_ARGS: d_cvt_dvec_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), + fun->args_dense[ii]); break; case IGNORE_ARGUMENT: @@ -888,32 +933,38 @@ void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void * { case COLMAJ: d_cvt_casadi_to_colmaj((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], + fun->res_dense[ii]); break; case BLASFEO_DMAT: d_cvt_casadi_to_dmat((double *) fun->res[ii], (int *) fun->casadi_sparsity_out(ii), - out[ii]); + out[ii], + fun->res_dense[ii]); break; case BLASFEO_DVEC: d_cvt_casadi_to_dvec((double *) fun->res[ii], (int *) fun->casadi_sparsity_out(ii), - out[ii]); + out[ii], + fun->res_dense[ii]); break; case COLMAJ_ARGS: d_cvt_casadi_to_colmaj_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], + fun->res_dense[ii]); break; case BLASFEO_DMAT_ARGS: d_cvt_casadi_to_dmat_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], + fun->res_dense[ii]); break; case BLASFEO_DVEC_ARGS: d_cvt_casadi_to_dvec_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], + fun->res_dense[ii]); break; case IGNORE_ARGUMENT: @@ -1053,8 +1104,8 @@ acados_size_t external_function_param_casadi_calculate_size(external_function_pa size += fun->res_num * sizeof(double *); // res // ints - size += fun->args_num * sizeof(int); // args_size - size += fun->res_num * sizeof(int); // res_size + size += 2 * fun->args_num * sizeof(int); // args_size, args_dense + size += 2 * fun->res_num * sizeof(int); // res_size, res_dense size += fun->iw_size * sizeof(int); // iw // doubles @@ -1094,14 +1145,22 @@ void external_function_param_casadi_assign(external_function_param_casadi *fun, // res assign_and_advance_double_ptrs(fun->res_num, &fun->res, &c_ptr); - // args_size + // args_size, args_dense assign_and_advance_int(fun->args_num, &fun->args_size, &c_ptr); + assign_and_advance_int(fun->args_num, &fun->args_dense, &c_ptr); for (ii = 0; ii < fun->args_num; ii++) + { fun->args_size[ii] = casadi_nnz(fun->casadi_sparsity_in(ii)); - // res_size + fun->args_dense[ii] = casadi_is_dense(fun->casadi_sparsity_in(ii)); + } + // res_size, res_dense assign_and_advance_int(fun->res_num, &fun->res_size, &c_ptr); + assign_and_advance_int(fun->res_num, &fun->res_dense, &c_ptr); for (ii = 0; ii < fun->res_num; ii++) + { fun->res_size[ii] = casadi_nnz(fun->casadi_sparsity_out(ii)); + fun->res_dense[ii] = casadi_is_dense(fun->casadi_sparsity_out(ii)); + } // iw assign_and_advance_int(fun->iw_size, &fun->iw, &c_ptr); @@ -1142,31 +1201,31 @@ void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, { case COLMAJ: d_cvt_colmaj_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case BLASFEO_DMAT: d_cvt_dmat_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case BLASFEO_DVEC: d_cvt_dvec_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case COLMAJ_ARGS: d_cvt_colmaj_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case BLASFEO_DMAT_ARGS: d_cvt_dmat_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case BLASFEO_DVEC_ARGS: d_cvt_dvec_args_to_casadi(in[ii], (double *) fun->args[ii], - (int *) fun->casadi_sparsity_in(ii)); + (int *) fun->casadi_sparsity_in(ii), fun->args_dense[ii]); break; case IGNORE_ARGUMENT: @@ -1190,31 +1249,32 @@ void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, { case COLMAJ: d_cvt_casadi_to_colmaj((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], + fun->res_dense[ii]); break; case BLASFEO_DMAT: d_cvt_casadi_to_dmat((double *) fun->res[ii], (int *) fun->casadi_sparsity_out(ii), - out[ii]); + out[ii], fun->res_dense[ii]); break; case BLASFEO_DVEC: d_cvt_casadi_to_dvec((double *) fun->res[ii], (int *) fun->casadi_sparsity_out(ii), - out[ii]); + out[ii], fun->res_dense[ii]); break; case COLMAJ_ARGS: d_cvt_casadi_to_colmaj_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], fun->res_dense[ii]); break; case BLASFEO_DMAT_ARGS: d_cvt_casadi_to_dmat_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], fun->res_dense[ii]); break; case BLASFEO_DVEC_ARGS: d_cvt_casadi_to_dvec_args((double *) fun->res[ii], - (int *) fun->casadi_sparsity_out(ii), out[ii]); + (int *) fun->casadi_sparsity_out(ii), out[ii], fun->res_dense[ii]); break; case IGNORE_ARGUMENT: diff --git a/acados/utils/external_function_generic.h b/acados/utils/external_function_generic.h index 1e68dc155d..2f62b921cf 100644 --- a/acados/utils/external_function_generic.h +++ b/acados/utils/external_function_generic.h @@ -142,6 +142,8 @@ typedef struct int *iw; int *args_size; // size of args[i] int *res_size; // size of res[i] + int *args_dense; // indicates if args[i] is dense + int *res_dense; // indicates if res[i] is dense int args_num; // number of args arrays int args_size_tot; // total size of args arrays int res_num; // number of res arrays @@ -200,6 +202,8 @@ typedef struct int *iw; int *args_size; // size of args[i] int *res_size; // size of res[i] + int *args_dense; // indicates if args[i] is dense + int *res_dense; // indicates if res[i] is dense int args_num; // number of args arrays int args_size_tot; // total size of args arrays int res_num; // number of res arrays diff --git a/examples/c/simple_dae_example.c b/examples/c/simple_dae_example.c index f6396ea49c..5af09aed18 100644 --- a/examples/c/simple_dae_example.c +++ b/examples/c/simple_dae_example.c @@ -395,6 +395,7 @@ int main() { } ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts); + ocp_nlp_precompute(solver, nlp_in, nlp_out); // NLP solution acados_timer timer; diff --git a/test/ocp_nlp/test_chain.cpp b/test/ocp_nlp/test_chain.cpp index d63e2ab2e8..f64113647c 100644 --- a/test/ocp_nlp/test_chain.cpp +++ b/test/ocp_nlp/test_chain.cpp @@ -1415,12 +1415,14 @@ void setup_and_solve_nlp(int NN, ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts); + int status; + status = ocp_nlp_precompute(solver, nlp_in, nlp_out); + /************************************************ * sqp solve ************************************************/ - int status; // warm start output initial guess of // solution From adb1244f1b4d19adcdcaa9150946bee8760634f9 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 19 Feb 2024 10:37:16 +0100 Subject: [PATCH 05/16] Fix slack function update in CONL, NLS module (#1039) --- acados/ocp_nlp/ocp_nlp_cost_conl.c | 6 ++++-- acados/ocp_nlp/ocp_nlp_cost_external.c | 2 ++ acados/ocp_nlp/ocp_nlp_cost_ls.c | 2 ++ acados/ocp_nlp/ocp_nlp_cost_nls.c | 6 ++++-- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c index 4b9372afa5..edbe2bb517 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.c +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.c @@ -705,9 +705,11 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); // slack update function value + // tmp_2ns = 2 * z + Z .* slack blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &work->tmp_2ns, 0); + // fun += .5 * (tmp_2ns .* slack) + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); // scale if(model->scaling!=1.0) diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index 4ef88e6583..2d2f201716 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -675,8 +675,10 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void * blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); // slack update function value + // tmp_2ns = 2 * z + Z .* slack blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &work->tmp_2ns, 0); + // fun += .5 * (tmp_2ns .* slack) memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); // scale diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index 7c74e14634..a768ce0837 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -841,8 +841,10 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); // slack update function value + // tmp_2ns = 2 * z + Z .* slack blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &work->tmp_2ns, 0); + // fun += .5 * (tmp_2ns .* slack) memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); // scale diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index d25b03f150..d628e44bef 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -839,9 +839,11 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); // slack update function value + // tmp_2ns = 2 * z + Z .* slack blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &work->tmp_2ns, 0); + // fun += .5 * (tmp_2ns .* slack) + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); // scale if (model->scaling!=1.0) From 534354000119a3d8edf7d40b819ffe4b4ae40d6a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 19 Feb 2024 16:08:03 +0100 Subject: [PATCH 06/16] Cleanup RTI, fix timings and comments (#1038) - fix timings for RTI: subtimers `time_qp*`, `time_reg` now start at 0 when starting preparation and count operations in both phases - cleanup comments - make enum for RTI phase - precompute integrator workspace_size --- acados/ocp_nlp/ocp_nlp_common.c | 3 +- acados/ocp_nlp/ocp_nlp_common.h | 12 +- acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 14 ++- acados/ocp_nlp/ocp_nlp_dynamics_cont.h | 1 + acados/ocp_nlp/ocp_nlp_sqp.c | 43 ++----- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 132 +++++++--------------- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 12 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 4 - 9 files changed, 75 insertions(+), 148 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 0b0df6e70a..711e1620ff 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -2204,7 +2204,8 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, // update QP rhs for SQP (step prim var, abs dual var) -// evaluate constraints wrt bounds -> allows to update all bounds between preparation and feedback phase. +// - use cost gradient and dynamics residual from memory +// - evaluate constraints wrt bounds -> allows to update all bounds between preparation and feedback phase. void ocp_nlp_approximate_qp_vectors_sqp(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/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 5474e8f0b3..b6098edb80 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -91,7 +91,7 @@ typedef struct ocp_nlp_config // prepare memory int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); - // initalize this struct with default values + // initialize this struct with default values void (*config_initialize_default)(void *config); // general getter void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_); @@ -143,7 +143,7 @@ acados_size_t ocp_nlp_dims_calculate_size(void *config); ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory); /// Sets the dimension of optimization variables -/// (states, constrols, algebraic variables, slack variables). +/// (states, controls, algebraic variables, slack variables). /// /// \param config_ The configuration struct. /// \param dims_ The dimension struct. @@ -169,7 +169,7 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, /// \param config_ The configuration struct. /// \param dims_ The dimension struct. /// \param stage Stage number. -/// \param field Type of cost term, can be eiter ny. +/// \param field Type of cost dimension /// \param value_field Number of cost terms/residuals for the given stage. void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field, const void* value_field); @@ -191,7 +191,7 @@ void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char /// Struct for storing the inputs of an OCP NLP solver typedef struct ocp_nlp_in { - /// Length of sampling intervals/timesteps. + /// Timesteps. double *Ts; /// Pointers to cost functions (TBC). @@ -225,9 +225,9 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * typedef struct ocp_nlp_out { struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs? - struct blasfeo_dvec *z; // algebraic vairables + struct blasfeo_dvec *z; // algebraic variables struct blasfeo_dvec *pi; // multipliers for dynamics - struct blasfeo_dvec *lam; // inequality mulitpliers + struct blasfeo_dvec *lam; // inequality multipliers struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution) // NOTE: the inequalities are internally organized in the following order: diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 777939673f..d4b3c4f9f3 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -640,7 +640,7 @@ static void ocp_nlp_dynamics_cont_cast_workspace(void *config_, void *dims_, voi ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_config *config = config_; ocp_nlp_dynamics_cont_dims *dims = dims_; - ocp_nlp_dynamics_cont_opts *opts = opts_; + // ocp_nlp_dynamics_cont_opts *opts = opts_; ocp_nlp_dynamics_cont_workspace *work = work_; char *c_ptr = (char *) work_; @@ -658,7 +658,7 @@ static void ocp_nlp_dynamics_cont_cast_workspace(void *config_, void *dims_, voi c_ptr += sim_out_calculate_size(config->sim_solver, dims->sim); // workspace work->sim_solver = c_ptr; - c_ptr += config->sim_solver->workspace_calculate_size(config->sim_solver, dims->sim, opts->sim_solver); + c_ptr += mem->sim_workspace_size; // blasfeo_mem align align_char_to(64, &c_ptr); @@ -971,13 +971,15 @@ int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims_, void *model_, v { ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_config *config = config_; + ocp_nlp_dynamics_cont_model *model = model_; + ocp_nlp_dynamics_cont_opts *opts = opts_; + ocp_nlp_dynamics_cont_dims *dims = dims_; + mem->workspace_size = ocp_nlp_dynamics_cont_workspace_calculate_size(config, dims_, opts_); - ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); + mem->sim_workspace_size = config->sim_solver->workspace_calculate_size(config->sim_solver, dims->sim, opts->sim_solver); - // ocp_nlp_dynamics_cont_dims *dims = dims_; - ocp_nlp_dynamics_cont_opts *opts = opts_; + ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); ocp_nlp_dynamics_cont_workspace *work = work_; - ocp_nlp_dynamics_cont_model *model = model_; work->sim_in->model = model->sim_model; work->sim_in->T = model->T; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 186c19de96..89867fa5a2 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -122,6 +122,7 @@ typedef struct struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed void *sim_solver; // sim solver memory acados_size_t workspace_size; + acados_size_t sim_workspace_size; } ocp_nlp_dynamics_cont_memory; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 1b25dfed94..395fb0e8cb 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -511,7 +511,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, for (; sqp_iter < opts->max_iter; sqp_iter++) { - // linearizate NLP and update QP matrices + // linearize NLP and update QP matrices acados_tic(&timer1); ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); mem->time_lin += acados_toc(&timer1); @@ -742,41 +742,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // int *ni = dims->ni; /* evaluate constraints & dynamics at new step */ - // The following (setting up ux + p in tmp_nlp_out and evaluation of constraints + dynamics) - // is not needed anymore because done in prelim. line search with early termination) + // 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 - // set up new linearization point in work->tmp_nlp_out - // for (ii = 0; ii < N; ii++) - // blasfeo_dveccp(nx[ii+1], nlp_out->pi+ii, 0, work->nlp_work->tmp_nlp_out->pi+ii, 0); - - // for (ii = 0; ii <= N; ii++) - // blasfeo_dveccp(2*ni[ii], nlp_out->lam+ii, 0, work->nlp_work->tmp_nlp_out->lam+ii, 0); - - // // tmp_nlp_out = iterate + step - // for (ii = 0; ii <= N; ii++) - // blasfeo_daxpy(nv[ii], 1.0, qp_out->ux+ii, 0, nlp_out->ux+ii, 0, work->nlp_work->tmp_nlp_out->ux+ii, 0); - - // // evaluate - // #if defined(ACADOS_WITH_OPENMP) - // #pragma omp parallel for - // #endif - // for (ii=0; iidynamics[ii]->compute_fun(config->dynamics[ii], dims->dynamics[ii], nlp_in->dynamics[ii], - // nlp_opts->dynamics[ii], nlp_mem->dynamics[ii], work->nlp_work->dynamics[ii]); - // } - // #if defined(ACADOS_WITH_OPENMP) - // #pragma omp parallel for - // #endif - // for (ii=0; ii<=N; ii++) - // { - // config->constraints[ii]->compute_fun(config->constraints[ii], dims->constraints[ii], - // nlp_in->constraints[ii], nlp_opts->constraints[ii], - // nlp_mem->constraints[ii], work->nlp_work->constraints[ii]); - // } - // #if defined(ACADOS_WITH_OPENMP) - // #pragma omp parallel for - // #endif // 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; @@ -799,7 +768,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // 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) + // 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 @@ -859,6 +829,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // acados_tic(&timer1); qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, qp_in, qp_out, opts->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. // tmp_time = acados_toc(&timer1); // mem->time_qp_sol += tmp_time; // qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index b3037025ad..b2d3215b2e 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -107,15 +107,6 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, ocp_nlp_sqp_rti_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - // ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // ocp_nlp_dynamics_config **dynamics = config->dynamics; - // ocp_nlp_constraints_config **constraints = config->constraints; - - // int ii; - - // int N = dims->N; - - // this first !!! ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP RTI opts @@ -123,25 +114,6 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, opts->warm_start_first_qp = false; opts->rti_phase = 0; - // overwrite default submodules opts - - // do not compute adjoint in dynamics and constraints - // int compute_adj = 0; - - // // dynamics - // for (ii = 0; ii < N; ii++) - // { - // dynamics[ii]->opts_set(dynamics[ii], - // opts->nlp_opts->dynamics[ii], "compute_adj", &compute_adj); - // } - - // // constraints - // for (ii = 0; ii <= N; ii++) - // { - // constraints[ii]->opts_set(constraints[ii], - // opts->nlp_opts->constraints[ii], "compute_adj", &compute_adj); - // } - return; } @@ -252,16 +224,6 @@ acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, ocp_nlp_sqp_rti_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - // ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // ocp_nlp_dynamics_config **dynamics = config->dynamics; - // ocp_nlp_cost_config **cost = config->cost; - // ocp_nlp_constraints_config **constraints = config->constraints; - - // int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; - // int *nz = dims->nz; - acados_size_t size = 0; size += sizeof(ocp_nlp_sqp_rti_memory); @@ -293,20 +255,8 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, ocp_nlp_sqp_rti_opts *opts = opts_; ocp_nlp_opts *nlp_opts = opts->nlp_opts; - // ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; - // ocp_nlp_dynamics_config **dynamics = config->dynamics; - // ocp_nlp_cost_config **cost = config->cost; - // ocp_nlp_constraints_config **constraints = config->constraints; - char *c_ptr = (char *) raw_memory; - // int ii; - - // int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; - // int *nz = dims->nz; - // initial align align_char_to(8, &c_ptr); @@ -349,7 +299,6 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, acados_size_t size = 0; - // sqp size += sizeof(ocp_nlp_sqp_rti_workspace); // nlp @@ -419,6 +368,18 @@ static void ocp_nlp_sqp_rti_cast_workspace( } +// utility functions +static void reset_sub_timers(ocp_nlp_sqp_rti_memory *mem) +{ + mem->time_lin = 0.0; + mem->time_reg = 0.0; + mem->time_qp_sol = 0.0; + mem->time_qp_solver_call = 0.0; + mem->time_qp_xcond = 0.0; + mem->time_glob = 0.0; +} + + /************************************************ * functions @@ -434,9 +395,7 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim ocp_nlp_workspace *nlp_work = work->nlp_work; - mem->time_lin = 0.0; - mem->time_reg = 0.0; - + reset_sub_timers(mem); #if defined(ACADOS_WITH_OPENMP) // backup number of threads int num_threads_bkp = omp_get_num_threads(); @@ -444,19 +403,16 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim omp_set_num_threads(opts->nlp_opts->num_threads); #endif - // initialize QP + // prepare submodules ocp_nlp_initialize_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); - /* SQP body */ // linearize NLP and update QP matrices acados_tic(&timer1); ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); - mem->time_lin += acados_toc(&timer1); - - if (opts->rti_phase == 1) + if (opts->rti_phase == PREPARATION) { // regularize Hessian acados_tic(&timer1); @@ -464,9 +420,11 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); mem->time_reg += acados_toc(&timer1); // condense lhs + acados_tic(&timer1); qp_solver->condense_lhs(qp_solver, dims->qp_solver, nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); + mem->time_qp_sol += acados_toc(&timer1); } #if defined(ACADOS_WITH_OPENMP) // restore number of threads @@ -474,11 +432,9 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim #endif return; - } - static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, ocp_nlp_sqp_rti_opts *opts, ocp_nlp_sqp_rti_memory *mem, ocp_nlp_sqp_rti_workspace *work) { @@ -492,31 +448,32 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * int qp_iter = 0; int qp_status = 0; double tmp_time; - mem->time_qp_sol = 0.0; - mem->time_qp_solver_call = 0.0; - mem->time_qp_xcond = 0.0; - mem->time_glob = 0.0; // update QP rhs for SQP (step prim var, abs dual var) + acados_tic(&timer1); ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + mem->time_lin += acados_toc(&timer1); - if (opts->rti_phase == 2) + // regularization + acados_tic(&timer1); + if (opts->rti_phase == FEEDBACK) { // finish regularization - acados_tic(&timer1); config->regularize->regularize_rhs(config->regularize, dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); - mem->time_reg += acados_toc(&timer1); } - else + else if (opts->rti_phase == PREPARATION_AND_FEEDBACK) { // full regularization - acados_tic(&timer1); config->regularize->regularize(config->regularize, dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); - mem->time_reg += acados_toc(&timer1); } + else + { + printf("ocp_nlp_sqp_rti_feedback_step: rti_phase must be FEEDBACK or PREPARATION_AND_FEEDBACK\n"); + } + mem->time_reg += acados_toc(&timer1); if (nlp_opts->print_level > 0) { printf("\n------- qp_in --------\n"); @@ -530,23 +487,23 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * opts->nlp_opts->qp_solver_opts, "warm_start", &tmp_int); } - // solve qp + // solve QP acados_tic(&timer1); - if (opts->rti_phase == 0) + if (opts->rti_phase == FEEDBACK) { - qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, + qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); } - else // just feedback + else if (opts->rti_phase == PREPARATION_AND_FEEDBACK) { - qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, + qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); } - + // add qp timings mem->time_qp_sol += acados_toc(&timer1); - + // NOTE: timings within qp solver are added internally (lhs+rhs) qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); mem->time_qp_solver_call += tmp_time; qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time); @@ -565,13 +522,10 @@ 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) { - ocp_qp_res_compute(nlp_mem->qp_in, nlp_mem->qp_out, - work->qp_res, work->qp_res_ws); - + 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)); } - // save statistics mem->stat[mem->stat_n*1+0] = qp_status; mem->stat[mem->stat_n*1+1] = qp_iter; @@ -592,7 +546,6 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * return; } - // globalization acados_tic(&timer1); // TODO: not clear if line search should be called with sqp_iter==0 in RTI; @@ -626,20 +579,16 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, switch(rti_phase) { - - // perform preparation and feedback rti_phase - case 0: + case PREPARATION_AND_FEEDBACK: ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); break; - // perform preparation rti_phase - case 1: + case PREPARATION: ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); break; - // perform feedback rti_phase - case 2: + case FEEDBACK: ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); break; } @@ -662,7 +611,6 @@ void ocp_nlp_sqp_rti_memory_reset_qp_solver(void *config_, void *dims_, void *nl ocp_nlp_sqp_rti_workspace *work = work_; ocp_nlp_workspace *nlp_work = work->nlp_work; - // printf("in ocp_nlp_sqp_rti_memory_reset_qp_solver\n\n"); config->qp_solver->memory_reset(qp_solver, dims->qp_solver, nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); @@ -754,7 +702,6 @@ void ocp_nlp_sqp_rti_eval_param_sens(void *config_, void *dims_, void *opts_, sens_nlp_out->t + i, 0); } - } else { @@ -770,7 +717,6 @@ void ocp_nlp_sqp_rti_eval_param_sens(void *config_, void *dims_, void *opts_, -// TODO rename memory_get ??? void ocp_nlp_sqp_rti_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_) { diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 364d0f4717..08338b85bb 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -53,6 +53,15 @@ extern "C" { * options ************************************************/ +typedef enum +{ + PREPARATION_AND_FEEDBACK, // = 0, + PREPARATION, // = 1, + FEEDBACK, // = 2, +} rti_phase_t; + + + typedef struct { ocp_nlp_opts *nlp_opts; @@ -60,7 +69,7 @@ typedef struct int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp. bool warm_start_first_qp; // to set qp_warm_start in first iteration - int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both) + rti_phase_t rti_phase; } ocp_nlp_sqp_rti_opts; @@ -89,6 +98,7 @@ typedef struct // nlp memory ocp_nlp_memory *nlp_mem; + // timers double time_qp_sol; double time_qp_solver_call; double time_qp_xcond; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index b0649c0b4e..fdb97b7ff9 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -543,7 +543,7 @@ int ocp_qp_partial_condensing_condense_rhs(void *qp_in_, void *pcond_qp_in_, voi d_part_cond_qp_cond_rhs(mem->red_qp, pcond_qp_in, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); // stop timer - mem->time_qp_xcond = acados_toc(&timer); + mem->time_qp_xcond += acados_toc(&timer); return ACADOS_SUCCESS; } diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 7d42a413ee..be93405ff8 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -476,10 +476,6 @@ ocp_nlp_out *ocp_nlp_out_create(ocp_nlp_config *config, ocp_nlp_dims *dims) ocp_nlp_out *nlp_out = ocp_nlp_out_assign(config, dims, ptr); nlp_out->raw_memory = ptr; - // initialize to zeros -// for (int ii = 0; ii <= dims->N; ++ii) -// blasfeo_dvecse(dims->qp_solver->nu[ii] + dims->qp_solver->nx[ii], 0.0, nlp_out->ux + ii, 0); - return nlp_out; } From d4908433cfadae03ebdc8fbb3ad69a65a72e0da6 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 20 Feb 2024 13:15:48 +0100 Subject: [PATCH 07/16] Cleanup SQP submodules - only use single evaluation point at a time (#1040) In some places `tmp_ux` and `ux` where used in a mixed way, i.e. `compute_fun` of constraint modules and what was fixed in #1039 - fixes bug when using globalization and cost computation - fixes bug in globalization with slacks. Constraint was evaluated with slack values from SQP iterate instead of values from the line search candidate Implementation: - Removed all `tmp_*` pointers in SQP submodules - Always evaluate at `ux` - When one wants to evaluate at another iterate, these pointers have to be temporarily set to another iterate and afterwards reset to the nominal SQP iterate, as done in `ocp_nlp_evaluate_merit_fun` now. --- acados/ocp_nlp/ocp_nlp_common.c | 48 ++++++++++-------- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 37 +++++--------- acados/ocp_nlp/ocp_nlp_constraints_bgh.h | 6 --- acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 39 +++++---------- acados/ocp_nlp/ocp_nlp_constraints_bgp.h | 6 --- acados/ocp_nlp/ocp_nlp_constraints_common.h | 2 - acados/ocp_nlp/ocp_nlp_cost_common.h | 1 - acados/ocp_nlp/ocp_nlp_cost_conl.c | 22 +++----- acados/ocp_nlp/ocp_nlp_cost_conl.h | 3 -- acados/ocp_nlp/ocp_nlp_cost_external.c | 25 +++------- acados/ocp_nlp/ocp_nlp_cost_external.h | 3 -- acados/ocp_nlp/ocp_nlp_cost_ls.c | 18 ++----- acados/ocp_nlp/ocp_nlp_cost_ls.h | 6 +-- acados/ocp_nlp/ocp_nlp_cost_nls.c | 25 +++------- acados/ocp_nlp/ocp_nlp_cost_nls.h | 6 +-- acados/ocp_nlp/ocp_nlp_dynamics_common.h | 3 -- acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 50 +++---------------- acados/ocp_nlp/ocp_nlp_dynamics_cont.h | 9 ---- acados/ocp_nlp/ocp_nlp_dynamics_disc.c | 46 +++-------------- acados/ocp_nlp/ocp_nlp_dynamics_disc.h | 9 ---- .../linear_mass_test_problem.py | 10 +--- 21 files changed, 93 insertions(+), 281 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 711e1620ff..916ced495e 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1920,6 +1920,25 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims * functions ************************************************/ +static void ocp_nlp_set_primal_variable_pointers_in_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, + ocp_nlp_out *nlp_out, ocp_nlp_memory *nlp_mem) +{ + int N = dims->N; + for (int i = 0; i < N; i++) + { + config->dynamics[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->dynamics[i]); + config->dynamics[i]->memory_set_ux1_ptr(nlp_out->ux+i+1, nlp_mem->dynamics[i]); + } + for (int i = 0; i <= N; i++) + { + config->cost[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->cost[i]); + config->constraints[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->constraints[i]); + } + return; +} + + + void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, ocp_nlp_opts *opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work) { @@ -1943,11 +1962,8 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di for (int i = 0; i < N; i++) { config->dynamics[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->dynamics[i]); - config->dynamics[i]->memory_set_tmp_ux_ptr(nlp_work->tmp_nlp_out->ux+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_ux1_ptr(nlp_out->ux+i+1, nlp_mem->dynamics[i]); - config->dynamics[i]->memory_set_tmp_ux1_ptr(nlp_work->tmp_nlp_out->ux+i+1, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_pi_ptr(nlp_out->pi+i, nlp_mem->dynamics[i]); - config->dynamics[i]->memory_set_tmp_pi_ptr(nlp_work->tmp_nlp_out->pi+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_BAbt_ptr(nlp_mem->qp_in->BAbt+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_RSQrq_ptr(nlp_mem->qp_in->RSQrq+i, nlp_mem->dynamics[i]); config->dynamics[i]->memory_set_dzduxt_ptr(nlp_mem->dzduxt+i, nlp_mem->dynamics[i]); @@ -1963,7 +1979,6 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di for (int i = 0; i <= N; i++) { config->cost[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->cost[i]); - config->cost[i]->memory_set_tmp_ux_ptr(nlp_work->tmp_nlp_out->ux+i, nlp_mem->cost[i]); config->cost[i]->memory_set_z_alg_ptr(nlp_mem->z_alg+i, nlp_mem->cost[i]); config->cost[i]->memory_set_dzdux_tran_ptr(nlp_mem->dzduxt+i, nlp_mem->cost[i]); config->cost[i]->memory_set_RSQrq_ptr(nlp_mem->qp_in->RSQrq+i, nlp_mem->cost[i]); @@ -1977,9 +1992,7 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di for (int i = 0; i <= N; i++) { config->constraints[i]->memory_set_ux_ptr(nlp_out->ux+i, nlp_mem->constraints[i]); - config->constraints[i]->memory_set_tmp_ux_ptr(nlp_work->tmp_nlp_out->ux+i, nlp_mem->constraints[i]); config->constraints[i]->memory_set_lam_ptr(nlp_out->lam+i, nlp_mem->constraints[i]); - config->constraints[i]->memory_set_tmp_lam_ptr(nlp_work->tmp_nlp_out->lam+i, nlp_mem->constraints[i]); config->constraints[i]->memory_set_z_alg_ptr(nlp_mem->z_alg+i, nlp_mem->constraints[i]); config->constraints[i]->memory_set_dzdux_tran_ptr(nlp_mem->dzduxt+i, nlp_mem->constraints[i]); config->constraints[i]->memory_set_DCt_ptr(nlp_mem->qp_in->DCt+i, nlp_mem->constraints[i]); @@ -2057,18 +2070,15 @@ void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp struct blasfeo_dvec *ineq_fun; int N = dims->N; int *ni = dims->ni; - int *ns = dims->ns; - int *nx = dims->nx; - int *nu = dims->nu; + // int *ns = dims->ns; + // int *nx = dims->nx; + // int *nu = dims->nu; #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for #endif for (int i = 0; i <= N; i++) { - // copy out->ux to tmp_nlp_out->ux, since this is used in compute_fun - blasfeo_dveccp(nx[i]+nu[i]+2*ns[i], out->ux+i, 0, work->tmp_nlp_out->ux+i, 0); - // evaluate inequalities config->constraints[i]->compute_fun(config->constraints[i], dims->constraints[i], in->constraints[i], opts->constraints[i], @@ -2426,6 +2436,7 @@ static double ocp_nlp_get_violation(ocp_nlp_config *config, ocp_nlp_dims *dims, + 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) @@ -2439,6 +2450,8 @@ double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, double merit_fun = 0.0; + // set evaluation point to tmp_nlp_out + ocp_nlp_set_primal_variable_pointers_in_submodules(config, dims, in, work->tmp_nlp_out, mem); // compute fun value #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for @@ -2468,6 +2481,8 @@ double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, in->constraints[i], opts->constraints[i], mem->constraints[i], work->constraints[i]); } + // reset evaluation point to SQP iterate + ocp_nlp_set_primal_variable_pointers_in_submodules(config, dims, in, out, mem); double *tmp_fun; double tmp; @@ -3017,20 +3032,11 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in if (cost_integration) { - // evaluate at out, instead tmp_out; - config->dynamics[i]->memory_set_tmp_ux_ptr(out->ux+i, mem->dynamics[i]); - // evaluate: TODO, where to put cost? config->dynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i], in->dynamics[i], opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); } } - // set pointers - // NOTE(oj): the cost compute function takes the tmp_ux_ptr as input, - // since it is also used for globalization, - // especially with primal variables that are NOT current SQP iterates. - config->cost[i]->memory_set_tmp_ux_ptr(out->ux+i, mem->cost[i]); - config->cost[i]->compute_fun(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i], mem->cost[i], work->cost[i]); tmp_cost = config->cost[i]->memory_get_fun_ptr(mem->cost[i]); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index dc97369ecd..29ff7d65b3 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -1080,14 +1080,6 @@ void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *me -void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_constraints_bgh_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; -} - - void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_) { @@ -1098,14 +1090,6 @@ void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void * -void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_) -{ - ocp_nlp_constraints_bgh_memory *memory = memory_; - - memory->tmp_lam = tmp_lam; -} - - void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory_) { @@ -1541,11 +1525,13 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model ext_fun_arg_t ext_fun_type_out[3]; void *ext_fun_out[3]; + struct blasfeo_dvec *ux = memory->ux; + // box - blasfeo_dvecex_sp(nb, 1.0, model->idxb, memory->tmp_ux, 0, &work->tmp_ni, 0); + blasfeo_dvecex_sp(nb, 1.0, model->idxb, ux, 0, &work->tmp_ni, 0); // general linear - blasfeo_dgemv_t(nu+nx, ng, 1.0, memory->DCt, 0, 0, memory->tmp_ux, 0, 0.0, &work->tmp_ni, nb, &work->tmp_ni, nb); + blasfeo_dgemv_t(nu+nx, ng, 1.0, memory->DCt, 0, 0, ux, 0, 0.0, &work->tmp_ni, nb, &work->tmp_ni, nb); // nonlinear if (nh > 0) @@ -1558,11 +1544,11 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model } struct blasfeo_dvec_args x_in; // input x of external fun; - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; struct blasfeo_dvec_args u_in; // input u of external fun; - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; // TODO tmp_z_alg !!! @@ -1599,10 +1585,13 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(nb+ng+nh, -1.0, &model->d, nb+ng+nh, &work->tmp_ni, 0, &memory->fun, nb+ng+nh); // soft - 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); + // 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); - 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); + // 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); return; } @@ -1668,9 +1657,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_constraints_bgh_memory_get_fun_ptr; config->memory_get_adj_ptr = &ocp_nlp_constraints_bgh_memory_get_adj_ptr; config->memory_set_ux_ptr = &ocp_nlp_constraints_bgh_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr; config->memory_set_lam_ptr = &ocp_nlp_constraints_bgh_memory_set_lam_ptr; - config->memory_set_tmp_lam_ptr = &ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr; 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; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 65f9b14ba8..126d9c5c3e 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -148,9 +148,7 @@ typedef struct struct blasfeo_dvec adj; struct blasfeo_dvec constr_eval_no_bounds; struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out - struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in @@ -171,12 +169,8 @@ struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_); // void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); // -void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); -// void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); // void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index 7366b3cccc..f20558651e 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -1007,16 +1007,6 @@ void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *me } - -void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_constraints_bgp_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; -} - - - void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_) { ocp_nlp_constraints_bgp_memory *memory = memory_; @@ -1026,14 +1016,6 @@ void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void * -void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_) -{ - ocp_nlp_constraints_bgp_memory *memory = memory_; - - memory->tmp_lam = tmp_lam; -} - - void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory_) { @@ -1407,11 +1389,13 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model ext_fun_arg_t ext_fun_type_out[3]; void *ext_fun_out[3]; + struct blasfeo_dvec *ux = memory->ux; + // box - blasfeo_dvecex_sp(nb, 1.0, model->idxb, memory->tmp_ux, 0, &work->tmp_ni, 0); + blasfeo_dvecex_sp(nb, 1.0, model->idxb, ux, 0, &work->tmp_ni, 0); // general linear - blasfeo_dgemv_t(nu+nx, ng, 1.0, memory->DCt, 0, 0, memory->tmp_ux, 0, 0.0, &work->tmp_ni, nb, &work->tmp_ni, nb); + blasfeo_dgemv_t(nu+nx, ng, 1.0, memory->DCt, 0, 0, ux, 0, 0.0, &work->tmp_ni, nb, &work->tmp_ni, nb); // nonlinear if (nphi > 0) @@ -1424,11 +1408,11 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model } struct blasfeo_dvec_args x_in; // input x of external fun; - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; struct blasfeo_dvec_args u_in; // input u of external fun; - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; // TODO tmp_z_alg !!! @@ -1462,10 +1446,13 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(nb+ng+nphi, -1.0, &model->d, nb+ng+nphi, &work->tmp_ni, 0, &memory->fun, nb+ng+nphi); // soft - 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); + // 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); - 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); + // 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); return; @@ -1535,9 +1522,7 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_constraints_bgp_memory_get_fun_ptr; config->memory_get_adj_ptr = &ocp_nlp_constraints_bgp_memory_get_adj_ptr; config->memory_set_ux_ptr = &ocp_nlp_constraints_bgp_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr; config->memory_set_lam_ptr = &ocp_nlp_constraints_bgp_memory_set_lam_ptr; - config->memory_set_tmp_lam_ptr = &ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr; 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; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index 8610690a53..03e0665b0b 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -134,9 +134,7 @@ typedef struct struct blasfeo_dvec adj; struct blasfeo_dvec constr_eval_no_bounds; struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out - struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in @@ -158,12 +156,8 @@ struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_); // void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); // -void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); -// void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); // void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index a3cc693325..70f5cf6ec2 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -70,9 +70,7 @@ typedef struct struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory); struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory); void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); - void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory); - void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory); 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); diff --git a/acados/ocp_nlp/ocp_nlp_cost_common.h b/acados/ocp_nlp/ocp_nlp_cost_common.h index 7bc3c91bdb..19120dc4a5 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -75,7 +75,6 @@ typedef struct struct blasfeo_dvec *(*model_get_y_ref_ptr)(void *memory); struct blasfeo_dmat *(*memory_get_W_chol_ptr)(void *memory_); void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); - void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory); void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c index edbe2bb517..da2ada2436 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.c +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.c @@ -466,17 +466,6 @@ void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_) -void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_cost_conl_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; - - return; -} - - - void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_) { ocp_nlp_cost_conl_memory *memory = memory_; @@ -738,6 +727,8 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_, int nu = dims->nu; int ns = dims->ns; + struct blasfeo_dvec *ux = memory->ux; + if (opts->integrator_cost == 0) { /* specify input types and pointers for external cost function */ @@ -748,11 +739,11 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_, // INPUT struct blasfeo_dvec_args x_in; // input x - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; struct blasfeo_dvec_args u_in; // input u - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; ext_fun_type_in[0] = BLASFEO_DVEC_ARGS; @@ -776,8 +767,8 @@ void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_, // slack update function value blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, ux, nu+nx, &work->tmp_2ns, 0); + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale if (model->scaling!=1.0) @@ -813,7 +804,6 @@ void ocp_nlp_cost_conl_config_initialize_default(void *config_) config->memory_get_W_chol_ptr = &ocp_nlp_cost_conl_memory_get_W_chol_ptr; config->model_get_y_ref_ptr = &ocp_nlp_cost_conl_model_get_y_ref_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_conl_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_cost_conl_memory_set_tmp_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; config->memory_set_RSQrq_ptr = &ocp_nlp_cost_conl_memory_set_RSQrq_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.h b/acados/ocp_nlp/ocp_nlp_cost_conl.h index 06690456f6..3bb4560900 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.h +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.h @@ -136,7 +136,6 @@ typedef struct { struct blasfeo_dvec grad; // gradient of cost function struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *Z; // pointer to Z in qp_in struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out @@ -161,8 +160,6 @@ void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); // void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index 2d2f201716..6b32da3fe7 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -448,18 +448,6 @@ void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memo } - -void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_cost_external_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; - - return; -} - - - void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_) { ocp_nlp_cost_external_memory *memory = memory_; @@ -700,8 +688,6 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void * void ocp_nlp_cost_external_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { -// printf("\nerror: ocp_external_cost_nls_compute_fun: not implemented yet\n"); -// exit(1); ocp_nlp_cost_external_dims *dims = dims_; ocp_nlp_cost_external_model *model = model_; @@ -711,6 +697,8 @@ void ocp_nlp_cost_external_compute_fun(void *config_, void *dims_, void *model_, ocp_nlp_cost_external_cast_workspace(config_, dims, opts_, work_); + struct blasfeo_dvec *ux = memory->ux; + int nx = dims->nx; int nu = dims->nu; int ns = dims->ns; @@ -723,11 +711,11 @@ void ocp_nlp_cost_external_compute_fun(void *config_, void *dims_, void *model_, // INPUT struct blasfeo_dvec_args u_in; // input u - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; struct blasfeo_dvec_args x_in; // input x - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; ext_fun_type_in[0] = BLASFEO_DVEC_ARGS; @@ -751,8 +739,8 @@ void ocp_nlp_cost_external_compute_fun(void *config_, void *dims_, void *model_, // slack update function value blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, ux, nu+nx, &work->tmp_2ns, 0); + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale if(model->scaling!=1.0) @@ -788,7 +776,6 @@ void ocp_nlp_cost_external_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_cost_external_memory_get_fun_ptr; config->memory_get_grad_ptr = &ocp_nlp_cost_external_memory_get_grad_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_external_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_cost_external_memory_set_tmp_ux_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_cost_external_memory_set_z_alg_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_cost_external_memory_set_dzdux_tran_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_cost_external_memory_set_RSQrq_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.h b/acados/ocp_nlp/ocp_nlp_cost_external.h index 78958270de..9d98d69554 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -117,7 +117,6 @@ typedef struct { struct blasfeo_dvec grad; // gradient of cost function struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *Z; // pointer to Z in qp_in struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out @@ -140,8 +139,6 @@ void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); // void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index a768ce0837..a673d5bd63 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -576,15 +576,6 @@ void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_) -void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_cost_ls_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; -} - - - void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_) { ocp_nlp_cost_ls_memory *memory = memory_; @@ -875,6 +866,8 @@ void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims_, void *model_, void int ny = dims->ny; int ns = dims->ns; + struct blasfeo_dvec *ux = memory->ux; + // TODO should this overwrite memory->{res,fun,...} (as now) or not ???? if (nz > 0) { @@ -903,7 +896,7 @@ void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims_, void *model_, void else { // res = Cy * ux - yref - blasfeo_dgemv_t(nu+nx, ny, 1.0, &model->Cyt, 0, 0, memory->tmp_ux, 0, -1.0, + blasfeo_dgemv_t(nu+nx, ny, 1.0, &model->Cyt, 0, 0, ux, 0, -1.0, &model->y_ref, 0, &memory->res, 0); } @@ -920,8 +913,8 @@ void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims_, void *model_, void // slack update function value blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, ux, nu+nx, &work->tmp_2ns, 0); + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale if (model->scaling!=1.0) @@ -956,7 +949,6 @@ void ocp_nlp_cost_ls_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_cost_ls_memory_get_fun_ptr; config->memory_get_grad_ptr = &ocp_nlp_cost_ls_memory_get_grad_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_ls_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_cost_ls_memory_set_tmp_ux_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_cost_ls_memory_set_z_alg_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_cost_ls_memory_set_RSQrq_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.h b/acados/ocp_nlp/ocp_nlp_cost_ls.h index 429882dada..6c9f5d2382 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -162,7 +162,6 @@ typedef struct struct blasfeo_dvec res; ///< ls residual r(x) struct blasfeo_dvec grad; ///< gradient of cost function struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in @@ -185,8 +184,6 @@ void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); // void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); @@ -231,7 +228,8 @@ void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *o void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); // -void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); +void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, + void *memory_, void *work_); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index d628e44bef..562534943c 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -526,17 +526,6 @@ void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_) -void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_cost_nls_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; - - return; -} - - - void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_) { ocp_nlp_cost_nls_memory *memory = memory_; @@ -870,15 +859,14 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { -// printf("\nerror: ocp_nlp_cost_nls_compute_fun: not implemented yet\n"); -// exit(1); - ocp_nlp_cost_nls_dims *dims = dims_; ocp_nlp_cost_nls_model *model = model_; ocp_nlp_cost_nls_opts *opts = opts_; ocp_nlp_cost_nls_memory *memory = memory_; ocp_nlp_cost_nls_workspace *work = work_; + struct blasfeo_dvec *ux = memory->ux; + ocp_nlp_cost_nls_cast_workspace(config_, dims, opts_, work_); int nx = dims->nx; @@ -896,10 +884,10 @@ void ocp_nlp_cost_nls_compute_fun(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; - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; nls_y_fun_type_in[0] = BLASFEO_DVEC_ARGS; @@ -936,8 +924,8 @@ void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_, // slack update function value blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); - blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0); - memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, ux, nu+nx, &work->tmp_2ns, 0); + memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, ux, nu+nx); // scale if (model->scaling!=1.0) @@ -974,7 +962,6 @@ void ocp_nlp_cost_nls_config_initialize_default(void *config_) config->memory_get_W_chol_ptr = &ocp_nlp_cost_nls_memory_get_W_chol_ptr; config->model_get_y_ref_ptr = &ocp_nlp_cost_nls_model_get_y_ref_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_nls_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_cost_nls_memory_set_tmp_ux_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_cost_nls_memory_set_z_alg_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_cost_nls_memory_set_RSQrq_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.h b/acados/ocp_nlp/ocp_nlp_cost_nls.h index a1582fda39..5884ebc0e0 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -141,7 +141,6 @@ typedef struct struct blasfeo_dvec res; // nls residual r(x) struct blasfeo_dvec grad; // gradient of cost function struct blasfeo_dvec *ux; // pointer to ux in nlp_out - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *Z; // pointer to Z in qp_in struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out @@ -166,8 +165,6 @@ void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); // void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); // -void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); -// void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); @@ -203,7 +200,8 @@ void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void * // void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); // -void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); +void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, + void *memory_, void *work_); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/acados/ocp_nlp/ocp_nlp_dynamics_common.h index 6fbc0b88c3..56e7463605 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_common.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -85,11 +85,8 @@ typedef struct struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_); struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_); void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_); - void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_); void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_); - void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_); void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_); - void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_); void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_); void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_); void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index d4b3c4f9f3..86be433293 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -449,17 +449,6 @@ void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memo -void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_dynamics_cont_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; - - return; -} - - - void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory_) { ocp_nlp_dynamics_cont_memory *memory = memory_; @@ -470,18 +459,6 @@ void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *me } - -void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory_) -{ - ocp_nlp_dynamics_cont_memory *memory = memory_; - - memory->tmp_ux1 = tmp_ux1; - - return; -} - - - void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory_) { ocp_nlp_dynamics_cont_memory *memory = memory_; @@ -493,17 +470,6 @@ void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memo -void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory_) -{ - ocp_nlp_dynamics_cont_memory *memory = memory_; - - memory->tmp_pi = tmp_pi; - - return; -} - - - void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory_) { ocp_nlp_dynamics_cont_memory *memory = memory_; @@ -906,6 +872,9 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, ocp_nlp_dynamics_cont_memory *mem = mem_; ocp_nlp_dynamics_cont_model *model = model_; + struct blasfeo_dvec *ux = mem->ux; + struct blasfeo_dvec *ux1 = mem->ux1; + int nx = dims->nx; int nu = dims->nu; // int nz = dims->nz; @@ -917,8 +886,8 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, work->sim_in->T = model->T; // pass state and control to integrator - blasfeo_unpack_dvec(nu, mem->tmp_ux, 0, work->sim_in->u, 1); - blasfeo_unpack_dvec(nx, mem->tmp_ux, nu, work->sim_in->x, 1); + blasfeo_unpack_dvec(nu, ux, 0, work->sim_in->u, 1); + blasfeo_unpack_dvec(nx, ux, nu, work->sim_in->x, 1); // printf("sim_guess, bool %d\n", mem->set_sim_guess[0]); // blasfeo_print_exp_dvec(nx + nz, mem->sim_guess, 0); @@ -952,16 +921,14 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_adj", &sens_adj_bkp); config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_hess", &sens_hess_bkp); - // function + // fun = integrator(x, u) - x[next_stage] blasfeo_pack_dvec(nx1, work->sim_out->xn, 1, &mem->fun, 0); - // fun -= x[next_stage] - blasfeo_daxpy(nx1, -1.0, mem->tmp_ux1, nu1, &mem->fun, 0, &mem->fun, 0); + blasfeo_daxpy(nx1, -1.0, ux1, nu1, &mem->fun, 0, &mem->fun, 0); // blasfeo_pack_dvec(nz, work->sim_out->zn, 1, mem->z_alg, 0); // printf("\ndyn_cont: compute f:\n"); // blasfeo_print_exp_tran_dvec(nx1, &mem->fun, 0); return; - } @@ -1018,11 +985,8 @@ void ocp_nlp_dynamics_cont_config_initialize_default(void *config_) config->memory_get_adj_ptr = &ocp_nlp_dynamics_cont_memory_get_adj_ptr; config->memory_set = &ocp_nlp_dynamics_cont_memory_set; config->memory_set_ux_ptr = &ocp_nlp_dynamics_cont_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr; config->memory_set_ux1_ptr = &ocp_nlp_dynamics_cont_memory_set_ux1_ptr; - config->memory_set_tmp_ux1_ptr = &ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr; config->memory_set_pi_ptr = &ocp_nlp_dynamics_cont_memory_set_pi_ptr; - config->memory_set_tmp_pi_ptr = &ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_dynamics_cont_memory_set_BAbt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_dynamics_cont_memory_set_RSQrq_ptr; config->memory_set_dzduxt_ptr = &ocp_nlp_dynamics_cont_memory_set_dzduxt_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 89867fa5a2..c4226cf6f8 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -108,11 +108,8 @@ typedef struct struct blasfeo_dvec fun; struct blasfeo_dvec adj; struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage - struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage - struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dvec *z_alg; // pointer to output z at t = 0 @@ -137,16 +134,10 @@ struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory); // void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); // -void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); -// void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); // -void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); -// void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); // -void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); -// void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c index f51efaf7ed..af8beb0246 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c @@ -365,18 +365,6 @@ void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memo } - -void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_) -{ - ocp_nlp_dynamics_disc_memory *memory = memory_; - - memory->tmp_ux = tmp_ux; - - return; -} - - - void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory_) { ocp_nlp_dynamics_disc_memory *memory = memory_; @@ -387,18 +375,6 @@ void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *me } - -void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory_) -{ - ocp_nlp_dynamics_disc_memory *memory = memory_; - - memory->tmp_ux1 = tmp_ux1; - - return; -} - - - void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory_) { ocp_nlp_dynamics_disc_memory *memory = memory_; @@ -410,16 +386,6 @@ void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memo -void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory_) -{ - ocp_nlp_dynamics_disc_memory *memory = memory_; - - memory->tmp_pi = tmp_pi; - - return; -} - - void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory_) { @@ -762,13 +728,16 @@ void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims_, void *model_, ext_fun_arg_t ext_fun_type_out[1]; void *ext_fun_out[1]; + struct blasfeo_dvec *ux = memory->ux; + struct blasfeo_dvec *ux1 = memory->ux1; + // pass state and control to integrator struct blasfeo_dvec_args x_in; // input x of external fun; - x_in.x = memory->tmp_ux; + x_in.x = ux; x_in.xi = nu; struct blasfeo_dvec_args u_in; // input u of external fun; - u_in.x = memory->tmp_ux; + u_in.x = ux; u_in.xi = 0; struct blasfeo_dvec_args fun_out; @@ -787,7 +756,7 @@ void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims_, void *model_, model->disc_dyn_fun->evaluate(model->disc_dyn_fun, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); // fun - blasfeo_daxpy(nx1, -1.0, memory->tmp_ux1, nu1, &memory->fun, 0, &memory->fun, 0); + blasfeo_daxpy(nx1, -1.0, ux1, nu1, &memory->fun, 0, &memory->fun, 0); return; } @@ -824,11 +793,8 @@ void ocp_nlp_dynamics_disc_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_dynamics_disc_memory_get_fun_ptr; config->memory_get_adj_ptr = &ocp_nlp_dynamics_disc_memory_get_adj_ptr; config->memory_set_ux_ptr = &ocp_nlp_dynamics_disc_memory_set_ux_ptr; - config->memory_set_tmp_ux_ptr = &ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr; config->memory_set_ux1_ptr = &ocp_nlp_dynamics_disc_memory_set_ux1_ptr; - config->memory_set_tmp_ux1_ptr = &ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr; config->memory_set_pi_ptr = &ocp_nlp_dynamics_disc_memory_set_pi_ptr; - config->memory_set_tmp_pi_ptr = &ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_dynamics_disc_memory_set_BAbt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_dynamics_disc_memory_set_RSQrq_ptr; config->memory_set_dzduxt_ptr = &ocp_nlp_dynamics_disc_memory_set_dzduxt_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/acados/ocp_nlp/ocp_nlp_dynamics_disc.h index 9f00796ab5..9d2f52f800 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_disc.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_disc.h @@ -102,11 +102,8 @@ typedef struct struct blasfeo_dvec fun; struct blasfeo_dvec adj; struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage - struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage - struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage - struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in } ocp_nlp_dynamics_disc_memory; @@ -122,16 +119,10 @@ struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory); // void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); // -void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); -// void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); // -void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); -// void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); // -void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); -// void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); 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 ff4d2d037e..e554a1049e 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 @@ -223,14 +223,8 @@ def solve_marathos_ocp(setting): if sqp_iter != 18: raise Exception(f"acados solver took {sqp_iter} iterations, expected 18.") elif globalization == "MERIT_BACKTRACKING": - if globalization_use_SOC == 1 and line_search_use_sufficient_descent == 0 and sqp_iter not in range(21, 23): - raise Exception(f"acados solver took {sqp_iter} iterations, expected range(21, 23).") - elif globalization_use_SOC == 1 and line_search_use_sufficient_descent == 1 and sqp_iter not in range(21, 24): - raise Exception(f"acados solver took {sqp_iter} iterations, expected range(21, 24).") - elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 0 and sqp_iter not in range(155, 165): - raise Exception(f"acados solver took {sqp_iter} iterations, expected range(155, 165).") - elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 1 and sqp_iter not in range(160, 175): - raise Exception(f"acados solver took {sqp_iter} iterations, expected range(160, 175).") + if sqp_iter not in range(17, 23): + raise Exception(f"acados solver took {sqp_iter} iterations, expected range(17, 23).") if PLOT: plot_linear_mass_system_X_state_space(simX, circle=circle, x_goal=x_goal) From 44fb8f4a6a90737af03c21e2632626c8efbf9f21 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Mon, 4 Mar 2024 18:15:56 +0100 Subject: [PATCH 08/16] Fix a few typos regarding the least squares cost formulation (MATLAB examples, Python docs) (#1044) --- .../linear_mpc/example_closed_loop.m | 17 ++++-- .../linear_mpc/quadcopter.m | 58 ++++++++++++------- .../pendulum_on_cart_model.m | 4 +- .../acados_template/acados_ocp_cost.py | 8 +-- 4 files changed, 54 insertions(+), 33 deletions(-) diff --git a/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m b/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m index a5d67beee7..b46ef70f45 100644 --- a/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m +++ b/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m @@ -34,13 +34,13 @@ check_acados_requirements() %% create the controller -model = quadcopter(); % system dynamics -[nx, nu] = size(model.Bd); % state and input dimensions - h = 1/14; % [s] sampling time N_ocp = 10; % [-] number of prediction steps T = N_ocp*h; % [s] prediction horizon length +model = quadcopter(h); % system dynamics +[nx, nu] = size(model.Bd); % state and input dimensions + % ocp model ocp_model = acados_ocp_model(); ocp_model.set('name', 'quad_mpc'); @@ -54,14 +54,21 @@ 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_0', model.Vu_0); + ocp_model.set('cost_Vx_0', model.Vx_0); + ocp_model.set('cost_W_0', model.W_0); + ocp_model.set('cost_y_ref_0', model.y_ref_0); + ocp_model.set('cost_Vu', model.Vu); ocp_model.set('cost_Vx', model.Vx); - ocp_model.set('cost_Vx_e', model.Vx_e); ocp_model.set('cost_W', model.W); - ocp_model.set('cost_W_e', model.W_e); ocp_model.set('cost_y_ref', model.y_ref); + + ocp_model.set('cost_Vx_e', model.Vx_e); + ocp_model.set('cost_W_e', model.W_e); ocp_model.set('cost_y_ref_e', model.y_ref_e); else + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end diff --git a/examples/acados_matlab_octave/linear_mpc/quadcopter.m b/examples/acados_matlab_octave/linear_mpc/quadcopter.m index 0ff4cf02b4..060c27bb7a 100644 --- a/examples/acados_matlab_octave/linear_mpc/quadcopter.m +++ b/examples/acados_matlab_octave/linear_mpc/quadcopter.m @@ -26,13 +26,13 @@ % 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.; - % % this function implements the quadcopter model from https://osqp.org/docs/examples/mpc.html % more infomation can be found here: https://github.com/orgs/osqp/discussions/558 -function model = quadcopter() +function model = quadcopter(h) +% input: sampling time import casadi.* % system matrices @@ -73,27 +73,36 @@ % cost matrices Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]); % state cost -R = 0.1*eye(4); % input cost +R = 0.1*eye(nu); % input cost % generic cost formulation -cost_expr_ext_cost_e = (sym_x-xr)'*Q*(sym_x-xr); % terminal cost -cost_expr_ext_cost = cost_expr_ext_cost_e + sym_u'*R*sym_u; % stage cost -h = 1/14; % [s] sampling time +cost_expr_ext_cost_e = (sym_x-xr)'*Q*(sym_x-xr); % terminal cost (only states) +cost_expr_ext_cost = cost_expr_ext_cost_e + sym_u'*R*sym_u; % stage cost (states and inputs) cost_expr_ext_cost = 1/h * cost_expr_ext_cost; % scale the stage cost to match the discrete formulation +cost_expr_ext_cost_0 = 1/h * sym_u'*R*sym_u; % penalize only the inputs in the first stage % more info on discrete cost scaling: -% https://discourse.acados.org/t/question-regarding-terminal-cost-in-discrete-time/1096 +% https://docs.acados.org/python_interface/index.html#acados_template.acados_ocp_cost.AcadosOcpCost % linear least-squares cost formulation (alternative) -ny = nu+nx; % number of outputs in the stage cost -Vu = [eye(nu); zeros(nx,nu)]; % input-to-output matrix in the stage cost -Vx = [zeros(nu, nx); eye(nx)]; % state-to-output matrix in the stage cost -W = blkdiag(R,Q); % weight matrix in the stage cost -y_ref = zeros(ny, 1); % output reference in the stage cost - -ny_e = nx; % number of outputs in the terminal cost -Vx_e = eye(ny_e, nx); % state-to-output matrix in the terminal cost -W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in the terminal cost -y_ref_e = zeros(ny_e, 1); % output reference in the terminal cost +% initial cost +ny_0 = nu; +Vu_0 = eye(ny_0); +Vx_0 = zeros(ny_0,nx); +W_0 = 1/h * 2 * R; % scale to match the original cost +y_ref_0 = zeros(ny_0,1); + +% intermediate cost +ny = nu+nx; +Vu = [eye(nu); zeros(nx,nu)]; +Vx = [zeros(nu, nx); eye(nx)]; +W = 1/h * 2 * blkdiag(R,Q); % scale to match the original cost +y_ref = zeros(ny, 1); + +% terminal cost +ny_e = nx; +Vx_e = eye(ny_e, nx); +W_e = 2 * Q; % terminal cost is not scaled with h later +y_ref_e = zeros(ny_e, 1); % input constraints u0 = 10.5916; % steady-state input @@ -102,6 +111,7 @@ ubu = [13; 13; 13; 13] - u0; % input upper bounds % state constraints on the first, second and sixth state +% (not applied to the initial state) Jbx = zeros(3,nx); Jbx(1,1) = 1; Jbx(2,2) = 1; @@ -127,15 +137,22 @@ model.dyn_expr_phi = dyn_expr_phi; +model.cost_expr_ext_cost_0 = cost_expr_ext_cost_0; model.cost_expr_ext_cost = cost_expr_ext_cost; model.cost_expr_ext_cost_e = cost_expr_ext_cost_e; +model.Vu_0 = Vu_0; +model.Vx_0 = Vx_0; +model.W_0 = W_0; +model.y_ref_0 = y_ref_0; + model.Vu = Vu; model.Vx = Vx; -model.Vx_e = Vx_e; model.W = W; -model.W_e = W_e; model.y_ref = y_ref; + +model.Vx_e = Vx_e; +model.W_e = W_e; model.y_ref_e = y_ref_e; model.Jbu = Jbu; @@ -148,6 +165,3 @@ model.lbx_e = lbx_e; model.ubx_e = ubx_e; end - - - diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m index 02932f3d85..b8fd563ed4 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m @@ -75,8 +75,8 @@ %% cost W_x = diag([1e3, 1e3, 1e-2, 1e-2]); W_u = 1e-2; -expr_ext_cost_e = sym_x'* W_x * sym_x; -expr_ext_cost = expr_ext_cost_e + sym_u' * W_u * sym_u; +expr_ext_cost_e = 0.5 * sym_x'* W_x * sym_x; +expr_ext_cost = expr_ext_cost_e + 0.5 * sym_u' * W_u * sym_u; % nonlinear least sqares cost_expr_y = vertcat(sym_x, sym_u); W = blkdiag(W_x, W_u); diff --git a/interfaces/acados_template/acados_template/acados_ocp_cost.py b/interfaces/acados_template/acados_template/acados_ocp_cost.py index 89e910a9ba..c24d71bc4a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_cost.py +++ b/interfaces/acados_template/acados_template/acados_ocp_cost.py @@ -43,15 +43,15 @@ class AcadosOcpCost: In case of LINEAR_LS: stage cost is - :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, + :math:`l(x,u,z) = 0.5 \cdot || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, terminal cost is - :math:`m(x) = || V^e_x \, x - y_\\text{ref}^e||^2_{W^e}` + :math:`m(x) = 0.5 \cdot || V^e_x \, x - y_\\text{ref}^e||^2_{W^e}` In case of NONLINEAR_LS: stage cost is - :math:`l(x,u,z,p) = || y(x,u,z,p) - y_\\text{ref}||^2_W`, + :math:`l(x,u,z,p) = 0.5 \cdot || y(x,u,z,p) - y_\\text{ref}||^2_W`, terminal cost is - :math:`m(x,p) = || y^e(x,p) - y_\\text{ref}^e||^2_{W^e}` + :math:`m(x,p) = 0.5 \cdot || y^e(x,p) - y_\\text{ref}^e||^2_{W^e}` In case of CONVEX_OVER_NONLINEAR: stage cost is From 4cfab342188b3fe80f83d041f008c07f83b882ac Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 5 Mar 2024 16:43:03 +0100 Subject: [PATCH 09/16] Minor C changes (#1045) - formatting: tabs to spaces - move cost integration memory preparation to precompute --- acados/ocp_nlp/ocp_nlp_common.c | 40 +++++++++++--------------- acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 22 +++++++------- 2 files changed, 28 insertions(+), 34 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 916ced495e..7bb0e1eef1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1970,6 +1970,23 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->dynamics[i]->memory_set_sim_guess_ptr(nlp_mem->sim_guess+i, nlp_mem->set_sim_guess+i, nlp_mem->dynamics[i]); // NOTE: no z at terminal stage, since dynamics modules dont compute it. config->dynamics[i]->memory_set_z_alg_ptr(nlp_mem->z_alg+i, nlp_mem->dynamics[i]); + + int cost_integration; + config->dynamics[i]->opts_get(config->dynamics[i], opts->dynamics[i], + "cost_computation", &cost_integration); + if (cost_integration) + { + // set pointers to cost function & gradient in integrator + double *cost_fun = config->cost[i]->memory_get_fun_ptr(nlp_mem->cost[i]); + struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(nlp_mem->cost[i]); + struct blasfeo_dvec *y_ref = config->cost[i]->model_get_y_ref_ptr(nlp_in->cost[i]); + struct blasfeo_dmat *W_chol = config->cost[i]->memory_get_W_chol_ptr(nlp_mem->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); + config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "y_ref", y_ref); + config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "W_chol", W_chol); + } } // alias to cost_memory @@ -2102,29 +2119,7 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, int *nx = dims->nx; int *nu = dims->nu; - /* prepare memory */ - int cost_integration; - for (int i = 0; i < N; i++) - { - config->dynamics[i]->opts_get(config->dynamics[i], opts->dynamics[i], - "cost_computation", &cost_integration); - if (cost_integration) - { - // set pointers to cost function & gradient in integrator - double *cost_fun = config->cost[i]->memory_get_fun_ptr(mem->cost[i]); - struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); - struct blasfeo_dvec *y_ref = config->cost[i]->model_get_y_ref_ptr(in->cost[i]); - struct blasfeo_dmat *W_chol = config->cost[i]->memory_get_W_chol_ptr(mem->cost[i]); - - config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "cost_grad", cost_grad); - config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "cost_fun", cost_fun); - config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "y_ref", y_ref); - config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], mem->dynamics[i], "W_chol", W_chol); - } - } - /* stage-wise multiple shooting lagrangian evaluation */ - #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for #endif @@ -2165,7 +2160,6 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, } /* collect stage-wise evaluations */ - #if defined(ACADOS_WITH_OPENMP) #pragma omp parallel for #endif diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 86be433293..6d01562ab6 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -252,7 +252,7 @@ void ocp_nlp_dynamics_cont_opts_initialize_default(void *config_, void *dims_, v // sim opts config->sim_solver->opts_initialize_default(config->sim_solver, dims->sim, opts->sim_solver); - // overwrite defaults + // overwrite defaults bool sens_forw = true; bool sens_adj = false; @@ -296,7 +296,7 @@ void ocp_nlp_dynamics_cont_opts_set(void *config_, void *opts_, const char *fiel { int *int_ptr = value; opts->compute_adj = *int_ptr; - // TODO set in the sim solver too ??? + // TODO set in the sim solver too ??? } else if (!strcmp(field, "compute_hess")) { @@ -552,16 +552,16 @@ void ocp_nlp_dynamics_cont_memory_get(void *config_, void *dims_, void *mem_, co ocp_nlp_dynamics_cont_dims *dims = dims_; ocp_nlp_dynamics_cont_memory *mem = mem_; - sim_config *sim = config->sim_solver; + sim_config *sim = config->sim_solver; if (!strcmp(field, "time_sim") || !strcmp(field, "time_sim_ad") || !strcmp(field, "time_sim_la")) { - sim->memory_get(sim, dims->sim, mem->sim_solver, field, value); + sim->memory_get(sim, dims->sim, mem->sim_solver, field, value); } else { - printf("\nerror: ocp_nlp_dynamics_cont_memory_get: field %s not available\n", field); - exit(1); + printf("\nerror: ocp_nlp_dynamics_cont_memory_get: field %s not available\n", field); + exit(1); } } @@ -900,14 +900,14 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, mem->set_sim_guess[0] = false; } - // backup sens options - bool sens_forw_bkp, sens_adj_bkp, sens_hess_bkp; + // backup sens options + bool sens_forw_bkp, sens_adj_bkp, sens_hess_bkp; config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_forw", &sens_forw_bkp); config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_adj", &sens_adj_bkp); config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_hess", &sens_hess_bkp); - // set all sens to false - bool sens_all = false; + // set all sens to false + bool sens_all = false; config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_forw", &sens_all); config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_adj", &sens_all); config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_hess", &sens_all); @@ -916,7 +916,7 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, config->sim_solver->evaluate(config->sim_solver, work->sim_in, work->sim_out, opts->sim_solver, mem->sim_solver, work->sim_solver); - // restore sens options + // restore sens options config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_forw", &sens_forw_bkp); config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_adj", &sens_adj_bkp); config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_hess", &sens_hess_bkp); From f4ea1cd73b96b38ecb9bc0caee38d737496b96d5 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 14 Mar 2024 11:34:59 +0100 Subject: [PATCH 10/16] Implement advanced-step real-time iteration: AS-RTI (#1046) This pull request implements the advanced-step real-time iteration (AS-RTI) It is implemented as a feature within the `RTI` module. In order to use it, set the new options: - `as_rti_level` - `as_rti_iter` The feedback phase of RTI and AS-RTI is the same. The AS-RTI preparation phase is used instead of the "standard" RTI preparation phase, when the options above are set. Additionally, this adds the flag: - `rti_log_residuals`: which allows to log the residuals within (AS-)RTI. This was needed for the experiment in the paper, which reports the residuals of the inner iterations. Limited to convex-over-nonlinear and external cost modules for now. The work in this PR is presented in the publication "Advanced-Step Real-time Iterations with Four Levels -- New Error Bounds and Fast Implementation in acados" by Jonathan Frey, Armin Nurkanovic, Moritz Diehl https://arxiv.org/abs/2403.07101 Limitations: - AS-RTI-B not implemented for ns > 0. - AS-RTI-C only implemented for linear inequality constraints. --- acados/ocp_nlp/ocp_nlp_common.c | 160 +++- acados/ocp_nlp/ocp_nlp_common.h | 10 + acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 7 + acados/ocp_nlp/ocp_nlp_cost_common.h | 1 + acados/ocp_nlp/ocp_nlp_cost_conl.c | 114 +++ acados/ocp_nlp/ocp_nlp_cost_external.c | 77 ++ acados/ocp_nlp/ocp_nlp_cost_external.h | 4 +- acados/ocp_nlp/ocp_nlp_cost_ls.c | 8 + acados/ocp_nlp/ocp_nlp_cost_nls.c | 10 + acados/ocp_nlp/ocp_nlp_dynamics_common.h | 1 + acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 83 ++ acados/ocp_nlp/ocp_nlp_dynamics_cont.h | 2 + acados/ocp_nlp/ocp_nlp_dynamics_disc.c | 71 ++ acados/ocp_nlp/ocp_nlp_sqp.h | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 732 +++++++++++++++++- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 20 + acados/sim/sim_erk_integrator.c | 6 +- .../as_rti/as_rti_closed_loop_example.py | 202 +++++ .../pendulum_on_cart/common/utils.py | 6 +- .../mass_spring_example.c | 4 + examples/c/sim_gnsf_crane.c | 2 + interfaces/CMakeLists.txt | 4 + .../ocp_nlp_solver_options_json.m | 6 + .../acados_template/acados_ocp_options.py | 61 ++ .../acados_template/acados_ocp_solver.py | 54 +- .../c_templates_tera/acados_multi_solver.in.c | 9 + .../c_templates_tera/acados_solver.in.c | 10 + 27 files changed, 1597 insertions(+), 69 deletions(-) create mode 100644 examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 7bb0e1eef1..5294e48675 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1659,6 +1659,9 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d // constraints size += (N+1)*sizeof(void *); + // doubles + size += nxu_max * sizeof(double); // tmp_nxu_double + // module workspace if (opts->reuse_workspace) { @@ -1749,6 +1752,7 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d } size += 8; // struct align + size += 64; // blasfeo align return size; } @@ -1768,6 +1772,15 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims int *nu = dims->nu; int *ni = dims->ni; // int *nz = dims->nz; + int nxu_max = 0; + int nx_max = 0; + int ni_max = 0; + for (int i = 0; i <= N; i++) + { + nx_max = nx_max > nx[i] ? nx_max : nx[i]; + nxu_max = nxu_max > (nx[i]+nu[i]) ? nxu_max : (nx[i]+nu[i]); + ni_max = ni_max > ni[i] ? ni_max : ni[i]; + } char *c_ptr = (char *) raw_memory; @@ -1796,16 +1809,11 @@ 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); + assign_and_advance_double(nxu_max, &work->tmp_nxu_double, &c_ptr); + // align for blasfeo mem + align_char_to(64, &c_ptr); + // blasfeo_dvec - int nxu_max = 0; - int nx_max = 0; - int ni_max = 0; - for (int i = 0; i <= N; i++) - { - nx_max = nx_max > nx[i] ? nx_max : nx[i]; - nxu_max = nxu_max > (nx[i]+nu[i]) ? nxu_max : (nx[i]+nu[i]); - ni_max = ni_max > ni[i] ? ni_max : ni[i]; - } assign_and_advance_blasfeo_dvec_mem(nxu_max, &work->tmp_nxu, &c_ptr); assign_and_advance_blasfeo_dvec_mem(ni_max, &work->tmp_ni, &c_ptr); assign_and_advance_blasfeo_dvec_mem(nx_max, &work->dxnext_dy, &c_ptr); @@ -2191,6 +2199,7 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, } if (i > 0) { + // TODO: this could be simplified by not copying pi in the dynamics module. struct blasfeo_dvec *dyn_adj = config->dynamics[i-1]->memory_get_adj_ptr(mem->dynamics[i-1]); blasfeo_daxpy(nx[i], 1.0, dyn_adj, nu[i-1]+nx[i-1], mem->dyn_adj+i, nu[i], @@ -2247,6 +2256,134 @@ void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, } +// zero order update QP: Update all constraint evaluations in QP +void ocp_nlp_zero_order_qp_update(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; + // int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + int *ni = dims->ni; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + // evaluate constraint residuals + 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 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->qp_in->d + i, 0); + // copy into nlp_mem + blasfeo_dveccp(2 * ni[i], ineq_fun, 0, mem->ineq_fun + i, 0); + } + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i=0; idynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i], in->dynamics[i], + opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); + + struct blasfeo_dvec *dyn_fun = config->dynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); + blasfeo_dveccp(nx[i + 1], dyn_fun, 0, mem->qp_in->b + i, 0); + blasfeo_dveccp(nx[i + 1], dyn_fun, 0, mem->dyn_fun + i, 0); + } + + // add gradient correction + // rqz += Hess * last_step = RQ * qp_out + for (int i = 0; i <= N; i++) + { + // NOTE: only lower triagonal of RSQ is stored + blasfeo_dsymv_l_mn(nx[i]+nu[i], nx[i]+nu[i], 1.0, mem->qp_in->RSQrq+i, 0, 0, + mem->qp_out->ux+i, 0, 1.0, mem->qp_in->rqz+i, 0, mem->qp_in->rqz+i, 0); + // TODO: fix for ns > 0. + } +} + + +// Level C iterations Update all constraint evaluations in QP and Lagrange gradient +void ocp_nlp_level_c_update(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; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + int *ni = dims->ni; + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i = 0; i <= N; i++) + { + // evaluate constraint residuals + 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 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->qp_in->d + i, 0); + // copy into nlp_mem + blasfeo_dveccp(2 * ni[i], ineq_fun, 0, mem->ineq_fun + i, 0); + } + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i=0; i<=N; i++) + { + // nlp mem: cost_grad + config->cost[i]->compute_gradient(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i], mem->cost[i], work->cost[i]); + struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); + blasfeo_dveccp(nv[i], cost_grad, 0, mem->cost_grad + i, 0); + blasfeo_dveccp(nv[i], mem->cost_grad + i, 0, mem->qp_in->rqz + i, 0); + } + + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i=0; idynamics[i]->update_qp_matrices(config->dynamics[i], dims->dynamics[i], in->dynamics[i], + config->dynamics[i]->compute_fun_and_adjoint(config->dynamics[i], dims->dynamics[i], in->dynamics[i], + opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); + + struct blasfeo_dvec *dyn_fun = config->dynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); + blasfeo_dveccp(nx[i + 1], dyn_fun, 0, mem->qp_in->b + i, 0); + blasfeo_dveccp(nx[i + 1], dyn_fun, 0, mem->dyn_fun + i, 0); + + // add adjoint contribution to gradient + struct blasfeo_dvec *dyn_adj = config->dynamics[i]->memory_get_adj_ptr(mem->dynamics[i]); + blasfeo_dvecad(nu[i] + nx[i], -1.0, dyn_adj, 0, mem->qp_in->rqz+i, 0); + // add adjoint contribution C * lambda_k + blasfeo_dgemv_n(nu[i] + nx[i], nx[i+1], -1.0, mem->qp_in->BAbt+i, 0, 0, out->pi+i, 0, 1.0, mem->qp_in->rqz+i, 0, mem->qp_in->rqz+i, 0); + + // - I part is linear, so dont need to add that! + // blasfeo_dvecad(nx[i+1], 1.0, out->pi+i, 0, mem->qp_in->rqz+i, 0) + + // DEBUG: + // printf("\ndyn_adj i %d\n", i); + // blasfeo_print_exp_tran_dvec(nu[i] + nx[i], dyn_adj, 0); + // blasfeo_dgemv_n(nu[i] + nx[i], nx[i+1], 1.0, mem->qp_in->BAbt+i, 0, 0, out->pi+i, 0, 0.0, &work->tmp_nxu, 0, &work->tmp_nxu, 0); + // printf("C * lam\n"); + // blasfeo_print_exp_tran_dvec(nu[i] + nx[i], &work->tmp_nxu, 0); + } + + // TODO: + // - adjoint call for inequalities as for dynamics +} + + + 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) @@ -2952,8 +3089,8 @@ void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, o // res_stat for (int i = 0; i <= N; i++) { - blasfeo_daxpy(nv[i], -1.0, mem->ineq_adj + i, 0, mem->cost_grad + i, 0, res->res_stat + i, - 0); + blasfeo_daxpy(nv[i], -1.0, mem->ineq_adj + i, 0, mem->cost_grad + i, 0, + res->res_stat + i, 0); blasfeo_daxpy(nu[i] + nx[i], -1.0, mem->dyn_adj + i, 0, res->res_stat + i, 0, res->res_stat + i, 0); blasfeo_dvecnrm_inf(nv[i], res->res_stat + i, 0, &tmp_res); @@ -3041,4 +3178,3 @@ void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in // printf("\ncomputed total cost: %e\n", total_cost); } - diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index b6098edb80..69f97a2609 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -384,6 +384,8 @@ typedef struct ocp_nlp_workspace struct blasfeo_dvec tmp_nxu; struct blasfeo_dvec tmp_ni; struct blasfeo_dvec dxnext_dy; + // AS-RTI + double *tmp_nxu_double; } ocp_nlp_workspace; @@ -411,6 +413,14 @@ void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, void ocp_nlp_approximate_qp_vectors_sqp(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_zero_order_qp_update(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_level_c_update(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_update_variables_sqp(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); // diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 29ff7d65b3..6b37d65a76 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -1479,13 +1479,20 @@ void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims_, void // nlp_mem: ineq_adj if (opts->compute_adj) { + // adj = zeros(nu+nx+2*ns) blasfeo_dvecse(nu+nx+2*ns, 0.0, &memory->adj, 0); + // tmp_ni = - lam_lower + lam_upper blasfeo_daxpy(nb+ng+nh, -1.0, memory->lam, nb+ng+nh, memory->lam, 0, &work->tmp_ni, 0); + // adj[idxb] += tmp_ni[:nb] blasfeo_dvecad_sp(nb, 1.0, &work->tmp_ni, 0, model->idxb, &memory->adj, 0); + // 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); + // 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); } diff --git a/acados/ocp_nlp/ocp_nlp_cost_common.h b/acados/ocp_nlp/ocp_nlp_cost_common.h index 19120dc4a5..9c9a930a44 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -87,6 +87,7 @@ typedef struct void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); // computes the cost function value (intended for globalization) void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*compute_gradient)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); void (*config_initialize_default)(void *config); void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c index da2ada2436..9c21a49776 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.c +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.c @@ -712,6 +712,119 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode + +void ocp_nlp_cost_conl_compute_gradient(void *config_, void *dims_, void *model_, void *opts_, + void *memory_, void *work_) +{ + ocp_nlp_cost_conl_dims *dims = dims_; + ocp_nlp_cost_conl_model *model = model_; + ocp_nlp_cost_conl_memory *memory = memory_; + ocp_nlp_cost_conl_workspace *work = work_; + ocp_nlp_cost_conl_opts *opts = (ocp_nlp_cost_conl_opts *) opts_; + + ocp_nlp_cost_conl_cast_workspace(config_, dims, opts_, work_); + + int nx = dims->nx; + int nz = dims->nz; + int nu = dims->nu; + int ny = dims->ny; + int ns = dims->ns; + + if (opts->integrator_cost == 0) + { + ext_fun_arg_t conl_fun_jac_hess_type_in[5]; + void *conl_fun_jac_hess_in[5]; + ext_fun_arg_t conl_fun_jac_hess_type_out[5]; + void *conl_fun_jac_hess_out[5]; + + // INPUT + struct blasfeo_dvec_args x_in; // input x + x_in.x = memory->ux; + x_in.xi = nu; + + struct blasfeo_dvec_args u_in; // input u + u_in.x = memory->ux; + u_in.xi = 0; + + conl_fun_jac_hess_type_in[0] = BLASFEO_DVEC_ARGS; + conl_fun_jac_hess_in[0] = &x_in; + conl_fun_jac_hess_type_in[1] = BLASFEO_DVEC_ARGS; + conl_fun_jac_hess_in[1] = &u_in; + + conl_fun_jac_hess_type_in[2] = BLASFEO_DVEC; + conl_fun_jac_hess_in[2] = memory->z_alg; + conl_fun_jac_hess_type_in[3] = BLASFEO_DVEC; + conl_fun_jac_hess_in[3] = &model->y_ref; + + conl_fun_jac_hess_type_in[4] = COLMAJ; + conl_fun_jac_hess_in[4] = &model->t; + + // OUTPUT + conl_fun_jac_hess_type_out[0] = COLMAJ; + conl_fun_jac_hess_out[0] = &memory->fun; // fun: scalar + conl_fun_jac_hess_type_out[1] = BLASFEO_DVEC; + conl_fun_jac_hess_out[1] = &work->tmp_ny; // grad of outer loss wrt residual, ny + conl_fun_jac_hess_type_out[2] = BLASFEO_DMAT; + conl_fun_jac_hess_out[2] = &work->Jt_ux; // inner Jacobian wrt ux, transposed, (nu+nx) x ny + conl_fun_jac_hess_type_out[3] = BLASFEO_DMAT; + conl_fun_jac_hess_out[3] = &work->Jt_z; // inner Jacobian wrt z, transposed, nz x ny + conl_fun_jac_hess_type_out[4] = BLASFEO_DMAT; + conl_fun_jac_hess_out[4] = &work->W; // outer hessian: ny x ny + + // NOTE: could be done more efficiently by generating a function that does not evalutate hessian + // evaluate external function + model->conl_cost_fun_jac_hess->evaluate(model->conl_cost_fun_jac_hess, conl_fun_jac_hess_type_in, + conl_fun_jac_hess_in, conl_fun_jac_hess_type_out, conl_fun_jac_hess_out); + + // hessian of outer loss function + // blasfeo_dpotrf_l(ny, &work->W, 0, 0, &memory->W_chol, 0, 0); + + if (nz > 0) + { + // Jt_ux_tilde = Jt_ux + dzdux_tran*Jt_z + blasfeo_dgemm_nn(nu + nx, ny, nz, 1.0, memory->dzdux_tran, 0, 0, + &work->Jt_z, 0, 0, 1.0, &work->Jt_ux, 0, 0, &work->Jt_ux_tilde, 0, 0); + + // 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 + blasfeo_dveccp(2*ns, &model->z, 0, &memory->grad, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); + + // scale + if(model->scaling!=1.0) + { + blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + } + + return; +} + + + + void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { @@ -812,6 +925,7 @@ void ocp_nlp_cost_conl_config_initialize_default(void *config_) config->initialize = &ocp_nlp_cost_conl_initialize; config->update_qp_matrices = &ocp_nlp_cost_conl_update_qp_matrices; config->compute_fun = &ocp_nlp_cost_conl_compute_fun; + config->compute_gradient = &ocp_nlp_cost_conl_compute_gradient; config->config_initialize_default = &ocp_nlp_cost_conl_config_initialize_default; config->precompute = &ocp_nlp_cost_conl_precompute; diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index 6b32da3fe7..172b88ff8e 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -685,6 +685,82 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void * +void ocp_nlp_cost_external_compute_gradient(void *config_, void *dims_, void *model_, void *opts_, + void *memory_, void *work_) +{ + ocp_nlp_cost_external_dims *dims = dims_; + ocp_nlp_cost_external_model *model = model_; + // ocp_nlp_cost_external_opts *opts = opts_; + ocp_nlp_cost_external_memory *memory = memory_; + ocp_nlp_cost_external_workspace *work = work_; + + ocp_nlp_cost_external_cast_workspace(config_, dims, opts_, work_); + + int nx = dims->nx; + int nz = dims->nz; + int nu = dims->nu; + int ns = dims->ns; + + /* specify input types and pointers for external cost function */ + ext_fun_arg_t ext_fun_type_in[3]; + void *ext_fun_in[3]; + ext_fun_arg_t ext_fun_type_out[2]; + void *ext_fun_out[2]; + + // INPUT + struct blasfeo_dvec_args u_in; // input u + u_in.x = memory->ux; + u_in.xi = 0; + struct blasfeo_dvec_args x_in; // input x + x_in.x = memory->ux; + x_in.xi = nu; + + ext_fun_type_in[0] = BLASFEO_DVEC_ARGS; + ext_fun_in[0] = &x_in; + ext_fun_type_in[1] = BLASFEO_DVEC_ARGS; + ext_fun_in[1] = &u_in; + ext_fun_type_in[2] = BLASFEO_DVEC; + ext_fun_in[2] = memory->z_alg; + + // OUTPUT + ext_fun_type_out[0] = COLMAJ; + ext_fun_out[0] = &memory->fun; // fun: scalar + + ext_fun_type_out[1] = BLASFEO_DVEC; + ext_fun_out[1] = &work->tmp_nunxnz; // tmp_nunxnz: nu+nx+nz + + // evaluate external function + 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); + + // gradient + blasfeo_dveccp(nu+nx, &work->tmp_nunxnz, 0, &memory->grad, 0); + if (nz > 0) + { + blasfeo_dgemv_n(nu+nx, nz, 1.0, memory->dzdux_tran, 0, 0, &work->tmp_nunxnz, nu+nx, 1., &memory->grad, 0, &memory->grad, 0); + } + + // slack update gradient + blasfeo_dveccp(2*ns, &model->z, 0, &memory->grad, nu+nx); + blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx); + + // slack update function value + // tmp_2ns = 2 * z + Z .* slack + // blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0); + // blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &work->tmp_2ns, 0); + // fun += .5 * (tmp_2ns .* slack) + // memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->ux, nu+nx); + + // scale + if (model->scaling!=1.0) + { + blasfeo_dvecsc(nu+nx+2*ns, model->scaling, &memory->grad, 0); + // memory->fun *= model->scaling; + } +} + + + void ocp_nlp_cost_external_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { @@ -784,6 +860,7 @@ void ocp_nlp_cost_external_config_initialize_default(void *config_) config->initialize = &ocp_nlp_cost_external_initialize; config->update_qp_matrices = &ocp_nlp_cost_external_update_qp_matrices; config->compute_fun = &ocp_nlp_cost_external_compute_fun; + config->compute_gradient = &ocp_nlp_cost_external_compute_gradient; config->config_initialize_default = &ocp_nlp_cost_external_config_initialize_default; config->precompute = &ocp_nlp_cost_external_precompute; diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.h b/acados/ocp_nlp/ocp_nlp_cost_external.h index 9d98d69554..a12e210511 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -176,7 +176,9 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *m // void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); - +// +void ocp_nlp_cost_external_compute_gradient(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index a673d5bd63..0e6e775a41 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -849,6 +849,13 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, } +void ocp_nlp_cost_ls_compute_gradient(void *config_, void *dims_, void *model_, void *opts_, + void *memory_, void *work_) +{ + printf("\nocp_nlp_cost_ls_compute_gradient not implemented.\n\n"); + exit(1); +} + void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) @@ -957,6 +964,7 @@ void ocp_nlp_cost_ls_config_initialize_default(void *config_) config->initialize = &ocp_nlp_cost_ls_initialize; config->update_qp_matrices = &ocp_nlp_cost_ls_update_qp_matrices; config->compute_fun = &ocp_nlp_cost_ls_compute_fun; + config->compute_gradient = &ocp_nlp_cost_ls_compute_gradient; config->config_initialize_default = &ocp_nlp_cost_ls_config_initialize_default; config->precompute = &ocp_nlp_cost_ls_precompute; diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index 562534943c..bd72a675c4 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -856,6 +856,15 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model +void ocp_nlp_cost_nls_compute_gradient(void *config_, void *dims_, void *model_, void *opts_, + void *memory_, void *work_) +{ + printf("\nocp_nlp_cost_nls_compute_gradient not implemented.\n\n"); + exit(1); +} + + + void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { @@ -970,6 +979,7 @@ void ocp_nlp_cost_nls_config_initialize_default(void *config_) config->initialize = &ocp_nlp_cost_nls_initialize; config->update_qp_matrices = &ocp_nlp_cost_nls_update_qp_matrices; config->compute_fun = &ocp_nlp_cost_nls_compute_fun; + config->compute_gradient = &ocp_nlp_cost_nls_compute_gradient; config->config_initialize_default = &ocp_nlp_cost_nls_config_initialize_default; config->precompute = &ocp_nlp_cost_nls_precompute; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/acados/ocp_nlp/ocp_nlp_dynamics_common.h index 56e7463605..9d77ef4fd0 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_common.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -99,6 +99,7 @@ typedef struct void (*initialize)(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 (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*compute_fun_and_adjoint)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); } ocp_nlp_dynamics_config; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 6d01562ab6..1461f45c4f 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -818,6 +818,7 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims_, void * { blasfeo_dgemv_n(nu+nx, nx1, -1.0, mem->BAbt, 0, 0, mem->pi, 0, 0.0, &mem->adj, 0, &mem->adj, 0); } + // adj[nu+nx: nu+nx+nx1] = pi blasfeo_dveccp(nx1, mem->pi, 0, &mem->adj, nu+nx); } @@ -933,6 +934,87 @@ void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims_, void *model_, + +void ocp_nlp_dynamics_cont_compute_fun_and_adjoint(void *config_, void *dims_, void *model_, void *opts_, void *mem_, void *work_) +{ + ocp_nlp_dynamics_cont_cast_workspace(config_, dims_, opts_, work_, mem_); + + ocp_nlp_dynamics_config *config = config_; + ocp_nlp_dynamics_cont_dims *dims = dims_; + ocp_nlp_dynamics_cont_opts *opts = opts_; + ocp_nlp_dynamics_cont_workspace *work = work_; + ocp_nlp_dynamics_cont_memory *mem = mem_; + ocp_nlp_dynamics_cont_model *model = model_; + + struct blasfeo_dvec *ux = mem->ux; + struct blasfeo_dvec *ux1 = mem->ux1; + + int nx = dims->nx; + int nu = dims->nu; + // int nz = dims->nz; + int nx1 = dims->nx1; + int nu1 = dims->nu1; + + // setup model + work->sim_in->model = model->sim_model; + work->sim_in->T = model->T; + + // pass state and control to integrator + blasfeo_unpack_dvec(nu, ux, 0, work->sim_in->u, 1); + blasfeo_unpack_dvec(nx, ux, nu, work->sim_in->x, 1); + + if (mem->set_sim_guess!=NULL && mem->set_sim_guess[0]) + { + config->sim_solver->memory_set(config->sim_solver, work->sim_in->dims, mem->sim_solver, + "guesses_blasfeo", mem->sim_guess); + // only use/pass the initial guess once + mem->set_sim_guess[0] = false; + } + + // adjoint seed + for (int jj = 0; jj < nx + nu; jj++) + work->sim_in->S_adj[jj] = 0.0; + blasfeo_unpack_dvec(nx1, mem->pi, 0, work->sim_in->S_adj, 1); + + // backup sens options + bool sens_forw_bkp, sens_adj_bkp, sens_hess_bkp; + config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_forw", &sens_forw_bkp); + config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_adj", &sens_adj_bkp); + config->sim_solver->opts_get(config->sim_solver, opts->sim_solver, "sens_hess", &sens_hess_bkp); + + // compute only adjoint sensitivities + bool sens_tmp = false; + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_forw", &sens_tmp); + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_hess", &sens_tmp); + sens_tmp = true; + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_adj", &sens_tmp); + + // call integrator + config->sim_solver->evaluate(config->sim_solver, work->sim_in, work->sim_out, opts->sim_solver, + mem->sim_solver, work->sim_solver); + + // restore sens options + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_forw", &sens_forw_bkp); + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_adj", &sens_adj_bkp); + config->sim_solver->opts_set(config->sim_solver, opts->sim_solver, "sens_hess", &sens_hess_bkp); + + // fun = integrator(x, u) - x[next_stage] + blasfeo_pack_dvec(nx1, work->sim_out->xn, 1, &mem->fun, 0); + blasfeo_daxpy(nx1, -1.0, ux1, nu1, &mem->fun, 0, &mem->fun, 0); + + // pack adjoint + blasfeo_pack_dvec(nu, work->sim_out->S_adj+nx, 1, &mem->adj, 0); + blasfeo_pack_dvec(nx, work->sim_out->S_adj+0, 1, &mem->adj, nu); + blasfeo_dvecsc(nu+nx, -1.0, &mem->adj, 0); + + // adj[nu+nx: nu+nx+nx1] = pi + blasfeo_dveccp(nx1, mem->pi, 0, &mem->adj, nu+nx); + + return; +} + + + int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims_, void *model_, void *opts_, void *mem_, void *work_) { @@ -997,6 +1079,7 @@ void ocp_nlp_dynamics_cont_config_initialize_default(void *config_) config->initialize = &ocp_nlp_dynamics_cont_initialize; config->update_qp_matrices = &ocp_nlp_dynamics_cont_update_qp_matrices; config->compute_fun = &ocp_nlp_dynamics_cont_compute_fun; + config->compute_fun_and_adjoint = &ocp_nlp_dynamics_cont_compute_fun_and_adjoint; config->precompute = &ocp_nlp_dynamics_cont_precompute; config->config_initialize_default = &ocp_nlp_dynamics_cont_config_initialize_default; diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index c4226cf6f8..77043d31c9 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -191,6 +191,8 @@ void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *m // void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); // +void ocp_nlp_dynamics_cont_compute_fun_and_adjoint(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c index af8beb0246..66fc3bb2cf 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_disc.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_disc.c @@ -763,6 +763,76 @@ void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims_, void *model_, +void ocp_nlp_dynamics_disc_compute_fun_and_adjoint(void *config_, void *dims_, void *model_, void *opts_, + void *mem_, void *work_) +{ + /* TODO: this is inefficient! Generate a separate function for discrete dynamics to compute fun and adj! */ + ocp_nlp_dynamics_disc_cast_workspace(config_, dims_, opts_, work_); + + // 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_memory *memory = mem_; + ocp_nlp_dynamics_disc_model *model = model_; + + int nx = dims->nx; + int nu = dims->nu; + int nx1 = dims->nx1; + int nu1 = dims->nu1; + + ext_fun_arg_t ext_fun_type_in[3]; + void *ext_fun_in[3]; + ext_fun_arg_t ext_fun_type_out[3]; + void *ext_fun_out[3]; + + // pass state and control to integrator + struct blasfeo_dvec_args x_in; // input x of external fun; + x_in.x = memory->ux; + x_in.xi = nu; + + struct blasfeo_dvec_args u_in; // input u of external fun; + u_in.x = memory->ux; + u_in.xi = 0; + + struct blasfeo_dvec_args fun_out; + fun_out.x = &memory->fun; + fun_out.xi = 0; + + struct blasfeo_dmat_args jac_out; + jac_out.A = &work->tmp_nv_nv; + jac_out.ai = 0; + jac_out.aj = 0; + + + ext_fun_type_in[0] = BLASFEO_DVEC_ARGS; + ext_fun_in[0] = &x_in; + ext_fun_type_in[1] = BLASFEO_DVEC_ARGS; + ext_fun_in[1] = &u_in; + + ext_fun_type_out[0] = BLASFEO_DVEC_ARGS; + ext_fun_out[0] = &fun_out; // fun: nx1 + ext_fun_type_out[1] = BLASFEO_DMAT_ARGS; + ext_fun_out[1] = &jac_out; // jac': (nu+nx) * nx1 + + // call external function + model->disc_dyn_fun_jac->evaluate(model->disc_dyn_fun_jac, ext_fun_type_in, ext_fun_in, ext_fun_type_out, ext_fun_out); + + // fun + blasfeo_daxpy(nx1, -1.0, memory->ux1, nu1, &memory->fun, 0, &memory->fun, 0); + + // adj TODO if not computed by the external function + if (opts->compute_adj) + { + blasfeo_dgemv_n(nu+nx, nx1, -1.0, &work->tmp_nv_nv, 0, 0, memory->pi, 0, 0.0, &memory->adj, 0, &memory->adj, 0); + blasfeo_dveccp(nx1, memory->pi, 0, &memory->adj, nu + nx); + } + + return; +} + + + int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_) { @@ -805,6 +875,7 @@ void ocp_nlp_dynamics_disc_config_initialize_default(void *config_) config->initialize = &ocp_nlp_dynamics_disc_initialize; config->update_qp_matrices = &ocp_nlp_dynamics_disc_update_qp_matrices; config->compute_fun = &ocp_nlp_dynamics_disc_compute_fun; + config->compute_fun_and_adjoint = &ocp_nlp_dynamics_disc_compute_fun_and_adjoint; config->precompute = &ocp_nlp_dynamics_disc_precompute; config->config_initialize_default = &ocp_nlp_dynamics_disc_config_initialize_default; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index fdb96417f9..e3aefca5b8 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -64,7 +64,7 @@ typedef struct int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) int qp_warm_start; // qp_warm_start in all but the first sqp iterations bool warm_start_first_qp; // to set qp_warm_start in first iteration - int rti_phase; // only phase 0 at the moment + int rti_phase; // only phase 0 at the moment int initialize_t_slacks; // 0-false or 1-true } 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 b2d3215b2e..ae694397ac 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -54,7 +54,9 @@ #include "acados/utils/print.h" #include "acados/utils/timing.h" #include "acados/utils/types.h" +// acados_c #include "acados_c/ocp_qp_interface.h" +#include "acados_c/ocp_nlp_interface.h" @@ -113,6 +115,10 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, opts->ext_qp_res = 0; opts->warm_start_first_qp = false; opts->rti_phase = 0; + opts->as_rti_level = STANDARD_RTI; + opts->as_rti_advancement_strategy = SIMULATE_ADVANCE; + opts->as_rti_iter = 0; + opts->rti_log_residuals = 0; return; } @@ -183,11 +189,34 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, else if (!strcmp(field, "rti_phase")) { int* rti_phase = (int *) value; - if (*rti_phase < 0 || *rti_phase > 2) { - printf("\nerror: ocp_nlp_sqp_opts_set: invalid value for rti_phase field."); - printf("possible values are: 0, 1, 2\n"); + if (*rti_phase < 0 || *rti_phase > 2) + { + printf("\nerror: ocp_nlp_sqp_opts_set: invalid value for rti_phase field.\n"); + printf("possible values are: 0, 1, 2, got %d.\n", *rti_phase); exit(1); - } else opts->rti_phase = *rti_phase; + } + opts->rti_phase = *rti_phase; + } + else if (!strcmp(field, "as_rti_iter")) + { + int* as_rti_iter = (int *) value; + opts->as_rti_iter = *as_rti_iter; + } + else if (!strcmp(field, "rti_log_residuals")) + { + int* rti_log_residuals = (int *) value; + opts->rti_log_residuals = *rti_log_residuals; + } + else if (!strcmp(field, "as_rti_level")) + { + int* as_rti_level = (int *) value; + if (*as_rti_level < LEVEL_A || *as_rti_level > STANDARD_RTI) + { + printf("\nerror: ocp_nlp_sqp_opts_set: invalid value for as_rti_level field.\n"); + printf("possible values are: 0, 1, 2, 3, 4, got %d.\n", *as_rti_level); + exit(1); + } + opts->as_rti_level = *as_rti_level; } else { @@ -232,10 +261,12 @@ acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, size += ocp_nlp_memory_calculate_size(config, dims, nlp_opts); // stat - int stat_m = 1+1; - int stat_n = 2; + int stat_m = 2 + opts->as_rti_iter; + int stat_n = 2; // qp_status, qp_iter + if (opts->rti_log_residuals) + stat_n += 4; // nlp_res if (opts->ext_qp_res) - stat_n += 4; + stat_n += 4; // qp_res size += stat_n*stat_m*sizeof(double); size += 8; // initial align @@ -269,13 +300,21 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, // stat mem->stat = (double *) c_ptr; - mem->stat_m = 1+1; - mem->stat_n = 2; + mem->stat_m = 2 + opts->as_rti_iter; + mem->stat_n = 2; // qp_status, qp_iter + if (opts->rti_log_residuals) + mem->stat_n += 4; // nlp_res if (opts->ext_qp_res) - mem->stat_n += 4; + mem->stat_n += 4; // qp_res c_ptr += mem->stat_m*mem->stat_n*sizeof(double); + for (int i=0; istat_m * mem->stat_n; i++) + { + mem->stat[i] = 0.0; + } + mem->status = ACADOS_READY; + mem->is_first_call = true; assert((char *) raw_memory+ocp_nlp_sqp_rti_memory_calculate_size( config, dims, opts) >= c_ptr); @@ -369,7 +408,7 @@ static void ocp_nlp_sqp_rti_cast_workspace( // utility functions -static void reset_sub_timers(ocp_nlp_sqp_rti_memory *mem) +static void reset_stats_and_sub_timers(ocp_nlp_sqp_rti_memory *mem) { mem->time_lin = 0.0; mem->time_reg = 0.0; @@ -377,6 +416,88 @@ static void reset_sub_timers(ocp_nlp_sqp_rti_memory *mem) mem->time_qp_solver_call = 0.0; mem->time_qp_xcond = 0.0; mem->time_glob = 0.0; + mem->sqp_iter = 0; +} + + + +static void rti_store_residuals_in_stats(ocp_nlp_sqp_rti_opts *opts, ocp_nlp_sqp_rti_memory *mem) +{ + ocp_nlp_memory *nlp_mem = mem->nlp_mem; + ocp_nlp_res *nlp_res = nlp_mem->nlp_res; + if (mem->sqp_iter < mem->stat_m) + { + int m_offset = 2 + 4 * 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 * mem->sqp_iter+0+m_offset] = nlp_res->inf_norm_res_stat; + mem->stat[mem->stat_n * mem->sqp_iter+1+m_offset] = nlp_res->inf_norm_res_eq; + mem->stat[mem->stat_n * mem->sqp_iter+2+m_offset] = nlp_res->inf_norm_res_ineq; + mem->stat[mem->stat_n * mem->sqp_iter+3+m_offset] = nlp_res->inf_norm_res_comp; + } + // printf("storting residuals in line %d\n", mem->sqp_iter); +} + + + + +static void prepare_full_residual_computation(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; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + // int *ni = dims->ni; + + for (int i=0; i < N; i++) + { + // dynamics: evaluate function and adjoint + config->dynamics[i]->compute_fun_and_adjoint(config->dynamics[i], dims->dynamics[i], in->dynamics[i], + opts->dynamics[i], mem->dynamics[i], work->dynamics[i]); + } + + for (int i=0; i <= N; i++) + { + // constraints: evaluate function and adjoint + config->constraints[i]->update_qp_matrices(config->constraints[i], dims->constraints[i], in->constraints[i], + opts->constraints[i], mem->constraints[i], work->constraints[i]); + struct blasfeo_dvec *ineq_adj = + config->constraints[i]->memory_get_adj_ptr(mem->constraints[i]); + blasfeo_dveccp(nv[i], ineq_adj, 0, mem->ineq_adj + i, 0); + } + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i=0; i <= N; i++) + { + // nlp mem: cost_grad + config->cost[i]->compute_gradient(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i], mem->cost[i], work->cost[i]); + struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); + blasfeo_dveccp(nv[i], cost_grad, 0, mem->cost_grad + i, 0); + + // nlp mem: dyn_adj + if (i < N) + { + struct blasfeo_dvec *dyn_adj + = config->dynamics[i]->memory_get_adj_ptr(mem->dynamics[i]); + blasfeo_dveccp(nu[i] + nx[i], dyn_adj, 0, mem->dyn_adj + i, 0); + } + else + { + blasfeo_dvecse(nu[N] + nx[N], 0.0, mem->dyn_adj + N, 0); + } + if (i > 0) + { + // TODO: this could be simplified by not copying pi in the dynamics module. + struct blasfeo_dvec *dyn_adj + = config->dynamics[i-1]->memory_get_adj_ptr(mem->dynamics[i-1]); + blasfeo_daxpy(nx[i], 1.0, dyn_adj, nu[i-1]+nx[i-1], mem->dyn_adj+i, nu[i], + mem->dyn_adj+i, nu[i]); + } + } } @@ -395,7 +516,7 @@ static void ocp_nlp_sqp_rti_preparation_step(ocp_nlp_config *config, ocp_nlp_dim ocp_nlp_workspace *nlp_work = work->nlp_work; - reset_sub_timers(mem); + reset_stats_and_sub_timers(mem); #if defined(ACADOS_WITH_OPENMP) // backup number of threads int num_threads_bkp = omp_get_num_threads(); @@ -455,6 +576,13 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * nlp_out, nlp_opts, nlp_mem, nlp_work); mem->time_lin += acados_toc(&timer1); + if (opts->rti_log_residuals) + { + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + mem->sqp_iter += 1; + // regularization acados_tic(&timer1); if (opts->rti_phase == FEEDBACK) @@ -525,14 +653,17 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * 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)); } + if (nlp_opts->print_level > 0) { + printf("\n------- qp_out --------\n"); + print_ocp_qp_out(nlp_mem->qp_out); + } // save statistics - mem->stat[mem->stat_n*1+0] = qp_status; - mem->stat[mem->stat_n*1+1] = qp_iter; + mem->stat[mem->stat_n * mem->sqp_iter+0] = qp_status; + mem->stat[mem->stat_n * mem->sqp_iter+1] = qp_iter; if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) { - // print_ocp_qp_in(mem->qp_in); #ifndef ACADOS_SILENT printf("\nSQP_RTI: QP solver returned error status %d QP iteration %d.\n", qp_status, qp_iter); @@ -554,9 +685,538 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * // update variables ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); - mem->status = ACADOS_SUCCESS; + if (opts->rti_log_residuals) + { + prepare_full_residual_computation(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + +} + + + +/*************************** + * AS-RTI functionality +****************************/ + +static void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to) +{ + // extract dims + int N = dims->N; + int *nv = dims->nv; + int *nx = dims->nx; + // int *nu = dims->nu; + int *ni = dims->ni; + int *nz = dims->nz; + for (int i = 0; i <= N; i++) + { + blasfeo_dveccp(nv[i], from->ux+i, 0, to->ux+i, 0); + blasfeo_dveccp(nz[i], from->z+i, 0, to->z+i, 0); + blasfeo_dveccp(2*ni[i], from->lam+i, 0, to->lam+i, 0); + blasfeo_dveccp(2*ni[i], from->t+i, 0, to->t+i, 0); + } + + for (int i = 0; i < N; i++) + blasfeo_dveccp(nx[i+1], from->pi+i, 0, to->pi+i, 0); + + return; +} + + +static void as_rti_sanity_checks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_sqp_rti_opts *opts) +{ + // sanity checks + if (dims->nx[0] != dims->nx[1]) + { + printf("dimensions nx[0] != nx[1], cannot perform AS-RTI!\n"); + exit(1); + } + if (opts->as_rti_level == LEVEL_C) + { + int ng_ineq, ng_qp; + // does not work for nonlinear constraints yet + for (int k = 0; k < dims->N; k++) + { + config->constraints[k]->dims_get(config->constraints[k], dims->constraints[k], "ng", &ng_ineq); + config->qp_solver->dims_get(config->qp_solver, dims->qp_solver, k, "ng", &ng_qp); + if (ng_ineq != ng_qp) + { + printf("AS-RTI with LEVEL_C not implemented for nonlinear inequality constraints."); + printf("Got ng_ineq = %d != %d = ng_qp at stage %d \n\n", ng_ineq, ng_qp, k); + exit(1); + } + } + } + if (opts->as_rti_level == LEVEL_B) + { + for (int k = 0; k <= dims->N; k++) + { + if (dims->ns[k] > 0) + { + printf("\n\nAS-RTI with LEVEL_B not implemented for soft constraints yet.\n"); + printf("Got ns[%d] = %d. Exiting\n", k, dims->ns[k]); + exit(1); + } + } + } + if (opts->as_rti_iter < 1) + { + printf("\n\nAS-RTI: as_rti_iter < 1: no advanced step iteration will be performed!\n\n"); + } +} + +static void as_rti_advance_problem(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, ocp_nlp_sqp_rti_opts *opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work) +{ + ocp_nlp_opts *nlp_opts = opts->nlp_opts; + // setup advanced problem + if (opts->as_rti_advancement_strategy == SHIFT_ADVANCE) + { + // tmp_nxu_double = x at stage 1 + blasfeo_unpack_dvec(dims->nx[1], nlp_out->ux+1, dims->nu[1], nlp_work->tmp_nxu_double, 1); + } + else if (opts->as_rti_advancement_strategy == SIMULATE_ADVANCE) + { + // dyn_fun = phi(x_0, u_0) - x_1 + config->dynamics[0]->compute_fun(config->dynamics[0], dims->dynamics[0], nlp_in->dynamics[0], + nlp_opts->dynamics[0], nlp_mem->dynamics[0], nlp_work->dynamics[0]); + struct blasfeo_dvec *dyn_fun = config->dynamics[0]->memory_get_fun_ptr(nlp_mem->dynamics[0]); + // dyn_fun += x_1 + blasfeo_daxpy(dims->nx[0], +1.0, nlp_out->ux+1, dims->nu[1], dyn_fun, 0, dyn_fun, 0); + // tmp_nxu_double = phi(x0, u0) + blasfeo_unpack_dvec(dims->nx[1], dyn_fun, 0, nlp_work->tmp_nxu_double, 1); + } + if (opts->as_rti_advancement_strategy != NO_ADVANCE) + { + ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", nlp_work->tmp_nxu_double); + ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", nlp_work->tmp_nxu_double); + } + // printf("advanced x value\n"); + // blasfeo_print_exp_tran_dvec(dims->nx[1], nlp_out->ux+1, dims->nu[1]); +} + + + +static void level_c_prepare_residual_computation(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; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + // int *ni = dims->ni; + + + // evaluate constraint adjoint + for (int i=0; i <= N; i++) + { + // constraints: evaluate function and adjoint + config->constraints[i]->update_qp_matrices(config->constraints[i], dims->constraints[i], in->constraints[i], + opts->constraints[i], mem->constraints[i], work->constraints[i]); + struct blasfeo_dvec *ineq_adj = + config->constraints[i]->memory_get_adj_ptr(mem->constraints[i]); + blasfeo_dveccp(nv[i], ineq_adj, 0, mem->ineq_adj + i, 0); + } + +#if defined(ACADOS_WITH_OPENMP) + #pragma omp parallel for +#endif + for (int i=0; i <= N; i++) + { + + // nlp mem: cost_grad + config->cost[i]->compute_gradient(config->cost[i], dims->cost[i], in->cost[i], opts->cost[i], mem->cost[i], work->cost[i]); + struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); + blasfeo_dveccp(nv[i], cost_grad, 0, mem->cost_grad + i, 0); + + // nlp mem: dyn_adj + if (i < N) + { + struct blasfeo_dvec *dyn_adj + = config->dynamics[i]->memory_get_adj_ptr(mem->dynamics[i]); + blasfeo_dveccp(nu[i] + nx[i], dyn_adj, 0, mem->dyn_adj + i, 0); + } + else + { + blasfeo_dvecse(nu[N] + nx[N], 0.0, mem->dyn_adj + N, 0); + } + if (i > 0) + { + // TODO: this could be simplified by not copying pi in the dynamics module. + struct blasfeo_dvec *dyn_adj + = config->dynamics[i-1]->memory_get_adj_ptr(mem->dynamics[i-1]); + blasfeo_daxpy(nx[i], 1.0, dyn_adj, nu[i-1]+nx[i-1], mem->dyn_adj+i, nu[i], + mem->dyn_adj+i, nu[i]); + } + } +} + + + +static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, + ocp_nlp_out *nlp_out, ocp_nlp_sqp_rti_opts *opts, ocp_nlp_sqp_rti_memory *mem, ocp_nlp_sqp_rti_workspace *work) +{ + acados_timer timer1; + ocp_nlp_memory *nlp_mem = mem->nlp_mem; + ocp_nlp_opts *nlp_opts = opts->nlp_opts; + ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; + + ocp_nlp_workspace *nlp_work = work->nlp_work; + ocp_nlp_out *tmp_nlp_out = nlp_work->tmp_nlp_out; + + reset_stats_and_sub_timers(mem); + +#if defined(ACADOS_WITH_OPENMP) + // backup number of threads + int num_threads_bkp = omp_get_num_threads(); + // set number of threads + omp_set_num_threads(opts->nlp_opts->num_threads); +#endif + + // printf("AS_RTI preparation\n"); + qp_info *qp_info_; + int qp_iter, qp_status; + double alpha, tmp_time; + + // prepare submodules + ocp_nlp_initialize_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + + + if (!mem->is_first_call) + { + as_rti_advance_problem(config, dims, nlp_in, nlp_out, opts, nlp_mem, nlp_work); + } + else + { + as_rti_sanity_checks(config, dims, opts); + } + + // if AS_RTI-A and not first call! + if (opts->as_rti_level == LEVEL_A && !mem->is_first_call) + { + // load iterate from tmp + copy_ocp_nlp_out(dims, tmp_nlp_out, nlp_out); + // perform QP solve (implemented as feedback) + // similar to ocp_nlp_sqp_rti_feedback_step + + // update QP rhs for SQP (step prim var, abs dual var) + acados_tic(&timer1); + ocp_nlp_approximate_qp_vectors_sqp(config, dims, nlp_in, + nlp_out, nlp_opts, nlp_mem, nlp_work); + mem->time_lin += acados_toc(&timer1); + + if (opts->rti_log_residuals) + { + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + mem->sqp_iter += 1; + + // regularization rhs + acados_tic(&timer1); + config->regularize->regularize_rhs(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + + // solve QP + acados_tic(&timer1); + qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + mem->time_qp_sol += acados_toc(&timer1); + // NOTE: timings within qp solver are added internally (lhs+rhs) + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); + mem->time_qp_solver_call += tmp_time; + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time); + mem->time_qp_xcond += tmp_time; + + // save statistics + ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); + qp_iter = qp_info_->num_iter; + mem->stat[mem->stat_n * mem->sqp_iter+0] = qp_status; + mem->stat[mem->stat_n * mem->sqp_iter+1] = qp_iter; + + // compute correct dual solution in case of Hessian regularization + acados_tic(&timer1); + config->regularize->correct_dual_sol(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + + // globalization + acados_tic(&timer1); + // TODO: not clear if line search should be called with sqp_iter==0 in RTI; + alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1); + mem->time_glob += acados_toc(&timer1); + + // update variables + ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); + } + else if (opts->as_rti_level == LEVEL_B && !mem->is_first_call) + { + // perform zero-order iterations + for (; mem->sqp_iter < opts->as_rti_iter; mem->sqp_iter++) + { + acados_tic(&timer1); + // zero order QP update + ocp_nlp_zero_order_qp_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + mem->time_lin += acados_toc(&timer1); + + if (opts->rti_log_residuals) + { + // evaluate additional functions and compute residuals + prepare_full_residual_computation(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + + // rhs regularization + acados_tic(&timer1); + config->regularize->regularize_rhs(config->regularize, + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + // QP solve + acados_tic(&timer1); + qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + + // add qp timings + mem->time_qp_sol += acados_toc(&timer1); + // NOTE: timings within qp solver are added internally (lhs+rhs) + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); + mem->time_qp_solver_call += tmp_time; + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time); + mem->time_qp_xcond += tmp_time; + + ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); + qp_iter = qp_info_->num_iter; + + // save statistics + mem->stat[mem->stat_n * mem->sqp_iter+0] = qp_status; + mem->stat[mem->stat_n * mem->sqp_iter+1] = qp_iter; + + // 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); + mem->time_reg += acados_toc(&timer1); + if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) + { +#ifndef ACADOS_SILENT + printf("\nSQP_RTI: QP solver returned error status %d QP iteration %d.\n", + qp_status, qp_iter); +#endif + mem->status = ACADOS_QP_FAILURE; + return; + } + + if (nlp_opts->print_level > 0) { + printf("\n------- qp_in B-iter %d --------\n", mem->sqp_iter); + print_ocp_qp_in(nlp_mem->qp_in); + printf("\n------- qp_out B-iter %d --------\n", mem->sqp_iter); + print_ocp_qp_out(nlp_mem->qp_out); + } + + // globalization + acados_tic(&timer1); + // TODO: not clear if line search should be called with sqp_iter==0 in RTI; + alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1); + mem->time_glob += acados_toc(&timer1); + + // update variables + ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); + } + } + else if (opts->as_rti_level == LEVEL_C && !mem->is_first_call) + { + // perform iterations + for (; mem->sqp_iter < opts->as_rti_iter; mem->sqp_iter++) + { + // double norm, tmp_norm = 0.0; + acados_tic(&timer1); + // QP update + ocp_nlp_level_c_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + mem->time_lin += acados_toc(&timer1); + + if (opts->rti_log_residuals) + { + // evaluate additional functions and compute residuals + level_c_prepare_residual_computation(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + + + // rhs regularization + acados_tic(&timer1); + config->regularize->regularize_rhs(config->regularize, + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + // QP solve + acados_tic(&timer1); + qp_status = qp_solver->condense_rhs_and_solve(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + + // add qp timings + mem->time_qp_sol += acados_toc(&timer1); + // NOTE: timings within qp solver are added internally (lhs+rhs) + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); + mem->time_qp_solver_call += tmp_time; + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time); + mem->time_qp_xcond += tmp_time; + + ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); + qp_iter = qp_info_->num_iter; + + // save statistics + mem->stat[mem->stat_n * mem->sqp_iter+0] = qp_status; + mem->stat[mem->stat_n * mem->sqp_iter+1] = qp_iter; + + // 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); + mem->time_reg += acados_toc(&timer1); + if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) + { +#ifndef ACADOS_SILENT + printf("\nSQP_RTI: QP solver returned error status %d QP iteration %d.\n", + qp_status, qp_iter); +#endif + mem->status = ACADOS_QP_FAILURE; + return; + } + + if (nlp_opts->print_level > 0) { + printf("\n------- qp_in B-iter %d --------\n", mem->sqp_iter); + print_ocp_qp_in(nlp_mem->qp_in); + printf("\n------- qp_out B-iter %d --------\n", mem->sqp_iter); + print_ocp_qp_out(nlp_mem->qp_out); + } + + // globalization + acados_tic(&timer1); + // TODO: not clear if line search should be called with sqp_iter==0 in RTI; + alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1); + mem->time_glob += acados_toc(&timer1); + + // update variables + ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); + + // norm = 0.0; + // for (int kk = 0; kk < dims->N; kk++) + // { + // blasfeo_dvecnrm_inf(dims->nx[kk] + dims->nu[kk], nlp_mem->qp_out->ux+kk, 0, &tmp_norm); + // norm = (tmp_norm > norm) ? tmp_norm : norm; + // } + // printf("step norm primal as_rti iter %d %e\n", i, norm); + } + } + else if (opts->as_rti_level == LEVEL_D) + { + // perform k full SQP iterations + for (; mem->sqp_iter < opts->as_rti_iter; mem->sqp_iter++) + { + if (opts->rti_log_residuals) + { + // TODO: evaluate what is needed!! + ocp_nlp_res_compute(dims, nlp_in, nlp_out, nlp_mem->nlp_res, nlp_mem); + rti_store_residuals_in_stats(opts, mem); + } + acados_tic(&timer1); + // linearize NLP + 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); + mem->time_lin += acados_toc(&timer1); + // full regularization + acados_tic(&timer1); + config->regularize->regularize(config->regularize, + dims->regularize, nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + // QP solve + acados_tic(&timer1); + qp_status = qp_solver->evaluate(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); + + // add qp timings + mem->time_qp_sol += acados_toc(&timer1); + // NOTE: timings within qp solver are added internally (lhs+rhs) + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_solver_call", &tmp_time); + mem->time_qp_solver_call += tmp_time; + qp_solver->memory_get(qp_solver, nlp_mem->qp_solver_mem, "time_qp_xcond", &tmp_time); + mem->time_qp_xcond += tmp_time; + + ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); + qp_iter = qp_info_->num_iter; + + // save statistics + mem->stat[mem->stat_n * mem->sqp_iter+0] = qp_status; + mem->stat[mem->stat_n * mem->sqp_iter+1] = qp_iter; + + // 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); + mem->time_reg += acados_toc(&timer1); + if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) + { +#ifndef ACADOS_SILENT + printf("\nSQP_RTI: QP solver returned error status %d QP iteration %d.\n", + qp_status, qp_iter); +#endif + mem->status = ACADOS_QP_FAILURE; + return; + } + + // globalization + acados_tic(&timer1); + // TODO: not clear if line search should be called with sqp_iter==0 in RTI; + alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1); + mem->time_glob += acados_toc(&timer1); + + // update variables + ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, alpha); + } + } + + /* NORMAL RTI PREPARATION */ + // linearize NLP and update QP matrices + acados_tic(&timer1); + ocp_nlp_approximate_qp_matrices(config, dims, nlp_in, + nlp_out, nlp_opts, nlp_mem, nlp_work); + mem->time_lin += acados_toc(&timer1); + + // regularize Hessian + acados_tic(&timer1); + config->regularize->regularize_lhs(config->regularize, + dims->regularize, opts->nlp_opts->regularize, nlp_mem->regularize_mem); + mem->time_reg += acados_toc(&timer1); + // condense lhs + qp_solver->condense_lhs(qp_solver, dims->qp_solver, + nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, + nlp_mem->qp_solver_mem, nlp_work->qp_work); +#if defined(ACADOS_WITH_OPENMP) + // restore number of threads + omp_set_num_threads(num_threads_bkp); +#endif + + /* AS-RTI */ + if (opts->as_rti_level == LEVEL_A) + { + // backup iterate: + // tmp_nlp_out <- nlp_out + copy_ocp_nlp_out(dims, nlp_out, tmp_nlp_out); + } + + mem->is_first_call = false; + + return; } @@ -577,20 +1237,28 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int rti_phase = opts->rti_phase; - switch(rti_phase) + if (rti_phase == FEEDBACK) { - case PREPARATION_AND_FEEDBACK: - ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); - ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); - break; - - case PREPARATION: - ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); - break; - - case FEEDBACK: - ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); - break; + ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); + } + else if (rti_phase == PREPARATION && opts->as_rti_level == STANDARD_RTI) + { + ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); + } + else if (rti_phase == PREPARATION) + { + ocp_nlp_sqp_rti_preparation_advanced_step(config, dims, nlp_in, nlp_out, opts, mem, work); + } + else if (rti_phase == PREPARATION_AND_FEEDBACK && opts->as_rti_level != STANDARD_RTI) + { + printf("ocp_nlp_sqp_rti: rti_phase == PREPARATION_AND_FEEDBACK not supported with AS-RTI (opts->as_rti_level != STANDARD_RTI).\n\n"); + exit(1); + } + else if (rti_phase == PREPARATION_AND_FEEDBACK) + { + // rti_phase == PREPARATION_AND_FEEDBACK + ocp_nlp_sqp_rti_preparation_step(config, dims, nlp_in, nlp_out, opts, mem, work); + ocp_nlp_sqp_rti_feedback_step(config, dims, nlp_in, nlp_out, opts, mem, work); } mem->time_tot = acados_toc(&timer); @@ -611,6 +1279,8 @@ void ocp_nlp_sqp_rti_memory_reset_qp_solver(void *config_, void *dims_, void *nl ocp_nlp_sqp_rti_workspace *work = work_; ocp_nlp_workspace *nlp_work = work->nlp_work; + mem->is_first_call = true; + config->qp_solver->memory_reset(qp_solver, dims->qp_solver, nlp_mem->qp_in, nlp_mem->qp_out, opts->nlp_opts->qp_solver_opts, nlp_mem->qp_solver_mem, nlp_work->qp_work); @@ -727,7 +1397,7 @@ void ocp_nlp_sqp_rti_get(void *config_, void *dims_, void *mem_, if (!strcmp("sqp_iter", field)) { int *value = return_value_; - *value = 1; + *value = mem->sqp_iter; } else if (!strcmp("status", field)) { @@ -794,7 +1464,7 @@ void ocp_nlp_sqp_rti_get(void *config_, void *dims_, void *mem_, } else if (!strcmp("statistics", field)) { - int n_row = 2; + int n_row = mem->stat_msqp_iter+1 ? mem->stat_m : mem->sqp_iter+1; double *value = return_value_; for (int ii=0; iins; int nx = dims->nx; 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 new file mode 100644 index 0000000000..e3de99637c --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/as_rti/as_rti_closed_loop_example.py @@ -0,0 +1,202 @@ +# -*- 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, AcadosSimSolver +from pendulum_model import export_pendulum_ode_model +from utils import plot_pendulum +import numpy as np +import scipy.linalg +from casadi import vertcat + + +REAL_TIME_ALGORITHMS = ["RTI", "AS-RTI-A", "AS-RTI-B", "AS-RTI-C", "AS-RTI-D"] +ALGORITHMS = ["SQP"] + REAL_TIME_ALGORITHMS + +def setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter=1): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.size()[0] + nu = model.u.size()[0] + ny = nx + nu + ny_e = nx + + ocp.dims.N = N_horizon + + # set cost module + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + 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([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + + ocp.constraints.x0 = x0 + ocp.constraints.idxbu = np.array([0]) + + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'IRK' + ocp.solver_options.sim_method_newton_iter = 10 + + ocp.translate_nls_cost_to_conl() + + if algorithm in REAL_TIME_ALGORITHMS: + ocp.solver_options.nlp_solver_type = 'SQP_RTI' + else: + ocp.solver_options.nlp_solver_type = 'SQP' + + if algorithm == "AS-RTI-A": + ocp.solver_options.as_rti_iter = as_rti_iter + ocp.solver_options.as_rti_level = 0 + elif algorithm == "AS-RTI-B": + ocp.solver_options.as_rti_iter = as_rti_iter + ocp.solver_options.as_rti_level = 1 + elif algorithm == "AS-RTI-C": + ocp.solver_options.as_rti_iter = as_rti_iter + ocp.solver_options.as_rti_level = 2 + elif algorithm == "AS-RTI-D": + ocp.solver_options.as_rti_iter = as_rti_iter + ocp.solver_options.as_rti_level = 3 + + ocp.solver_options.qp_solver_cond_N = N_horizon + + # set prediction horizon + ocp.solver_options.tf = Tf + + 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) + + return acados_ocp_solver, acados_integrator + + +def main(algorithm='RTI', as_rti_iter=1): + + x0 = np.array([0.0, np.pi, 0.0, 0.0]) + Fmax = 80 + + Tf = .8 + N_horizon = 40 + + ocp_solver, integrator = setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter) + + nx = ocp_solver.acados_ocp.dims.nx + nu = ocp_solver.acados_ocp.dims.nu + + Nsim = 100 + simX = np.ndarray((Nsim+1, nx)) + simU = np.ndarray((Nsim, nu)) + + simX[0,:] = x0 + + if algorithm != "SQP": + t_preparation = np.zeros((Nsim)) + t_feedback = np.zeros((Nsim)) + + else: + t = np.zeros((Nsim)) + + # closed loop + for i in range(Nsim): + + if algorithm != "SQP": + # 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, :]) + + t[i] = ocp_solver.get_stats('time_tot') + + # simulate system + simX[i+1, :] = integrator.simulate(x=simX[i, :], u=simU[i,:]) + + # evaluate timings + if algorithm != "SQP": + # 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}') + + # plot results + plot_pendulum(np.linspace(0, (Tf/N_horizon)*Nsim, Nsim+1), Fmax, simU, simX, title=algorithm) + + ocp_solver = None + + +if __name__ == '__main__': + 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) diff --git a/examples/acados_python/pendulum_on_cart/common/utils.py b/examples/acados_python/pendulum_on_cart/common/utils.py index a529afd415..b372d9a11b 100644 --- a/examples/acados_python/pendulum_on_cart/common/utils.py +++ b/examples/acados_python/pendulum_on_cart/common/utils.py @@ -34,7 +34,8 @@ from acados_template import latexify_plot def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, latexify=True, plt_show=True, X_true_label=None, - states_lables = ['$x$', r'$\theta$', '$v$', r'$\dot{\theta}$'] + states_lables = ['$x$', r'$\theta$', '$v$', r'$\dot{\theta}$'], + title = None ): """ Params: @@ -69,7 +70,8 @@ def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, line.set_label(X_true_label) else: line.set_color('r') - plt.title('closed-loop simulation') + if title is not None: + plt.title(title) plt.ylabel('$u$') plt.xlabel('$t$') plt.hlines(u_max, t[0], t[-1], linestyles='dashed', alpha=0.7) diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index fcc5d28a53..ca9acf633f 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -306,6 +306,10 @@ int main() { case INVALID_QP_SOLVER: printf("\nInvalid QP solver\n\n"); + default: + printf("\nQP solver not implemented in example\n\n"); + + } // ocp_qp_full_condensing_dims *xcond_dims = qp_dims->xcond_dims; diff --git a/examples/c/sim_gnsf_crane.c b/examples/c/sim_gnsf_crane.c index 7b31a5e1ce..ac8e131e65 100644 --- a/examples/c/sim_gnsf_crane.c +++ b/examples/c/sim_gnsf_crane.c @@ -45,6 +45,8 @@ #include "acados_c/external_function_interface.h" #include "acados_c/sim_interface.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" // for printing + // model #include "examples/c/crane_nx9_model/crane_nx9_model.h" diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 3388d98749..56ce915f20 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -234,6 +234,10 @@ 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) diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/ocp_nlp_solver_options_json.m b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/ocp_nlp_solver_options_json.m index f1c072ee67..3dd44f263f 100644 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/ocp_nlp_solver_options_json.m +++ b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/ocp_nlp_solver_options_json.m @@ -70,6 +70,9 @@ ext_cost_num_hess alpha_min alpha_reduction + as_rti_iter + as_rti_level + rti_log_residuals globalization collocation_type line_search_use_sufficient_descent @@ -127,6 +130,9 @@ obj.nlp_solver_ext_qp_res = 0; obj.ext_fun_compile_flags = '-O2'; obj.cost_discretization = 'EULER'; + obj.as_rti_iter = 1; + obj.as_rti_level = 4; + obj.rti_log_residuals = 0; end function s = struct(self) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 2820f164ea..8fa4a5de90 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -71,6 +71,7 @@ def __init__(self): self.__nlp_solver_tol_comp = 1e-6 self.__nlp_solver_max_iter = 100 self.__nlp_solver_ext_qp_res = 0 + self.__rti_log_residuals = 0 self.__Tsim = None self.__print_level = 0 self.__initialize_t_slacks = 0 @@ -99,6 +100,8 @@ def __init__(self): self.__custom_update_header_filename = '' self.__custom_templates = [] self.__custom_update_copy = True + self.__as_rti_iter = 1 + self.__as_rti_level = 4 @property def qp_solver(self): @@ -405,6 +408,31 @@ def qp_solver_iter_max(self): """ return self.__qp_solver_iter_max + + @property + def as_rti_iter(self): + """ + Maximum number of iterations in the advanced-step real-time iteration. + Default: 1 + """ + return self.__as_rti_iter + + + @property + def as_rti_level(self): + """ + Level of the advanced-step real-time iteration. + + LEVEL-A: 0 + LEVEL-B: 1 + LEVEL-C: 2 + LEVEL-D: 3 + STANDARD_RTI: 4 + + Default: 4 + """ + return self.__as_rti_level + @property def tol(self): """ @@ -507,6 +535,17 @@ def nlp_solver_ext_qp_res(self): """ return self.__nlp_solver_ext_qp_res + + @property + def rti_log_residuals(self): + """Determines if residuals are computed and logged within RTI / AS-RTI iterations (for debugging). + + Type: int; 0 or 1; + Default: 0. + """ + return self.__rti_log_residuals + + @property def nlp_solver_tol_comp(self): """NLP solver complementarity tolerance""" @@ -877,6 +916,21 @@ def qp_solver_iter_max(self, qp_solver_iter_max): else: raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.') + @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.') + + @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].') + + @qp_solver_ric_alg.setter def qp_solver_ric_alg(self, qp_solver_ric_alg): if qp_solver_ric_alg in [0, 1]: @@ -982,6 +1036,13 @@ def nlp_solver_ext_qp_res(self, 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].') + @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].') + @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: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 26c2f80688..ae46c1892a 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -837,14 +837,23 @@ def print_statistics(self): stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj])) print('\n') elif self.solver_options['nlp_solver_type'] == 'SQP_RTI': - print('\niter\tqp_stat\tqp_iter') - if stat.shape[0]>3: - print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp') + header = '\niter\tqp_stat\tqp_iter' + if self.solver_options['nlp_solver_ext_qp_res'] == 1: + header += '\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp' + if self.solver_options['rti_log_residuals'] == 1: + header += '\tres_stat\tres_eq\tres_ineq\tres_comp' + print(header) for jj in range(stat.shape[1]): - print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj]))) - if stat.shape[0]>3: - print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ - stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj])) + line = '{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj])) + offset = 2 + if self.solver_options['nlp_solver_ext_qp_res'] == 1: + line += '\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[offset+1][jj], stat[offset+2][jj], stat[offset+3][jj], stat[offset+4][jj]) + offset += 4 + if self.solver_options['rti_log_residuals'] == 1: + line += '\t{:e}\t{:e}\t{:e}\t{:e}'.format( \ + stat[offset+1][jj], stat[offset+2][jj], stat[offset+3][jj], stat[offset+4][jj]) + print(line) print('\n') return @@ -1002,6 +1011,8 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: 'stat_n', 'residuals', 'alpha', + 'res_eq_all', + 'res_stat_all', ] field = field_.encode('utf-8') @@ -1054,16 +1065,25 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: elif field_ == 'residuals': return self.get_residuals() - elif field_ == 'residuals': - if self.acados_ocp.solver_options.nlp_solver_type == 'SQP': - full_stats = self.get_stats('statistics') - if self.status != 2: - out = (full_stats.T)[-1][1:5] - else: # when exiting with max_iter, residuals are computed for second last iterate only - out = (full_stats.T)[-2][1:5] - else: - Exception("residuals are not computed for SQP_RTI") + elif field_ == 'res_eq_all': + full_stats = self.get_stats('statistics') + if self.solver_options['nlp_solver_type'] == 'SQP': + return full_stats[2, :] + elif self.solver_options['nlp_solver_type'] == 'SQP_RTI': + 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.") + elif field_ == 'res_stat_all': + full_stats = self.get_stats('statistics') + if self.solver_options['nlp_solver_type'] == 'SQP': + return full_stats[1, :] + elif self.solver_options['nlp_solver_type'] == 'SQP_RTI': + 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.") else: raise Exception(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' @@ -1479,7 +1499,7 @@ def options_set(self, field_, value_): - warm_start_first_qp: indicates if first QP in SQP is warm_started """ int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', - 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp'] + 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp', "as_rti_level"] double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'] string_fields = ['globalization'] diff --git a/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 2ee4b77e0f..c442beaf6a 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 @@ -2156,6 +2156,15 @@ void {{ name }}_acados_create_6_set_opts({{ name }}_solver_capsule* capsule) int initialize_t_slacks = {{ solver_options.initialize_t_slacks }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks); +{%- elif solver_options.nlp_solver_type == "SQP_RTI" %} + int as_rti_iter = {{ solver_options.as_rti_iter }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_iter", &as_rti_iter); + + int as_rti_level = {{ solver_options.as_rti_level }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_level", &as_rti_level); + + int rti_log_residuals = {{ solver_options.rti_log_residuals }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_residuals", &rti_log_residuals); {%- endif %} int qp_solver_iter_max = {{ solver_options.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 8960991127..d8e7ea6edb 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 @@ -2126,6 +2126,16 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* int initialize_t_slacks = {{ solver_options.initialize_t_slacks }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks); + +{%- elif solver_options.nlp_solver_type == "SQP_RTI" %} + int as_rti_iter = {{ solver_options.as_rti_iter }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_iter", &as_rti_iter); + + int as_rti_level = {{ solver_options.as_rti_level }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "as_rti_level", &as_rti_level); + + int rti_log_residuals = {{ solver_options.rti_log_residuals }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_log_residuals", &rti_log_residuals); {%- endif %} int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; From 9579f1fab2c3655d0caeef063bd8edc0bd44559d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 14 Mar 2024 13:47:32 +0100 Subject: [PATCH 11/16] Fix DAQP for Matlab on Windows (#1043) --- .../acados_template/c_templates_tera/CMakeLists.in.txt | 2 +- .../acados_template/c_templates_tera/Makefile.in | 2 +- .../acados_template/c_templates_tera/multi_Makefile.in | 2 +- 3 files changed, 3 insertions(+), 3 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 03f82723bb..8a81010f57 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 @@ -130,7 +130,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 -%} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp -%} {%- set openmp_flag = acados_link_libs.openmp %} {%- else %} {%- set openmp_flag = " " %} 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 70d651c38b..6db06c5d5f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -130,7 +130,7 @@ {# 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 -%} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp -%} {%- set openmp_flag = acados_link_libs.openmp %} {%- else %} {%- set openmp_flag = " " %} 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 5a55acaf81..17ddd95f2e 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 @@ -72,7 +72,7 @@ {# 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 -%} + {%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp ~ " " ~ acados_link_libs.daqp -%} {%- set openmp_flag = acados_link_libs.openmp %} {%- else %} {%- set openmp_flag = " " %} From c52c283a6cbf9e517e29905ce84dc9559dbfb8c5 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Thu, 14 Mar 2024 15:57:25 +0100 Subject: [PATCH 12/16] Efficient factorization of diagonal outer Hessian in convex-over-nonlinear and nonlinear least-squares costs (#1024) * add flags indicating whether outer hessian is diagonal * if outer hessian is diagonal and `ny > 4`, its diagonal Cholesky factorization `W_chol` is computed manually and stored as `blasfeo_dvec` * adjust matrix-matrix/matrix-vector multiplications involving `W_chol` accordingly --------- Co-authored-by: Jonathan Frey --- acados/ocp_nlp/ocp_nlp_common.c | 4 + acados/ocp_nlp/ocp_nlp_cost_common.h | 2 + acados/ocp_nlp/ocp_nlp_cost_conl.c | 83 +++++++++++--- acados/ocp_nlp/ocp_nlp_cost_conl.h | 2 + acados/ocp_nlp/ocp_nlp_cost_ls.c | 84 ++++++++++++-- acados/ocp_nlp/ocp_nlp_cost_ls.h | 4 + acados/ocp_nlp/ocp_nlp_cost_nls.c | 106 ++++++++++++++++-- acados/ocp_nlp/ocp_nlp_cost_nls.h | 4 + acados/ocp_nlp/ocp_nlp_dynamics_cont.c | 2 +- acados/sim/sim_irk_integrator.c | 82 +++++++++++--- acados/sim/sim_irk_integrator.h | 3 +- .../acados_matlab_octave/acados_ocp_model.m | 7 +- .../acados_ocp_layout.json | 14 +-- .../acados_matlab_octave/detect_cost_type.m | 2 +- .../ocp_generate_c_code.m | 2 + .../set_up_acados_ocp_nlp_json.m | 2 + .../acados_template/acados_ocp_solver.py | 1 + .../c_templates_tera/acados_multi_solver.in.c | 5 +- .../c_templates_tera/acados_solver.in.c | 3 +- .../casadi_function_generation.py | 9 +- 20 files changed, 358 insertions(+), 63 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 5294e48675..514c770cf3 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1989,11 +1989,15 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di struct blasfeo_dvec *cost_grad = config->cost[i]->memory_get_grad_ptr(nlp_mem->cost[i]); struct blasfeo_dvec *y_ref = config->cost[i]->model_get_y_ref_ptr(nlp_in->cost[i]); 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]); 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); config->dynamics[i]->memory_set(config->dynamics[i], dims->dynamics[i], nlp_mem->dynamics[i], "y_ref", y_ref); 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); } } diff --git a/acados/ocp_nlp/ocp_nlp_cost_common.h b/acados/ocp_nlp/ocp_nlp_cost_common.h index 9c9a930a44..8266ce9bd2 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -74,6 +74,8 @@ typedef struct struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory); struct blasfeo_dvec *(*model_get_y_ref_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_); void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.c b/acados/ocp_nlp/ocp_nlp_cost_conl.c index 9c21a49776..e1348a28ec 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.c +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.c @@ -35,10 +35,12 @@ #include #include #include +#include // blasfeo #include "blasfeo/include/blasfeo_d_aux.h" #include "blasfeo/include/blasfeo_d_blas.h" +#include "blasfeo/include/blasfeo_common.h" // acados #include "acados/utils/mem.h" @@ -293,6 +295,8 @@ void *ocp_nlp_cost_conl_opts_assign(void *config_, void *dims_, void *raw_memory assert((char *) raw_memory + ocp_nlp_cost_conl_opts_calculate_size(config_, dims_) >= c_ptr); + + return opts; } @@ -363,6 +367,7 @@ acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config_, void *dims_ size += 64; // blasfeo_mem align size += 1 * blasfeo_memsize_dmat(ny, ny); // W_chol + size += 1 * blasfeo_memsize_dvec(ny); // W_chol_diag return size; } @@ -391,6 +396,9 @@ void *ocp_nlp_cost_conl_memory_assign(void *config_, void *dims_, void *opts_, v // W_chol assign_and_advance_blasfeo_dmat_mem(ny, ny, &memory->W_chol, &c_ptr); + // W_chol_diag + assign_and_advance_blasfeo_dvec_mem(ny, &memory->W_chol_diag, &c_ptr); + // grad assign_and_advance_blasfeo_dvec_mem(nu + nx + 2 * ns, &memory->grad, &c_ptr); @@ -414,6 +422,20 @@ struct blasfeo_dmat *ocp_nlp_cost_conl_memory_get_W_chol_ptr(void *memory_) return &memory->W_chol; } +struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_W_chol_diag_ptr(void *memory_) +{ + ocp_nlp_cost_conl_memory *memory = memory_; + return &memory->W_chol_diag; +} + + +double *ocp_nlp_cost_conl_get_outer_hess_is_diag_ptr(void *memory_, void *model_) +{ + ocp_nlp_cost_conl_memory *memory = memory_; + // ocp_nlp_cost_conl_model *model = model_; + + return &memory->outer_hess_is_diag; +} struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_) @@ -610,13 +632,12 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode int nu = dims->nu; int ny = dims->ny; int ns = dims->ns; - if (opts->integrator_cost == 0) { ext_fun_arg_t conl_fun_jac_hess_type_in[5]; void *conl_fun_jac_hess_in[5]; - ext_fun_arg_t conl_fun_jac_hess_type_out[5]; - void *conl_fun_jac_hess_out[5]; + ext_fun_arg_t conl_fun_jac_hess_type_out[6]; + void *conl_fun_jac_hess_out[6]; // INPUT struct blasfeo_dvec_args x_in; // input x @@ -651,13 +672,27 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode conl_fun_jac_hess_out[3] = &work->Jt_z; // inner Jacobian wrt z, transposed, nz x ny conl_fun_jac_hess_type_out[4] = BLASFEO_DMAT; conl_fun_jac_hess_out[4] = &work->W; // outer hessian: ny x ny + conl_fun_jac_hess_type_out[5] = COLMAJ; + conl_fun_jac_hess_out[5] = &memory->outer_hess_is_diag; // flag indicates if outer hess is diag // evaluate external function model->conl_cost_fun_jac_hess->evaluate(model->conl_cost_fun_jac_hess, conl_fun_jac_hess_type_in, conl_fun_jac_hess_in, conl_fun_jac_hess_type_out, conl_fun_jac_hess_out); - // hessian of outer loss function - blasfeo_dpotrf_l(ny, &work->W, 0, 0, &memory->W_chol, 0, 0); + // factorize hessian of outer loss function + // TODO: benchmark whether sparse factorization is faster + if (memory->outer_hess_is_diag) + { + // store only diagonal element of W_chol + for (int i = 0; i < ny; i++) + { + BLASFEO_DVECEL(&memory->W_chol_diag, i) = sqrt(BLASFEO_DMATEL(&work->W, i, i)); + } + } + else + { + blasfeo_dpotrf_l(ny, &work->W, 0, 0, &memory->W_chol, 0, 0); + } if (nz > 0) { @@ -669,9 +704,18 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode 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); + + if (memory->outer_hess_is_diag) + { + // tmp_nv_ny = Jt_ux_tilde * W_chol_diag + blasfeo_dgemm_nd(nu + nx, ny, 1.0, &work->Jt_ux_tilde, 0, 0, &memory->W_chol_diag, 0, 0., &work->Jt_ux_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // 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 { @@ -679,9 +723,18 @@ void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims_, void *mode 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); + + if (memory->outer_hess_is_diag) + { + // tmp_nv_ny = Jt_ux * W_chol_diag + blasfeo_dgemm_nd(nu+nx, ny, 1.0, &work->Jt_ux, 0, 0, &memory->W_chol_diag, 0, 0., &work->tmp_nv_ny, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // 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 @@ -734,8 +787,8 @@ void ocp_nlp_cost_conl_compute_gradient(void *config_, void *dims_, void *model_ { ext_fun_arg_t conl_fun_jac_hess_type_in[5]; void *conl_fun_jac_hess_in[5]; - ext_fun_arg_t conl_fun_jac_hess_type_out[5]; - void *conl_fun_jac_hess_out[5]; + ext_fun_arg_t conl_fun_jac_hess_type_out[6]; + void *conl_fun_jac_hess_out[6]; // INPUT struct blasfeo_dvec_args x_in; // input x @@ -770,6 +823,8 @@ void ocp_nlp_cost_conl_compute_gradient(void *config_, void *dims_, void *model_ conl_fun_jac_hess_out[3] = &work->Jt_z; // inner Jacobian wrt z, transposed, nz x ny conl_fun_jac_hess_type_out[4] = BLASFEO_DMAT; conl_fun_jac_hess_out[4] = &work->W; // outer hessian: ny x ny + conl_fun_jac_hess_type_out[5] = COLMAJ; + conl_fun_jac_hess_out[5] = &memory->outer_hess_is_diag; // flag indicates if outer hess is diag // NOTE: could be done more efficiently by generating a function that does not evalutate hessian // evaluate external function @@ -915,6 +970,8 @@ void ocp_nlp_cost_conl_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_cost_conl_memory_get_fun_ptr; config->memory_get_grad_ptr = &ocp_nlp_cost_conl_memory_get_grad_ptr; config->memory_get_W_chol_ptr = &ocp_nlp_cost_conl_memory_get_W_chol_ptr; + 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->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; diff --git a/acados/ocp_nlp/ocp_nlp_cost_conl.h b/acados/ocp_nlp/ocp_nlp_cost_conl.h index 3bb4560900..4b50691ae9 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_conl.h +++ b/acados/ocp_nlp/ocp_nlp_cost_conl.h @@ -141,8 +141,10 @@ typedef struct struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function + struct blasfeo_dvec W_chol_diag; // cholesky factor of hessian of outer loss function if Hessian is diagonal // NOTE: could be in work, but needed for compatibility with NLS and cost integration double fun; ///< value of the cost function + double outer_hess_is_diag; } ocp_nlp_cost_conl_memory; // diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index 0e6e775a41..6f003fd6f7 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -299,6 +299,31 @@ int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_, double *W_col_maj = (double *) value_; blasfeo_pack_dmat(ny, ny, W_col_maj, ny, &model->W, 0, 0); model->W_changed = 1; + if (ny > 4) + { + // detect if outer hess is diag + model->outer_hess_is_diag = 1.0; + double tmp; + for (int i = 0; i < ny; i++) + { + for (int j = 0; j < ny; j++) + { + if (j!=i) + { + tmp = BLASFEO_DMATEL(&model->W, i, j); + if (tmp != 0.0) + { + model->outer_hess_is_diag = 0.0; + } + } + } + } + } + else + { + // use BLASFEO matrices for small ny. + model->outer_hess_is_diag = 0.0; + } } else if (!strcmp(field, "Cyt")) { @@ -485,6 +510,7 @@ acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config_, size += 1 * blasfeo_memsize_dmat(ny, ny); // W_chol size += 1 * blasfeo_memsize_dvec(ny); // res size += 1 * blasfeo_memsize_dvec(nu + nx + 2 * ns); // grad + size += 1 * blasfeo_memsize_dvec(ny); // W_chol_diag size += 1 * 64; // blasfeo_mem align @@ -517,6 +543,8 @@ void *ocp_nlp_cost_ls_memory_assign(void *config_, void *dims_, void *opts_, assign_and_advance_blasfeo_dmat_mem(nu + nx, nu + nx, &memory->hess, &c_ptr); // W_chol assign_and_advance_blasfeo_dmat_mem(ny, ny, &memory->W_chol, &c_ptr); + // W_chol_diag + assign_and_advance_blasfeo_dvec_mem(ny, &memory->W_chol_diag, &c_ptr); // res assign_and_advance_blasfeo_dvec_mem(ny, &memory->res, &c_ptr); // grad @@ -684,7 +712,7 @@ static void ocp_nlp_cost_ls_cast_workspace(void *config_, -static void ocp_nlp_cost_ls_update_hessian(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) +static void ocp_nlp_cost_ls_update_W_factorization(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_) { ocp_nlp_cost_ls_dims *dims = dims_; ocp_nlp_cost_ls_model *model = model_; @@ -700,14 +728,34 @@ static void ocp_nlp_cost_ls_update_hessian(void *config_, void *dims_, void *mod // refactorize Hessian only if W has changed if (model->W_changed) { - blasfeo_dpotrf_l(ny, &model->W, 0, 0, &memory->W_chol, 0, 0); + if (model->outer_hess_is_diag) + { + // store only diagonal element of W_chol + for (int i = 0; i < ny; i++) + { + BLASFEO_DVECEL(&memory->W_chol_diag, i) = sqrt(BLASFEO_DMATEL(&model->W, i, i)); + } + } + else + { + blasfeo_dpotrf_l(ny, &model->W, 0, 0, &memory->W_chol, 0, 0); + } model->W_changed = 0; model->Cyt_or_scaling_changed = 1; // execute lower part } if (model->Cyt_or_scaling_changed) { - blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, &model->Cyt, - 0, 0, &work->tmp_nv_ny, 0, 0); + if (model->outer_hess_is_diag) + { + // tmp_nv_ny = W_chol_diag * Cyt + blasfeo_dgemm_nd(nu + nx, ny, 1.0, &model->Cyt, 0, 0, &memory->W_chol_diag, 0, 0., &model->Cyt, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // tmp_nv_ny = W_chol * Cyt + blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, + &model->Cyt, 0, 0, &work->tmp_nv_ny, 0, 0); + } // hess = scaling * tmp_nv_ny * tmp_nv_ny^T blasfeo_dsyrk_ln(nu+nx, ny, model->scaling, &work->tmp_nv_ny, 0, 0, @@ -724,7 +772,7 @@ void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void * { ocp_nlp_cost_ls_model *model = model_; model->W_changed = 1; - ocp_nlp_cost_ls_update_hessian(config_, dims_, model_, opts_, memory_, work_); + ocp_nlp_cost_ls_update_W_factorization(config_, dims_, model_, opts_, memory_, work_); return; } @@ -738,7 +786,7 @@ void ocp_nlp_cost_ls_initialize(void *config_, void *dims_, void *model_, ocp_nlp_cost_ls_memory *memory = memory_; ocp_nlp_cost_ls_cast_workspace(config_, dims_, opts_, work_); - ocp_nlp_cost_ls_update_hessian(config_, dims_, model_, opts_, memory_, work_); + ocp_nlp_cost_ls_update_W_factorization(config_, dims_, model_, opts_, memory_, work_); int ns = dims->ns; // mem->Z = scaling * model->Z @@ -784,8 +832,17 @@ void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims_, 0, 0, &work->tmp_nz, 0, 1.0, &model->y_ref, 0, &work->y_ref_tilde, 0); // tmp_nv_ny = W_chol * Cyt_tilde - blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, - &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + if (model->outer_hess_is_diag) + { + // tmp_nv_ny = W_chol_diag * Cyt_tilde + blasfeo_dgemm_nd(nu + nx, ny, 1.0, &work->Cyt_tilde, 0, 0, &memory->W_chol_diag, 0, 0., &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // tmp_nv_ny = W_chol * Cyt_tilde + blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, + &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } // add hessian of the cost contribution if (opts->compute_hess) @@ -908,7 +965,16 @@ void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims_, void *model_, void } // tmp_ny = W_chol^T * res - blasfeo_dtrmv_ltn(ny, &memory->W_chol, 0, 0, &memory->res, 0, &work->tmp_ny, 0); + if (model->outer_hess_is_diag) + { + // tmp_ny = W_chol_diag * nls_res (componentwise) + blasfeo_dvecmul(ny, &memory->W_chol_diag, 0, &memory->res, 0, &work->tmp_ny, 0); + } + else + { + // tmp_ny = W_chol * nls_res + blasfeo_dtrmv_ltn(ny, &memory->W_chol, 0, 0, &memory->res, 0, &work->tmp_ny, 0); + } // fun = .5 * tmp_ny^T * tmp_ny memory->fun = 0.5 * blasfeo_ddot(ny, &work->tmp_ny, 0, &work->tmp_ny, 0); diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.h b/acados/ocp_nlp/ocp_nlp_cost_ls.h index 6c9f5d2382..c3277b3c17 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -47,6 +47,8 @@ extern "C" { #endif +#include + // blasfeo #include "blasfeo/include/blasfeo_common.h" @@ -109,6 +111,7 @@ typedef struct struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper) struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper) double scaling; + double outer_hess_is_diag; int W_changed; ///< flag indicating whether W has changed and needs to be refactorized int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed } ocp_nlp_cost_ls_model; @@ -159,6 +162,7 @@ typedef struct { struct blasfeo_dmat hess; ///< hessian of cost function struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix + struct blasfeo_dvec W_chol_diag; ///< W_chol_diag struct blasfeo_dvec res; ///< ls residual r(x) struct blasfeo_dvec grad; ///< gradient of cost function struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index bd72a675c4..7480d9a828 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -35,6 +35,7 @@ #include #include #include +#include // blasfeo #include "blasfeo/include/blasfeo_d_aux.h" @@ -195,6 +196,7 @@ void *ocp_nlp_cost_nls_model_assign(void *config_, void *dims_, void *raw_memory // default initialization model->scaling = 1.0; model->t = 0.0; + model->outer_hess_is_diag = 0; // initialize to 1 to update factorization of W in precompute model->W_changed = 1; @@ -231,6 +233,31 @@ int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, double *W_col_maj = (double *) value_; blasfeo_pack_dmat(ny, ny, W_col_maj, ny, &model->W, 0, 0); model->W_changed = 1; + if (ny > 4) + { + // detect if outer hess is diag + model->outer_hess_is_diag = 1.0; + double tmp; + for (int i = 0; i < ny; i++) + { + for (int j = 0; j < ny; j++) + { + if (j!=i) + { + tmp = BLASFEO_DMATEL(&model->W, i, j); + if (tmp != 0.0) + { + model->outer_hess_is_diag = 0.0; + } + } + } + } + } + else + { + // use BLASFEO matrices for small ny. + model->outer_hess_is_diag = 0.0; + } } else if (!strcmp(field, "y_ref") || !strcmp(field, "yref")) { @@ -412,6 +439,7 @@ acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config_, void *dims_, size += sizeof(ocp_nlp_cost_nls_memory); size += 1 * blasfeo_memsize_dmat(ny, ny); // W_chol + size += 1 * blasfeo_memsize_dvec(ny); // W_chol_diag size += 1 * blasfeo_memsize_dmat(nu + nx, ny); // Jt size += 1 * blasfeo_memsize_dvec(ny); // res size += 1 * blasfeo_memsize_dvec(nu + nx + 2 * ns); // grad @@ -448,6 +476,8 @@ void *ocp_nlp_cost_nls_memory_assign(void *config_, void *dims_, void *opts_, vo assign_and_advance_blasfeo_dmat_mem(ny, ny, &memory->W_chol, &c_ptr); // Jt assign_and_advance_blasfeo_dmat_mem(nu + nx, ny, &memory->Jt, &c_ptr); + // W_chol_diag + assign_and_advance_blasfeo_dvec_mem(ny, &memory->W_chol_diag, &c_ptr); // res assign_and_advance_blasfeo_dvec_mem(ny, &memory->res, &c_ptr); // grad @@ -477,6 +507,25 @@ struct blasfeo_dmat *ocp_nlp_cost_nls_memory_get_W_chol_ptr(void *memory_) } +struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_W_chol_diag_ptr(void *memory_) +{ + ocp_nlp_cost_nls_memory *memory = memory_; + + return &memory->W_chol_diag; +} + + +double *ocp_nlp_cost_nls_get_outer_hess_is_diag_ptr(void *memory_, void *model_) +{ + // ocp_nlp_cost_nls_memory *memory = memory_; + ocp_nlp_cost_nls_model *model = model_; + + return &model->outer_hess_is_diag; +} + + + + struct blasfeo_dvec *ocp_nlp_cost_nls_model_get_y_ref_ptr(void *in_) { ocp_nlp_cost_nls_model *model = in_; @@ -643,7 +692,18 @@ static void ocp_nlp_cost_nls_update_W_factorization(void *config_, void *dims_, if (model->W_changed) { - blasfeo_dpotrf_l(ny, &model->W, 0, 0, &memory->W_chol, 0, 0); + if (model->outer_hess_is_diag) + { + // store only diagonal element of W_chol + for (int i = 0; i < ny; i++) + { + BLASFEO_DVECEL(&memory->W_chol_diag, i) = sqrt(BLASFEO_DMATEL(&model->W, i, i)); + } + } + else + { + blasfeo_dpotrf_l(ny, &model->W, 0, 0, &memory->W_chol, 0, 0); + } model->W_changed = 0; } return; @@ -756,11 +816,18 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model blasfeo_dgemv_n(nu+nx, ny, 1.0, &work->Cyt_tilde, 0, 0, &work->tmp_ny, 0, 0.0, &memory->grad, 0, &memory->grad, 0); - // gauss-newton component update - // tmp_nv_ny = W_chol * Cyt_tilde - blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, - &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + if (model->outer_hess_is_diag) + { + // tmp_nv_ny = W_chol_diag * Cyt_tilde + blasfeo_dgemm_nd(nu + nx, ny, 1.0, &work->Cyt_tilde, 0, 0, &memory->W_chol_diag, 0, 0., &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // tmp_nv_ny = W_chol * Cyt_tilde + blasfeo_dtrmm_rlnn(nu + nx, ny, 1.0, &memory->W_chol, 0, 0, + &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } } else { @@ -768,9 +835,17 @@ void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims_, void *model blasfeo_dgemv_n(nu+nx, ny, 1.0, &memory->Jt, 0, 0, &work->tmp_ny, 0, 0.0, &memory->grad, 0, &memory->grad, 0); // gauss-newton component update - // tmp_nv_ny = Jt * W_chol, where W_chol is lower triangular - blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, &memory->W_chol, 0, 0, &memory->Jt, 0, 0, - &work->tmp_nv_ny, 0, 0); + if (model->outer_hess_is_diag) + { + // tmp_nv_ny = Jt * W_chol_diag + blasfeo_dgemm_nd(nu + nx, ny, 1.0, &memory->Jt, 0, 0, &memory->W_chol_diag, 0, 0., &work->Cyt_tilde, 0, 0, &work->tmp_nv_ny, 0, 0); + } + else + { + // tmp_nv_ny = Jt * W_chol, where W_chol is lower triangular + blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, &memory->W_chol, 0, 0, &memory->Jt, 0, 0, + &work->tmp_nv_ny, 0, 0); + } } // function @@ -925,8 +1000,17 @@ void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims_, void *model_, // res = res - y_ref blasfeo_daxpy(ny, -1.0, &model->y_ref, 0, &memory->res, 0, &memory->res, 0); - // tmp_ny = W_chol * nls_res - blasfeo_dtrmv_ltn(ny, &memory->W_chol, 0, 0, &memory->res, 0, &work->tmp_ny, 0); + + if (model->outer_hess_is_diag) + { + // tmp_ny = W_chol_diag * nls_res (componentwise) + blasfeo_dvecmul(ny, &memory->W_chol_diag, 0, &memory->res, 0, &work->tmp_ny, 0); + } + else + { + // tmp_ny = W_chol * nls_res + blasfeo_dtrmv_ltn(ny, &memory->W_chol, 0, 0, &memory->res, 0, &work->tmp_ny, 0); + } memory->fun = 0.5 * blasfeo_ddot(ny, &work->tmp_ny, 0, &work->tmp_ny, 0); } @@ -969,6 +1053,8 @@ void ocp_nlp_cost_nls_config_initialize_default(void *config_) config->memory_get_fun_ptr = &ocp_nlp_cost_nls_memory_get_fun_ptr; config->memory_get_grad_ptr = &ocp_nlp_cost_nls_memory_get_grad_ptr; config->memory_get_W_chol_ptr = &ocp_nlp_cost_nls_memory_get_W_chol_ptr; + config->memory_get_W_chol_diag_ptr = &ocp_nlp_cost_nls_memory_get_W_chol_diag_ptr; + config->get_outer_hess_is_diag_ptr = &ocp_nlp_cost_nls_get_outer_hess_is_diag_ptr; config->model_get_y_ref_ptr = &ocp_nlp_cost_nls_model_get_y_ref_ptr; config->memory_set_ux_ptr = &ocp_nlp_cost_nls_memory_set_ux_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_cost_nls_memory_set_z_alg_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.h b/acados/ocp_nlp/ocp_nlp_cost_nls.h index 5884ebc0e0..0a34065d2b 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -95,6 +95,7 @@ typedef struct struct blasfeo_dvec z; // gradient of slacks as vector double scaling; double t; // time (always zero) to match signature of external function wrt cost integration + double outer_hess_is_diag; // flag indicating if outer_hess_is_diag; Note: double for compatibility with CONL cost int W_changed; ///< flag indicating whether W has changed and needs to be refactorized } ocp_nlp_cost_nls_model; @@ -137,6 +138,7 @@ void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void typedef struct { struct blasfeo_dmat W_chol; // cholesky factor of weight matrix + struct blasfeo_dvec W_chol_diag; // cholesky factor of weight matrix if the Hessian is diagonal struct blasfeo_dmat Jt; // jacobian of nls fun struct blasfeo_dvec res; // nls residual r(x) struct blasfeo_dvec grad; // gradient of cost function @@ -159,6 +161,8 @@ struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_); // struct blasfeo_dmat *ocp_nlp_cost_nls_memory_get_W_chol_ptr(void *memory_); // +struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_W_chol_diag_ptr(void *memory_); +// void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); // void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c index 1461f45c4f..42d7478593 100644 --- a/acados/ocp_nlp/ocp_nlp_dynamics_cont.c +++ b/acados/ocp_nlp/ocp_nlp_dynamics_cont.c @@ -533,7 +533,7 @@ void ocp_nlp_dynamics_cont_memory_set(void *config_, void *dims_, void *mem_, co sim_config *sim = config->sim_solver; - if (!strcmp(field, "W_chol") || !strcmp(field, "cost_fun") || !strcmp(field, "cost_hess") || !strcmp(field, "cost_grad") + if (!strcmp(field, "W_chol") || !strcmp(field, "W_chol_diag") || !strcmp(field, "cost_fun") || !strcmp(field, "outer_hess_is_diag") || !strcmp(field, "cost_hess") || !strcmp(field, "cost_grad") || !strcmp(field, "y_ref")) { sim->memory_set(sim, dims->sim, mem->sim_solver, field, value); diff --git a/acados/sim/sim_irk_integrator.c b/acados/sim/sim_irk_integrator.c index 1f5cb48d78..7cc17a16d7 100644 --- a/acados/sim/sim_irk_integrator.c +++ b/acados/sim/sim_irk_integrator.c @@ -36,6 +36,8 @@ #include #include #include +#include + // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" @@ -45,6 +47,7 @@ #include "blasfeo/include/blasfeo_d_aux.h" #include "blasfeo/include/blasfeo_d_blas.h" +#include "blasfeo/include/blasfeo_common.h" /************************************************ @@ -469,6 +472,14 @@ int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field { mem->W_chol = value; } + else if (!strcmp(field, "W_chol_diag")) + { + mem->W_chol_diag = value; + } + else if (!strcmp(field, "outer_hess_is_diag")) + { + mem->outer_hess_is_diag = value; + } else if (!strcmp(field, "y_ref")) { mem->y_ref = value; @@ -1362,12 +1373,25 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // transpose blasfeo_dgetr(ny, nx+nu, J_y_tilde, 0, 0, tmp_nux_ny, 0, 0); - // tmp_nux_ny2 = W_chol * J_y_tilde (ny * (nx+nu)) - blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, mem->W_chol, 0, 0, tmp_nux_ny, 0, 0, - tmp_nux_ny2, 0, 0); - // tmp_ny = W_chol * nls_res - blasfeo_dtrmv_lnn(ny, mem->W_chol, 0, 0, nls_res, 0, tmp_ny, 0); + if (*mem->outer_hess_is_diag) + { + // tmp_nv_ny = W_chol_diag * Cyt_tilde + blasfeo_dgemm_nd(nu+nx, ny, 1.0, tmp_nux_ny, 0, 0, mem->W_chol_diag, 0, 0., tmp_nux_ny2, 0, 0, tmp_nux_ny2, 0, 0); + + + // tmp_ny = W_chol_diag * nls_res (componentwise) + blasfeo_dvecmul(ny, mem->W_chol_diag, 0, nls_res, 0, tmp_ny, 0); + } + else + { + // tmp_nux_ny2 = W_chol * J_y_tilde (ny * (nx+nu)) + blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, mem->W_chol, 0, 0, tmp_nux_ny, 0, 0, + tmp_nux_ny2, 0, 0); + + // tmp_ny = W_chol * nls_res + blasfeo_dtrmv_lnn(ny, mem->W_chol, 0, 0, nls_res, 0, tmp_ny, 0); + } // 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, @@ -1388,8 +1412,8 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // prepare function ext_fun_arg_t conl_fun_jac_hess_type_in[5]; void *conl_fun_jac_hess_in[5]; - ext_fun_arg_t conl_fun_jac_hess_type_out[5]; - void *conl_fun_jac_hess_out[5]; + ext_fun_arg_t conl_fun_jac_hess_type_out[6]; + void *conl_fun_jac_hess_out[6]; // inputs conl_fun_jac_hess_type_in[0] = BLASFEO_DVEC; @@ -1415,6 +1439,8 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo conl_fun_jac_hess_out[3] = workspace->Jt_z; // inner Jacobian wrt z, transposed, nz x ny conl_fun_jac_hess_type_out[4] = BLASFEO_DMAT; conl_fun_jac_hess_out[4] = workspace->W; // outer hessian: ny x ny + conl_fun_jac_hess_type_out[5] = COLMAJ; + conl_fun_jac_hess_out[5] = mem->outer_hess_is_diag; // flag indicates if outer hess is diag for (int ii = 0; ii < ns; ii++) { @@ -1436,10 +1462,22 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // evaluate external function model->conl_cost_fun_jac_hess->evaluate(model->conl_cost_fun_jac_hess, conl_fun_jac_hess_type_in, conl_fun_jac_hess_in, conl_fun_jac_hess_type_out, conl_fun_jac_hess_out); - // hessian of outer loss function - blasfeo_dpotrf_l(ny, workspace->W, 0, 0, mem->W_chol, 0, 0); + // factorize hessian of outer loss function + if (*mem->outer_hess_is_diag) + { + // store only diagonal element of W_chol + for (int i = 0; i < ny; i++) + { + BLASFEO_DVECEL(mem->W_chol_diag, i) = sqrt(BLASFEO_DMATEL(workspace->W, i, i)); + } + } + else + { + blasfeo_dpotrf_l(ny, workspace->W, 0, 0, mem->W_chol, 0, 0); + } if (nz > 0) // TODO: test this! + // TODO use diag hess also here { // // Jt_ux_tilde = workspace->tmp_nux_ny + dzdux_tran*Jt_z // blasfeo_dgemm_nn(nu + nx, ny, nz, 1.0, memory->dzdux_tran, 0, 0, @@ -1470,9 +1508,18 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // transpose blasfeo_dgetr(ny, nx+nu, J_y_tilde, 0, 0, tmp_nux_ny, 0, 0); - // tmp_nux_ny2 = W_chol * J_y_tilde (ny * (nx+nu)) - blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, mem->W_chol, 0, 0, tmp_nux_ny, 0, 0, - tmp_nux_ny2, 0, 0); + + if (*mem->outer_hess_is_diag) + { + // tmp_nux_ny2 = W_chol_diag * J_y_tilde (ny * (nx+nu)) + blasfeo_dgemm_nd(nu+nx, ny, 1.0, tmp_nux_ny, 0, 0, mem->W_chol_diag, 0, 0., tmp_nux_ny2, 0, 0, tmp_nux_ny2, 0, 0); + } + else + { + // tmp_nux_ny2 = W_chol * J_y_tilde (ny * (nx+nu)) + blasfeo_dtrmm_rlnn(nu+nx, ny, 1.0, mem->W_chol, 0, 0, tmp_nux_ny, 0, 0, + tmp_nux_ny2, 0, 0); + } // 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, @@ -1534,8 +1581,15 @@ int sim_irk(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, vo // nls_res = nls_res - y_ref blasfeo_daxpy(ny, -1.0, mem->y_ref, 0, nls_res, 0, nls_res, 0); - // tmp_ny = W_chol * nls_res - blasfeo_dtrmv_lnn(ny, mem->W_chol, 0, 0, nls_res, 0, tmp_ny, 0); + if (*mem->outer_hess_is_diag) + { + // tmp_ny = W_chol_diag * nls_res (componentwise) + blasfeo_dvecmul(ny, mem->W_chol_diag, 0, nls_res, 0, tmp_ny, 0); + } + else { + // tmp_ny = W_chol * nls_res + blasfeo_dtrmv_lnn(ny, mem->W_chol, 0, 0, nls_res, 0, tmp_ny, 0); + } // cost function value // NOTE: slack contribution and scaling done in cost module diff --git a/acados/sim/sim_irk_integrator.h b/acados/sim/sim_irk_integrator.h index cd835575bc..8a3533f6c1 100644 --- a/acados/sim/sim_irk_integrator.h +++ b/acados/sim/sim_irk_integrator.h @@ -148,7 +148,6 @@ typedef struct struct blasfeo_dvec *nls_res; // only for cost_propagation with CONVEX_OVER_NONLINEAR struct blasfeo_dmat *W; - struct blasfeo_dmat *W_chol; struct blasfeo_dmat *tmp_nv_ny; struct blasfeo_dmat *Jt_z; @@ -166,7 +165,9 @@ typedef struct double time_la; double *cost_fun; + double *outer_hess_is_diag; 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 struct blasfeo_dvec *cost_grad; struct blasfeo_dmat *cost_hess; diff --git a/interfaces/acados_matlab_octave/acados_ocp_model.m b/interfaces/acados_matlab_octave/acados_ocp_model.m index b3a6d6ce28..c4c0d1891a 100644 --- a/interfaces/acados_matlab_octave/acados_ocp_model.m +++ b/interfaces/acados_matlab_octave/acados_ocp_model.m @@ -29,6 +29,7 @@ % + classdef acados_ocp_model < handle properties @@ -412,18 +413,18 @@ else if (strcmp(field, 'name')) - obj.model_struct.name = value; + obj.model_struct.name = value; elseif (strcmp(field, 'T')) obj.model_struct.T = value; else disp(['acados_ocp_model: set: wrong field: ', field]); keyboard; end - end + end end end % methods - + diff --git a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/acados_ocp_layout.json b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/acados_ocp_layout.json index 3d6de7c2c0..b1c4d72b6d 100644 --- a/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/acados_ocp_layout.json +++ b/interfaces/acados_matlab_octave/acados_template_mex/+acados_template_mex/acados_ocp_layout.json @@ -519,6 +519,13 @@ "ny" ] ], + "W_e": [ + "ndarray", + [ + "ny_e", + "ny_e" + ] + ], "Zl": [ "ndarray", [ @@ -543,13 +550,6 @@ "ns" ] ], - "W_e": [ - "ndarray", - [ - "ny_e", - "ny_e" - ] - ], "yref_0": [ "ndarray", [ diff --git a/interfaces/acados_matlab_octave/detect_cost_type.m b/interfaces/acados_matlab_octave/detect_cost_type.m index ebcc2225f2..1b8df6f849 100644 --- a/interfaces/acados_matlab_octave/detect_cost_type.m +++ b/interfaces/acados_matlab_octave/detect_cost_type.m @@ -78,7 +78,7 @@ if expr_cost.is_quadratic(x) && expr_cost.is_quadratic(u) && expr_cost.is_quadratic(z) ... && ~any(expr_cost.which_depends(p)) - + if expr_cost.is_zero() fprintf('Cost function is zero -> Reformulating as linear_ls cost.\n'); cost_type = 'linear_ls'; diff --git a/interfaces/acados_matlab_octave/ocp_generate_c_code.m b/interfaces/acados_matlab_octave/ocp_generate_c_code.m index 31c25e2bda..792cfadc5e 100644 --- a/interfaces/acados_matlab_octave/ocp_generate_c_code.m +++ b/interfaces/acados_matlab_octave/ocp_generate_c_code.m @@ -207,6 +207,8 @@ function ocp_generate_c_code(obj) if this_dims(1) == 1 && length(property_dim_names) ~= 1 % matrix with 1 row cost.(fields{i}) = {cost.(fields{i})}; end + elseif strcmp(cost_layout.(fields{i}){1}, 'int') + cost.(fields{i}) = cost.(fields{i}){1}; end end obj.acados_ocp_nlp_json.cost = cost; diff --git a/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m b/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m index f7b5f6edd1..605b296b6f 100644 --- a/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m +++ b/interfaces/acados_matlab_octave/set_up_acados_ocp_nlp_json.m @@ -448,6 +448,7 @@ end if strcmp(model.cost_type_0, 'nonlinear_ls') || strcmp(model.cost_type_0, 'linear_ls') ocp_json.cost.W_0 = model.cost_W_0; + if isfield(model, 'cost_y_ref_0') ocp_json.cost.yref_0 = model.cost_y_ref_0; else @@ -463,6 +464,7 @@ if strcmp(model.cost_type_e, 'nonlinear_ls') || strcmp(model.cost_type_e, 'linear_ls') if isfield(model, 'cost_W_e') ocp_json.cost.W_e = model.cost_W_e; + end if isfield(model, 'cost_y_ref_e') ocp_json.cost.yref_e = model.cost_y_ref_e; diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index ae46c1892a..16ca7cd6cb 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -328,6 +328,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: ocp_generate_external_functions(acados_ocp) ocp_formulation_json_dump(acados_ocp, json_file, simulink_opts=simulink_opts) ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder, simulink_opts=simulink_opts) + elif isinstance(acados_ocp, AcadosMultiphaseOcp): mocp_generate_external_functions(acados_ocp) mocp_formulation_json_dump(acados_ocp, json_file) 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 c442beaf6a..5a380bae81 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 @@ -409,7 +409,6 @@ void {{ name }}_acados_create_3_create_and_set_functions({{ name }}_solver_capsu { const int N = capsule->nlp_solver_plan->N; - /************************************************ * external functions ************************************************/ @@ -805,6 +804,8 @@ void {{ name }}_acados_create_5_set_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + int tmp_int = 0; + /************************************************ * nlp_in ************************************************/ @@ -1233,6 +1234,7 @@ void {{ name }}_acados_create_5_set_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { i_fun = i - {{ cost_start_idx[jj] }}; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun_{{ jj }}[i_fun]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt_{{ jj }}[i_fun]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess_{{ jj }}[i_fun]); @@ -1781,7 +1783,6 @@ void {{ name }}_acados_create_5_set_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); {%- elif cost_e.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun", &capsule->conl_cost_e_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun_jac_hess", &capsule->conl_cost_e_fun_jac_hess); 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 d8e7ea6edb..2c5ae6ff71 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 @@ -767,6 +767,8 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + int tmp_int = 0; + /************************************************ * nlp_in ************************************************/ @@ -1115,7 +1117,6 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun", &capsule->conl_cost_e_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun_jac_hess", &capsule->conl_cost_e_fun_jac_hess); diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 942c8ad91e..1793bef5b2 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -509,14 +509,17 @@ def generate_c_code_nls_cost(ocp: AcadosOcp, stage_type ): u = symbol('u', 0, 0) t = symbol('t', 0, 0) y_expr = model.cost_y_expr_e + outer_hess = ocp.cost.W_e elif stage_type == 'initial': middle_name = '_cost_y_0' y_expr = model.cost_y_expr_0 + outer_hess = ocp.cost.W_0 elif stage_type == 'path': middle_name = '_cost_y' y_expr = model.cost_y_expr + outer_hess = ocp.cost.W # checks on time dependency if any(ca.which_depends(y_expr, model.t)): @@ -639,6 +642,10 @@ def generate_c_code_conl_cost(ocp: AcadosOcp, stage_type: str): hess = custom_hess outer_hess_fun = ca.Function('outer_hess', [res_expr, t, p], [hess]) + outer_hess_expr = outer_hess_fun(inner_expr, t, p) + 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 @@ -651,7 +658,7 @@ def generate_c_code_conl_cost(ocp: AcadosOcp, stage_type: str): cost_fun_jac_hess = ca.Function( 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_fun(inner_expr, 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] ) # change directory From ec60a4895e7034b66b28e0e84108b1108f721803 Mon Sep 17 00:00:00 2001 From: Josip Kir Hromatko <36133788+josipkh@users.noreply.github.com> Date: Thu, 14 Mar 2024 17:22:28 +0100 Subject: [PATCH 13/16] Update MATLAB examples with general initial input constraints (#1048) In this PR the following changes are suggested: - all MATLAB examples are updated as suggested in #1009 - some examples were modified to remove errors caused by calling outdated structures (e.g., `sim.C_ocp`) - additional comments were added in some `getting_started` examples - some models were updated for improved naming consistency (e.g., using `constr_expr_h` instead of `expr_h`) - examples and tests were updated to work with the changes in variable naming - a test script that runs all the examples is added (also contains test results on my laptop) - a few lines were terminated to suppress unnecessary printing --- .../generic_dyn_disc/disc_dyn_example_ocp.m | 29 ++-- .../external_cost_example_ocp.m | 34 +++-- .../getting_started/extensive_example_ocp.m | 120 +++++----------- .../minimal_example_closed_loop.m | 59 ++++---- .../getting_started/minimal_example_ocp.m | 69 ++++----- .../getting_started/minimal_example_sim.m | 8 +- .../getting_started/simulink_example.m | 21 ++- .../simulink_example_advanced.m | 16 ++- .../simulink_model_advanced_closed_loop.slx | Bin 25778 -> 33390 bytes .../simulink_model_closed_loop.slx | Bin 15164 -> 25071 bytes .../simulink_model_integrator.slx | Bin 12383 -> 15015 bytes .../example_closed_loop.m | 58 ++++---- .../linear_mass_spring_model/example_ocp.m | 63 ++++----- .../linear_mass_spring_model/example_sim.m | 26 +--- .../linear_mass_spring_model.m | 47 ++++--- .../pendulum_dae/example_closed_loop.m | 6 +- .../pendulum_dae/example_sim.m | 2 +- .../example_closed_loop.m | 15 +- .../pendulum_on_cart_model/example_ocp.m | 14 +- .../example_ocp_custom_hess.m | 15 +- .../example_ocp_param_sens.m | 11 +- .../pendulum_on_cart_model/example_ocp_reg.m | 11 +- .../pendulum_on_cart_model/example_sim.m | 6 +- .../example_solution_sens_closed_loop.m | 19 +-- .../experiment_dae_formulation.m | 10 +- .../pendulum_on_cart_model.m | 72 +++++++--- .../pendulum_on_cart_model_dae.m | 8 +- .../simple_dae_model/example_ocp.m | 2 +- .../swarming/example_closed_loop.m | 14 +- .../swarming/example_ocp.m | 5 +- .../acados_matlab_octave/test/simulink_test.m | 1 + .../test/simulink_u_traj_ref.mat | Bin 340 -> 343 bytes .../test/test_all_examples.m | 132 ++++++++++++++++++ .../acados_matlab_octave/test/test_checks.m | 4 +- .../acados_matlab_octave/test/test_ocp_OSQP.m | 10 +- .../test/test_ocp_linear_mass_spring.m | 18 +-- .../test/test_ocp_pendulum_on_cart.m | 17 ++- .../test/test_ocp_qpdunes.m | 13 +- .../acados_matlab_octave/test/test_sens_adj.m | 4 +- .../test/test_sens_forw.m | 4 +- .../test/test_sens_hess.m | 4 +- .../generate_c_code_nonlinear_least_squares.m | 6 +- 42 files changed, 580 insertions(+), 393 deletions(-) create mode 100644 examples/acados_matlab_octave/test/test_all_examples.m diff --git a/examples/acados_matlab_octave/generic_dyn_disc/disc_dyn_example_ocp.m b/examples/acados_matlab_octave/generic_dyn_disc/disc_dyn_example_ocp.m index 0d78a1557d..15d8a5b707 100644 --- a/examples/acados_matlab_octave/generic_dyn_disc/disc_dyn_example_ocp.m +++ b/examples/acados_matlab_octave/generic_dyn_disc/disc_dyn_example_ocp.m @@ -30,14 +30,14 @@ % %% test of native matlab interface -clear all - +clear all; clc; +check_acados_requirements() addpath('../linear_mass_spring_model/'); %% arguments N = 20; tol = 1e-8; -shooting_nodes = [ linspace(0,10,N+1) ]; +shooting_nodes = linspace(0,10,N+1); model_name = 'lin_mass'; @@ -70,6 +70,8 @@ % constraints x0 = zeros(nx, 1); x0(1)=2.5; x0(2)=2.5; +lh_0 = -0.5 * ones(nu, 1); +uh_0 = 0.5 * ones(nu, 1); lh = - [ 0.5 * ones(nu, 1); 4.0 * ones(nx, 1)]; uh = + [ 0.5 * ones(nu, 1); 4.0 * ones(nx, 1)]; lh_e = -4.0 * ones(nx, 1); @@ -94,8 +96,10 @@ end % cost +ocp_model.set('cost_type_0', cost_type); ocp_model.set('cost_type', cost_type); ocp_model.set('cost_type_e', cost_type); + % dynamics ocp_model.set('dyn_type', 'discrete'); @@ -108,10 +112,14 @@ ocp_model.set('dyn_disc_fun_jac_hess', 'disc_dyn_fun_jac_hess'); % only needed for exact hessian else % dynamics expression - ocp_model.set('dyn_expr_phi', model.expr_phi); + ocp_model.set('dyn_expr_phi', model.dyn_expr_phi); end if (casadi_cost == 0) + % Generic initial cost + ocp_model.set('cost_ext_fun_type_0', 'generic'); + ocp_model.set('cost_source_ext_cost_0', 'generic_ext_cost.c'); + ocp_model.set('cost_function_ext_cost_0', 'ext_cost'); % Generic stage cost ocp_model.set('cost_ext_fun_type', 'generic'); ocp_model.set('cost_source_ext_cost', 'generic_ext_cost.c'); @@ -122,16 +130,20 @@ ocp_model.set('cost_function_ext_cost_e', 'ext_costN'); else % cost expression - ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); - ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % constraints ocp_model.set('constr_x0', x0); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h_0); +ocp_model.set('constr_lh_0', lh_0); +ocp_model.set('constr_uh_0', uh_0); +ocp_model.set('constr_expr_h', model.constr_expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); -ocp_model.set('constr_expr_h_e', model.expr_h_e); +ocp_model.set('constr_expr_h_e', model.constr_expr_h_e); ocp_model.set('constr_lh_e', lh_e); ocp_model.set('constr_uh_e', uh_e); @@ -263,5 +275,4 @@ ' differ too much. Should be < tol = ' num2str(tol_check)]); end -clear all cd .. diff --git a/examples/acados_matlab_octave/generic_external_cost/external_cost_example_ocp.m b/examples/acados_matlab_octave/generic_external_cost/external_cost_example_ocp.m index 17655f8fc4..792459ee09 100644 --- a/examples/acados_matlab_octave/generic_external_cost/external_cost_example_ocp.m +++ b/examples/acados_matlab_octave/generic_external_cost/external_cost_example_ocp.m @@ -31,7 +31,8 @@ import casadi.* %% test of native matlab interface -clear all +clear all; clc; +check_acados_requirements() model_path = fullfile(pwd,'..','pendulum_on_cart_model'); addpath(model_path) @@ -71,11 +72,16 @@ ocp_model.set('sym_p', params); % cost +ocp_model.set('cost_type_0', 'ext_cost'); ocp_model.set('cost_type', 'ext_cost'); ocp_model.set('cost_type_e', 'ext_cost'); generic_or_casadi = 0; % 0=generic, 1=casadi, 2=mixed if (generic_or_casadi == 0) + % Generic initial cost + ocp_model.set('cost_ext_fun_type_0', 'generic'); + ocp_model.set('cost_source_ext_cost_0', 'generic_ext_cost.c'); + ocp_model.set('cost_function_ext_cost_0', 'ext_cost'); % Generic stage cost ocp_model.set('cost_ext_fun_type', 'generic'); ocp_model.set('cost_source_ext_cost', 'generic_ext_cost.c'); @@ -85,37 +91,46 @@ ocp_model.set('cost_source_ext_cost_e', 'generic_ext_cost.c'); ocp_model.set('cost_function_ext_cost_e', 'ext_costN'); elseif (generic_or_casadi == 1) + % Casadi initial cost + ocp_model.set('cost_ext_fun_type_0', 'casadi'); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); % Casadi stage cost ocp_model.set('cost_ext_fun_type', 'casadi'); - ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); % Casadi terminal cost ocp_model.set('cost_ext_fun_type_e', 'casadi'); - ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); elseif (generic_or_casadi == 2) + % Casadi initial cost + ocp_model.set('cost_ext_fun_type_0', 'casadi'); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); % Generic stage cost ocp_model.set('cost_ext_fun_type', 'generic'); ocp_model.set('cost_source_ext_cost', 'generic_ext_cost.c'); ocp_model.set('cost_function_ext_cost', 'ext_cost'); % Casadi terminal cost ocp_model.set('cost_ext_fun_type_e', 'casadi'); - ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h_0); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); ocp_model.set('constr_x0', x0); % ... see ocp_model.model_struct to see what other fields can be set @@ -127,6 +142,7 @@ ocp_opts.set('sim_method', sim_method); ocp_opts.set('qp_solver', qp_solver); ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); +ocp_opts.set('parameter_values', zeros(size(params))); % initialize to zero, change later % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver 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 23f0d7a5d8..7b40a7f34e 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; addpath('../pendulum_on_cart_model') @@ -114,35 +114,27 @@ end %% cost +ocp_model.set('cost_type_0', cost_type); ocp_model.set('cost_type', cost_type); ocp_model.set('cost_type_e', cost_type); if strcmp( cost_type, 'linear_ls' ) - ny = nu+nx; % number of outputs in lagrange term - % input-to-output matrix in lagrange term - Vu = zeros(ny, nu); - Vu(1:nu,:) = eye(nu); - % state-to-output matrix in lagrange term - Vx = zeros(ny, nx); - Vx(nu+1:end, :) = eye(nx); - W = diag([1e-2, 1e3, 1e3, 1e-2, 1e-2]); - - % terminal cost term - ny_e = nx; % number of outputs in terminal cost term - Vx_e = eye(ny_e, nx); - W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in mayer term - y_ref = zeros(ny, 1); % output reference in lagrange term - y_ref_e = zeros(ny_e, 1); % output reference in mayer term - - 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', y_ref); - ocp_model.set('cost_y_ref_e', y_ref_e); + ocp_model.set('cost_Vu_0', model.cost_Vu_0); + ocp_model.set('cost_Vx_0', model.cost_Vx_0); + ocp_model.set('cost_W_0', model.cost_W_0); + ocp_model.set('cost_y_ref_0', model.cost_y_ref_0); + + ocp_model.set('cost_Vu', model.cost_Vu); + ocp_model.set('cost_Vx', model.cost_Vx); + ocp_model.set('cost_W', model.cost_W); + ocp_model.set('cost_y_ref', model.cost_y_ref); + + ocp_model.set('cost_Vx_e', model.cost_Vx_e); + ocp_model.set('cost_W_e', model.cost_W_e); + ocp_model.set('cost_y_ref_e', model.cost_y_ref_e); else % external, auto - ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); - ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end %% constraints @@ -150,6 +142,9 @@ lbu = -80*ones(nu, 1); ubu = 80*ones(nu, 1); if constraint_formulation_nonlinear % formulate constraint via h + ocp_model.set('constr_expr_h_0', model.expr_h_0); + ocp_model.set('constr_lh_0', lbu); + ocp_model.set('constr_uh_0', ubu); ocp_model.set('constr_expr_h', model.expr_h); ocp_model.set('constr_lh', lbu); ocp_model.set('constr_uh', ubu); @@ -182,16 +177,16 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); elseif (strcmp(sim_method, 'irk') || strcmp(sim_method, 'irk_gnsf')) ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); elseif strcmp(sim_method, 'discrete') ocp_model.set('dyn_type', 'discrete'); % build explicit euler discrete integrator import casadi.* expl_ode_fun = Function([model_name,'_expl_ode_fun'], ... - {model.sym_x, model.sym_u}, {model.expr_f_expl}); + {model.sym_x, model.sym_u}, {model.dyn_expr_f_expl}); dyn_expr_phi = model.sym_x + T/N * expl_ode_fun(model.sym_x, model.sym_u); ocp_model.set('dyn_expr_phi', dyn_expr_phi) if ~all(time_steps == T/N) @@ -200,6 +195,7 @@ end end +% initial state x0 = [0; pi; 0; 0]; ocp_model.set('constr_x0', x0); @@ -237,6 +233,7 @@ %% create ocp solver ocp = acados_ocp(ocp_model, ocp_opts); +% state and input initial guess x_traj_init = zeros(nx, N+1); x_traj_init(2, :) = linspace(pi, 0, N+1); % initialize theta @@ -260,7 +257,7 @@ % set trajectory initialization ocp.set('init_x', x_traj_init); ocp.set('init_u', u_traj_init); - ocp.set('init_pi', zeros(nx, N)) + ocp.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints % solve ocp.solve(); @@ -268,7 +265,7 @@ utraj = ocp.get('u'); xtraj = ocp.get('x'); - %% evaluation + % evaluation status = ocp.get('status'); sqp_iter = ocp.get('sqp_iter'); time_tot(i) = ocp.get('time_tot'); @@ -296,60 +293,15 @@ %% get QP matrices: % See https://docs.acados.org/problem_formulation +% |----- dynamics -----|------ cost --------|---------------------------- constraints ------------------------| +fields = {'qp_A','qp_B','qp_b','qp_R','qp_Q','qp_r','qp_C','qp_D','qp_lg','qp_ug','qp_lbx','qp_ubx','qp_lbu','qp_ubu'}; % either stage wise -for stage = [0, N-1] - % Note loop over field doesnt work because stupid matlab diff between - % chars and strings - % dynamics - field = 'qp_A'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_B'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_b'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - - % cost - field = 'qp_R'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_Q'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_r'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - - % constraints - field = 'qp_C'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_D'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - - field = 'qp_lg'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_ug'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - - field = 'qp_lbx'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_ubx'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - - field = 'qp_lbu'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) - field = 'qp_ubu'; - disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) +for stage = [0,N-1] + for k = 1:length(fields) + field = fields{k}; + disp(strcat(field, " at stage ", num2str(stage), " = ")); + ocp.get(field, stage) + end end stage = N; 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 44c9b50923..6e3402d042 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 @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; model_path = fullfile(pwd,'..','pendulum_on_cart_model'); addpath(model_path) @@ -70,44 +70,51 @@ nx = model.nx; nu = model.nu; -%% model to create the solver +%% acados ocp model ocp_model = acados_ocp_model(); model_name = 'pendulum'; - -%% acados ocp model ocp_model.set('name', model_name); ocp_model.set('T', T); + % symbolics ocp_model.set('sym_x', model.sym_x); ocp_model.set('sym_u', model.sym_u); ocp_model.set('sym_xdot', model.sym_xdot); % nonlinear-least squares cost +ocp_model.set('cost_type_0', 'nonlinear_ls'); ocp_model.set('cost_type', 'nonlinear_ls'); ocp_model.set('cost_type_e', 'nonlinear_ls'); +ocp_model.set('cost_expr_y_0', model.cost_expr_y_0); +ocp_model.set('cost_W_0', model.cost_W_0); ocp_model.set('cost_expr_y', model.cost_expr_y); +ocp_model.set('cost_W', model.cost_W); ocp_model.set('cost_expr_y_e', model.cost_expr_y_e); +ocp_model.set('cost_W_e', model.cost_W_e); -W_x = diag([1e2, 1e2, 1e-2, 1e-2]); -W_u = 1e-3; -W = blkdiag(W_x, W_u); -ocp_model.set('cost_W', W); -ocp_model.set('cost_W_e', model.W_e); +% intiialize reference to zero, change later +ocp_model.set('cost_y_ref_0', zeros(size(model.cost_expr_y_0))); +ocp_model.set('cost_y_ref', zeros(size(model.cost_expr_y))); +ocp_model.set('cost_y_ref_e', zeros(size(model.cost_expr_y_e))); % dynamics ocp_model.set('dyn_type', 'explicit'); -ocp_model.set('dyn_expr_f', model.expr_f_expl); +ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h_0); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); + ocp_model.set('constr_x0', x0); -%% acados ocp set opts +%% acados ocp options ocp_opts = acados_ocp_opts(); ocp_opts.set('param_scheme_N', N); ocp_opts.set('shooting_nodes', shooting_nodes); @@ -127,18 +134,17 @@ x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); - %% plant: create acados integrator % acados sim model sim_model = acados_sim_model(); sim_model.set('name', [model_name '_plant']); -sim_model.set('T', h); +sim_model.set('T', h); % simulate one time step sim_model.set('sym_x', model.sym_x); sim_model.set('sym_u', model.sym_u); sim_model.set('sym_xdot', model.sym_xdot); sim_model.set('dyn_type', 'implicit'); -sim_model.set('dyn_expr_f', model.expr_f_impl); +sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); % acados sim opts sim_opts = acados_sim_opts(); @@ -148,9 +154,10 @@ sim = acados_sim(sim_model, sim_opts); -%% Simulation +%% simulation N_sim = 150; +% preallocate memory x_sim = zeros(nx, N_sim+1); u_sim = zeros(nu, N_sim); @@ -159,6 +166,8 @@ % time-variant reference: move the cart with constant velocity while % keeping the pendulum in upwards position v_mean = 1; + +yref_0 = zeros(nu, 1); yref = zeros(nx+nu, 1); yref_e = zeros(nx, 1); @@ -174,11 +183,11 @@ t = (i-1)*h; p_ref = (t + shooting_nodes)*v_mean; - for k=0:N-1 - yref(1) = p_ref(k+1); - ocp.set('cost_y_ref', yref, k); + for k=1:N-1 % intermediate stages + yref(1) = p_ref(k); + ocp.set('cost_y_ref', yref, k); % last argument is the stage end - yref_e(1) = p_ref(k+1); + yref_e(1) = p_ref(k+1); % terminal stage ocp.set('cost_y_ref_e', yref_e, N); % solve @@ -188,11 +197,11 @@ u0 = ocp.get('u', 0); status = ocp.get('status'); % 0 - success - % set initial state + % set initial state for the simulation sim.set('x', x0); sim.set('u', u0); - % solve + % simulate one step sim_status = sim.solve(); if sim_status ~= 0 disp(['acados integrator returned error status ', num2str(sim_status)]) @@ -204,7 +213,7 @@ end -%% Plots +%% plots ts = linspace(0, N_sim*h, N_sim+1); figure; hold on; States = {'p', 'theta', 'v', 'dtheta'}; 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 d599b59fe3..dcef1ff89d 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m @@ -30,73 +30,77 @@ % %% test of native matlab interface -if ~exist('simulink_opts') +% options needed for the Simulink example +if ~exist('simulink_opts','var') disp('using acados simulink default options') simulink_opts = get_acados_simulink_opts; end - model_path = fullfile(pwd,'..','pendulum_on_cart_model'); addpath(model_path) check_acados_requirements() -%% discretization -N = 20; -T = 1; % time horizon length -x0 = [0; pi; 0; 0]; +%% solver settings +N = 20; % number of discretization steps +T = 1; % [s] prediction horizon length +x0 = [0; pi; 0; 0]; % initial state nlp_solver = 'sqp'; % sqp, sqp_rti qp_solver = 'partial_condensing_hpipm'; % full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases, full_condensing_daqp -qp_solver_cond_N = 5; % for partial condensing +qp_solver_cond_N = 5; % condensing horizon (for partial condensing) % integrator type sim_method = 'erk'; % erk, irk, irk_gnsf %% model dynamics -model = pendulum_on_cart_model(); -nx = model.nx; -nu = model.nu; -ny = size(model.cost_expr_y, 1); % used in simulink example -ny_e = size(model.cost_expr_y_e, 1); +model = pendulum_on_cart_model(); % dynamics, cost, constraints +nx = model.nx; % state size +nu = model.nu; % input size -%% model to create the solver -ocp_model = acados_ocp_model(); -model_name = 'pendulum'; +% output size for different stages, used in simulink example +ny_0 = size(model.cost_expr_y_0,1); +ny = size(model.cost_expr_y, 1); +ny_e = size(model.cost_expr_y_e, 1); %% acados ocp model -ocp_model.set('name', model_name); -ocp_model.set('T', T); +ocp_model = acados_ocp_model(); +ocp_model.set('name', 'pendulum'); +ocp_model.set('T', T); % prediction horizon % symbolics ocp_model.set('sym_x', model.sym_x); ocp_model.set('sym_u', model.sym_u); ocp_model.set('sym_xdot', model.sym_xdot); -% 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); +% cost (separate for initial, intermediate and terminal stages) +ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0); +ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); +ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end -% constraints +% constraints (separate for initial, intermediate and terminal stages) ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h_0); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); -ocp_model.set('constr_x0', x0); +ocp_model.set('constr_x0', x0); % set the initial state % ... see ocp_model.model_struct to see what other fields can be set -%% acados ocp set opts +%% acados ocp options ocp_opts = acados_ocp_opts(); ocp_opts.set('param_scheme_N', N); ocp_opts.set('nlp_solver', nlp_solver); @@ -109,6 +113,7 @@ %% create ocp solver ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts); +% solver initial guess x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); @@ -117,9 +122,9 @@ ocp.set('constr_x0', x0); % set trajectory initialization -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); -ocp.set('init_pi', zeros(nx, N)) +ocp.set('init_x', x_traj_init); % states +ocp.set('init_u', u_traj_init); % inputs +ocp.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints % change values for specific shooting node using: % ocp.set('field', value, optional: stage_index) @@ -134,7 +139,7 @@ status = ocp.get('status'); % 0 - success ocp.print('stat') -%% Plots +%% plots ts = linspace(0, T, N+1); figure; hold on; States = {'p', 'theta', 'v', 'dtheta'}; 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 2fd60c9be3..c12024fce0 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_sim.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_sim.m @@ -30,7 +30,7 @@ % %% minimal example of acados integrator matlab interface -clear all +clear all; clc; addpath('../pendulum_on_cart_model') @@ -66,14 +66,14 @@ % explit integrator (erk) take explicit ODE expression if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % implicit integrators (irk irk_gnsf) take implicit ODE expression sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); end -%% acados sim opts +%% acados sim options sim_opts = acados_sim_opts(); sim_opts.set('compile_interface', compile_interface); diff --git a/examples/acados_matlab_octave/getting_started/simulink_example.m b/examples/acados_matlab_octave/getting_started/simulink_example.m index 99e32372c3..1ce273380d 100644 --- a/examples/acados_matlab_octave/getting_started/simulink_example.m +++ b/examples/acados_matlab_octave/getting_started/simulink_example.m @@ -1,6 +1,6 @@ %% Simulink example % -clear all; +clear all; clc; %% Run minimal example % @@ -26,5 +26,20 @@ open_system(fullfile(target_folder, 'simulink_model_closed_loop')) -%% -disp('Press play in Simulink!'); +%% Run the models +try + sim('simulink_model_integrator.slx'); + cd .. +catch + cd .. + error('Simulink integrator example failed') +end + +try + cd c_generated_code + sim('simulink_model_closed_loop.slx'); + cd .. +catch + cd .. + error('Simulink closed loop example failed') +end diff --git a/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m b/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m index bc2cd88256..04582be17d 100644 --- a/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m +++ b/examples/acados_matlab_octave/getting_started/simulink_example_advanced.m @@ -1,5 +1,5 @@ %% Simulink example -clear all; +clear all; clc; %% get available simulink_opts with default options simulink_opts = get_acados_simulink_opts; @@ -38,13 +38,19 @@ make_sfun; % ocp solver -%% Copy Simulink example blocks into c_generated_code +%% Copy Simulink example block into c_generated_code source_folder = fullfile(pwd, '..'); target_folder = pwd; copyfile( fullfile(source_folder, 'simulink_model_advanced_closed_loop.slx'), target_folder ); -%% Open Simulink example blocks +%% Open Simulink example block open_system(fullfile(target_folder, 'simulink_model_advanced_closed_loop')) -%% -disp('Press play in Simulink!'); +%% Run the Simulink model +try + sim('simulink_model_advanced_closed_loop.slx'); + cd .. +catch + cd .. + error('Simulink advanced closed loop example failed') +end diff --git a/examples/acados_matlab_octave/getting_started/simulink_model_advanced_closed_loop.slx b/examples/acados_matlab_octave/getting_started/simulink_model_advanced_closed_loop.slx index a4806d90f8ff614736d02c66edf409e4f17435d4..87bbb44e8d457d5c4240275aac7f07f1484c8887 100644 GIT binary patch literal 33390 zcmaHyQ*dul)8=EI*tTukc5-4TCpQ1Fb7I@JZQD4pZ5xw!=H`26s^((tUA6Dlue(7<3={|m<>!mGh@GvosjaiVs)xO)lPeO9tc+=BqmqFU^&O?)zgz~dr*3q4o{U~V6)EZb-=ZqtUK2npZ|;_*QHfV4abFAk+OU%}WxIA8V~mHYSzc1s zaq+T5-DjtacL5JV`EbNa5p3$K;Cd|T)hP|sDcY|hs6c2-NkUIUwv#)jbAg^SD<>D> zI?Yd~NBgo8mS4#Ny}x{0;C6WCPcZI+AxQE4aU1u30|+SxpRnY_2^9Z}Z*D9lKgkc@ ztRKFxKYaBaO|6|6=>PL9N|^W$SFz_;n2GmcDM6?_(oG;`H>hBQRB{ocA%<0Yn{Rv! zQ5k7$F;ffOskDL$hwcq2A^utJYBiw+&O9)+DxB)IfNr&!2!M1ZMAgT>}FGlGkh%*&G-$b_Y9$luvp;a z%JcyBP4Q|pdm$u05|k=vR2&Uw)6o>fJ2Lzp^Vjl4K8`4rm&)&JQr|)v;*2IWrqA?r9Ai^HH*fkq zT4{onQ*w%SP6v7g$~!O?69TCSv34893qrerezybU-$3vL+eGpn5K1GM*~YP znv&lKGG{ra)Bc3LBc%$C|73_Choj?o_y0<$avkJ$^B(}?AV5GkKM-t8oefP4oedd` z?Ho-N9qsH*9i1&qo&E#BJ6=hlpAj{9`xQ-QyQTTpAZr2XANTkmO7VT|OM2t>W*P0i zPe4nZ9O>W);OgJP!g>a6w;B>|Z~MjxS3Z&u@E?nL9doCLPTk-ZN_)K%RaEf-T(up= z+MNQ?WLa!j94qNsen^Z(`86iZ>A9`txs&!b7IkR-6!w9l*c@U|Uh)G7 znj2y?y`5o3T1bgS&0@z|+8*mU>_TJ_V3#9}I2z*pUMQi~kf0IL>d-XEgNgf&l>?oDQyUd0EEM$z zW&`Ys#1LC*dfPrk}l9?)p44sI40L{oRIwHJeV54)f8QyfXoAJPdbP(7N)qS zo0lnA!2G5qAov9O?<8F+_nV*oiHpiljPU+vd~DqQ-w2t;GTDMKA&R~81t0QT2~y>c zT5)p|i;eF9-eq090vON=Lr9OO2d$5WR^@2_Q-dn za8R{^a6yIJ$FkI;YA>9+x0LPV6-E|$n#_p~2I5*xT}#*DF@ZcIow+NR?rjy9GOs|> zm$Fwpiqm;ma1%Aksm7gIO#dM~-l1D;mfalXD_t3}x?z1};`*hu~ydn-vlNq{)# z_2ppn9izSR$*raBcaH=LQoz+bRAXM1Z1FFkgd_C|^1neKvtW6Y{{YzfS>*pe5Mu5> z1;@$4&h|e!s~gRS9KeVP4v>$NLV!m6$^=SUM)&l?>;{I2XOu$h>@q5uQn%ZtwR2-D zp!m&!t-H^ekLjOWjN_ue|9**_jLF5|Okq}#H_Tv<*(BGyL>?7f!3O(VAV4IC7{wKUs9;h&4&d<@6+%Um)QC|D| z{WO=L34J#`PET(5TzQIva-$sLtQ5DVsDF`MG}NUB_zX=y(z1uA%La3V&d--WmMu>1 zhr#W(y+-qEO=d>J#=@9pc9tEc^uxoSs8%y;@oJ`XHeA=K@tBXWG@ic?sAh>~PpZPg z{YfOaWUcTKDLP#ada?zh;{Fs$>2L~clHAL$SWIVlH@_0b2o$bj21Z6!4$jX-ODn6Z zH!qL|XGUJo!_Xi_bq7jy0Ws*7rgZjCen14%2#ceKdw20bMVUPUs%dE^{tBH;PELAV z-G0B1e$x@e>F5lJwrpWivPS;as*jYTZ@{Ld+&)8HdS)_kB^{KWgqfhGd}-EmL(i^8 z?4u=RZEQuu!~5i6i`83OPKKnZLqn)$x#QMviA;-v3l$^UcvYOKHVb9W)T3R=9WEda z&RLB^rWZj)#a?4wKriw&+}+vPp+<RWl$?;gS%ZXI{dspYt0bJkYh@mpa}LoYKs$G-%wqKOXl6)-$I7 z@FL%HAvisHC8pt77(P{v-&7Yb)l04S5gQur-Zd-~WDARgYil`TBOx*@$dae|>Bqr5 z%wXNt=yg0f-)H3zoS6y&u~1O>dwD951h-#tKxG)a@!087#`qMG0uucsgiOhzo_Cv7 z37K@8&Wey(Og`KpB_zmEmS(sI&nrV;MHsfv9!dzEawUwWV3>?_#~3nRux0zo$}2iL z%c~OzM14%DSys ^)?zSUPZ=W($aoGcg$Sauw64$F{XFy+o)u*?esp885ifqkGe01r|H{nfDNobBx;#ElxB(J^!C3`=q!!K`-Jw2?y~voH|M z$&1IGF2=lfuNBi@(vg{6obOK{_V&uc4!$d8x`AP# z{ADsoK<$IPi+`s4+UkJK_p|{=pI?~1{@p%>`o;4w24yv@w4xaVuRX57y)=_sYue1W zY-lWfH4usr=vOkLiagmxaZK0_}mnF%w6As+Qb zBrj^76>&J{>u|mQFR;pk+)K9g0O1}d?~c~)y3Pg=)+a}=(<&$}7984MP)S=;Df_F2 zhGG+5TFS7oRjR?)KK87cKhKTGjP~Uk3IpPfK|xNAX&7GqYPUyF?F30CZjHcJAo!V^ z;c@dZUu+IgebY3}ZRw=c|1U&s!`q2sO;U-p_Zm?Beu&dN!);p=ebr@W!FK4DTC5{a z?(Ep3pH#VUc?KP1KhJ4-X}{+7ncKy`Fffmv8lSKVXqg(}gnN1} zIXdv-$O!FhTgU3x?x^(y29oB|8bFo9w_^BLCRKcf8! z>D2!`2>{r9)C?aM7^3+wY)349;Jq8@!UmMMo)Ehq3d>} zuL?{6i>83`d$;?@6wDw2^G_J5e927G(z3k)|ILy7vLkHzcEZoRSyT4J!aU*99j_zK1DAs}BW_I~lR(E#**xxw|%z#k=WM7Olhn}USfzzR$Ji9OdLq<9Xs z(h-MpzFl2jM$d6T@v&B}>m}nizOXY)0fjDg_klx8qnt}nQSAKNQqkocGv#&|bxgoB zKAPfycXX=;@9^v(P|jdmUhW{ezL;I%=lsBjoz-dk^hpP(51Oe%jM3wwzWUm@zSew2 zjPDS!TR~Ox=Q4ux4v`SpVzPHYDs|R`_d}z%$avn+^c>Reh~d&ZY&N%BE(yO$v2bDp z|9ZX0I6M0}ox^gjGTbktI@%P#b<38c-$9@rs|R!#xug%D3wcTRUhnlss58_5dB9nT zH2uonMO1WL5?NcRiGp^*TAs(htm>e{pPHH}DJfj^&D>^R7%SoB+OPoQ6!d*{T%cEn zz*&xcq_UfO%82ell(w}9J8G7Q+B-l)Lc$I+4Kez+(66K0ybc#r^R0zNFaA4X#?sH~ zmmtK7D10OIcNEU8p7c%1z@^ytxO7~YKY-mmEGP`+o|aCfu||-eK55;qlQ*_T*V4Oe z-s4tG4__l0LgH~(xq7xiZlSbara!t8zk^07*Oz==W>EqMku0~oTzY(FCPtWzgF`_M z2Mfyu2%URNIC<`b`;j6BZ=&C_r9`1-EOc&C{JKutQFwlpRje$G0c5@@BhKe7oCDD80%SNLzSOX+M ze>(6rKoyDndC3!SbeiZzz~*!vakS`(hZRmlL}ZBZ&T{M%ps4Wmk$`%KkHIA9#pk=` z*&=n_n~59El)E0BLjcXnNtv9S>@zj9zl0h4^7f`2IyNxrFSSQy=D*-Kb=5#}PyxXV zs>H`z$6Qw4Bbf9XHt0dOFh71b+$Fwh09in*O-UWnPz)mo6YFNH(<3e_N^aCC#mdb) zzECujvJ3LNz!R~*>Teyda5T${%EE#eI^QJY#l4^UI|U$N6c0#_&>QH(H?TR7G$A=* zfQz52jB4plJuxB0{OI_b%kn9hoQ-X1IR(rxdj=sgGBvkde`I*Ck8d6@TidTuaIJ*0 zmrp8`)aCOGFECV6X=x%ll4namiOKkMen1LZEil|<^{?2y&XlKweD^-z=)LEo#HhQ| zAP6huWzWc%#nFidZ=v9*Jzh#Uoyp(1Wdjz@vO42CH)O{;FzmDzMTPcHX+lC0*;nJR zm4r!SCa-;a_BtnTzFi|f-NdGj+j9>ixy|cwzLS;tarHTwIWbriHLL-2GX~&Ra_||8 zqovvMAsjsWT_JBy)6M0M?a|gOpqiDToAj)mcr}TXJdz`NB-G$qxAZH*1>D1t&Zab@~Fjm06lFGU< z8ZvTzaq$x_x@n-glYv%vR%S#z5H&K?6+a-cgV~E&t-Mu~tHFU#A0sXU%5TFw^e~Wj z1rx+(jFP$>&|x2%?$j1GI^NHRi#x2L@&%#tWypZj_wr)77iIZ`qoV)W5!@#Es?2o5 zLC_l`^AvG88`gQo62iMdP|4%X##;FKbh$nPyIJZx>n9_58QMxo zPv1W@B&Ntgr`QzvlA5@|y;A!5c^bBTevb6`7*DgWrW#u$jK8iSrlS#i5W>1VJ1a=R z0fJ`mPth8*5Y=9E`bmQERb^E1eGdwUb@G>|EOc=8+Ta=U- z#?Hafp<;R?D9?@AJ^(J1fRmGztrOExGmaDm50~WP+6`?jCN3A1U3zP>yGsO7!RmsD zhIUxX<%H1-j00JcR}pa_WiAYQk~sSEf~Xkmi5AVtXpCyCTr)JqY9C!C;L{1_kLzj$ zLJ)uU3BX1z)An#s#zO7cA;jS)KQXbeIQ>_n|DKcr>wgx^(HB#wtfo?4VNvD|XA%{) zzzxwCyu!KQFPACJ8TD$R$$XL&gvi};?&y=|aBf8|G*|5qir!upiKBHM;AaKs0VZvz zZzy_r_>;i%TlmJ&Bo5$GeQdNE3aGC zrBM+NehIGuu($ODB70?moubHnt>~6j!3_~V5)(|(zXF*z*y}S{3aQ!wj@Ctrd-(JU zj2RTYJ-cx_hi~{aW@hr8vX>ho@Im#~w+H*xPxpeuomC_fTQb)%S%0iw3r6+ozfyv- zWxSp*obP1CB|?kmU6KlRZUQ~oVbCxzAcxz52hWFF*9(!9!)4lnpU&9;DzVbz08Ypp z0VZjk$8XDRVsz6*x3mXl%~Gp@Q!s?SZ-x{BDDI^2kA(4}RrL-g6_w#7-)`i@n&jwi zdCA|JE|c66CtkK-gKh;lZiLVS9F%H}TJ5}!9EjdzEg?sQjm3E){P^$Gn+4eIjvstv7p0b*~N7{ZKNb zt%4$i2wyOuwR~3oR$q&-7`6chzkwe+^#H1jSHp0qbO^;bN^iFyaj3@*f+U~?WvAy( z5rHl()XkBy@@6NpsCWmQ=|Jva{hwc-@8^YuHwXm3TmDgCVRp8RiDqJa0QLM48mZW{ ze7i*L5=c(4)>SbQ3{zb@5m@tYk2pXq8k zx`0fh+1qQeTey;QM`s@w;}&|=b^E^Oyy4qNVeBG6GcFP+$GN0^EcjSKH4eF(XZ`id z9cI3{!&DMGGD5NR=Mqb}q{Y4~#JHVQX<+%*_qU}hAq<3Tjm?$x~qDyRxz}W^D+3 z$;iy_)s;wf3F+9&Ol%3;E8-Fl$`~^e#kiALL`MjF?z7G$k4IA( z&z?MaRWX%##x+M)^llRcTK%WkJrYjbjZP__)TLSj9}D#%)~AvKx`n1DsrhTM^=}<9 z!~{YF5TJ?U(;0&ws(SL?*ZT_1uHrG*x&P9QW`Q>_1bflZ(DDZQ(}hT?CLK2pbF(vc ziJal*sah10EPy$L;B@wUJQ=qqqjmwVdm&K~uz3=>HMzM?@<6ix@X(TyQ;tXVDJT+9 z5m3mfA-moEu__2sJsQB=AVC&YDLXL&pQKd>dHtCxj-C|SxS#R9BvFf~D_4t{4r6dU zTiaHr0TuJ(#UG9{H4xvUshAruT{ly55{mRKGe7M(oy-nC0F*$AAAwER{ij%sm+6VZNrlnS0OC7yq(enj-=fhvz zu`WW7&tUSL!;Vp3%J_xj#UDRFvaqNymoT7LYZINL!O<}m!C(LN(Lm8sJY0UTExYCF zWoY$OZxd{0#0RP#HZsKsSQ2$`sRe@*tUNs(IF>;+ARrM32ZvJNW*t5Tg}9e-*TLT2 zSp=8^D_V{z=F+pHJppJoTxTsuF`lB6e#a)B z2mPW*@*B76pa18VYvB0axMLeZC3O5#EUY6wK5=g16c5Vup;ZXtLO3G`k{^_C!CX87 z0l%zUaDkKsECH7P(t!@R)kJd9|kMxDpVKS^|OWo~ZUTlA);?l?H=N3oG{b|&~} z19|_BM~-&e_nGDqqst6cY*T8A`1yIq%i&Y@=ophVax;^M-#U=`XxsMyLPze^W5$jm zdvyvs?1Ttr(USm%SSCe&MD!;NJln8`oMxIa%0!!`aze@|`w^PSF*?iT70OFyZIhZ` zhQ^E0&md!$0+iNeZwlg3rTf`}pqS*jcG#6zLDfF3FX02i94Lv;*KfzaVFQDIr3XV8 zLP5_=K%TwPYe5~-+z+M;D(8O?CV=IrVVv#9Jw9v~>%YuQ%O9D@_BpE{wz&qAtOvcb z&c%?{&9U9u_*q(Cbi2eUx43{};;ANn{6*nmar#H_jLM6PThLVZmy}>r+N9$FgLJqh zhX4)l=^IlG&xMDJJ3iq2>+3W7{thTEGgD_p=V}gI^aFnBH5-~zbuuvXfqxAgt#s#% zx>QjJfA!G039wuzOztgsyTzx4@BHY@l9YF`KN9H^YI6R_Be6Mypj!<4NUeukr54vq z?CD~ONA~OJd~awI;pOeILalXw^;!@@O|gRT6Frj#9K0IH@Uc-e z9~SJLFT>Nk;$Wq$uJtDNN`{8@>db6_c*%esr;1B!fyD?jn^^2(g<6u}sW2e!RH z2chZl$~wSJusHt)G>9DVhyBI6)qbjjFQh{$MD)Vc2?}kI0nZ^+);i-fi^q+B2)x?= zE(DFW{COa^hUll0|K!ge&@Jz5-LGRhy#<1@t?tL{Qj`Mtyw4E4yN9H}!^4Liwb}0q zyTyeEfI=`Wo~@+n@kZNQX-?gmy{qnZ#8=min!{>4;i+>-b&ibVS+n!VfYXLX7Ma`^ zm}IZ(l{DulIKw8t6d?*JppwtjH&??h(sciO791k#)^mbLd)n*}l0M){Z*$Du5t7{# zZ>mG5&7n6VXK}L6xX;tTEX7(d|@A$*EkX{PlPLG!8Xq?@=V(R{bdL3egSEod%mFlCE2wZ3cXL zQidibV86*c@~?rRzv&+01y(W*W*T2#Ut{M$V)QEd(}57VJP)5R$;imUILi6>8il2i z3o*gLhx$#j!04IixVM_`a8q}yUaONo>ce+HKB>)^n7C8f`JtsI`j&YiVEExpZqa}u zOqhSaeV5@s2 z3Ey(3%bZqp9kn-Ph)XYEV9dFQf)#oE<&ePZXKEb@a=x%umCl+zlLRALWOG2wF*B~R zCr^E{i(j|u7!c($dl#-{n+JpAOI@!f+u?c*uzX=gEDu};PUup&c60NKndPYpj?_Kv zns9cdmr|n5=jrksGjRDgInqG5FymG3}F!I(y`$mF`{^$M?wkDi3rN_ObsE05=ejN^1JT&rmn{;6+iUH#Lx zlT%o34G8OS3i_%V+ee-cn#<>*BMGU#0l)aDfa~oEa*X3O-ioUMxhk=x^dM<(Lg#vHMc~)(#+m*lD z@>b;}v>4B@zWgKM#mbodx(nL8B$%Dp$$O|3Ph8P8f!K>;hLnR>@SuIF0}bGpc(z|n ze_8d37|glO435Fk+aWv#_d)I$)LO+Z*I>Y=@V$a8x`C`VXzZG0Ic!kW=2u@a298$8 zzH;;O%AD^gEiVpHGDbAy(ZdRIftei(LxAw?nRsse*S~u1?`v!o<^OJ%xv$2fUlM2@ ztjbdV8rL=Bal_>OS-Au`Eu6C@Gyu0u5 z1lFl`bR7576#1|!qpp`#RKEHdJsBl2shNUK?BTC#GTUNtzk(Wm7RIqb53c8WQ8qL* z#Ia~{)nPUFx2we2YZ>nE4G4ilT&L)FZ*0^ina3ElI|OD%MO|jqq5v-E(kW?pcvv<7 zc9Y?vTuKK^O^{eu_o>%uH%!nDb$(~p4GDp|0O;wpd{Oi79U#!)@Nf%(=l%IlQKY{O z1??=NjCoHB`rc?rOa?KYN0XOkDFVMi6_tNa$&hcv)0Kf(VyZ$zl=t-bmJa?MUl}aq ziahr=?HtJ6T>hJvao1qp-(+5r-MFW><{5+)%TZwJ3e#(TA$?t^WDqg-p#}@N0DLmn z*EfHk6+SvAZ}@EjLhHcDvFiucDr#gP5ept>1Nu6s-2fMlmU{%E*9AnEktH{Yb~F|G zO^PEMnUh0M+b!m^y5zLLzGKpMzoS4h20{Ai90LM6#EYWNmmYbTaGf?YjAZZHx$AZ# zug4z=4PpnY5YZPwC%fn_-h@f3Z3@dXg@nAYJse{RczwDQ(?UAtY2;7_(6$RuEBl`h zLwG&i-eOTV7}r*%7Ik8K+nKgv&Je_aJ!>Ej!4v;McjYxg>2Oq5J~Kbogjs&D-JA@T@BmW;HqD-xei|JCErD2l+$Qy%jhd#3AB^;!6?P$*ZK$o;~IXu z-S2+L3 z-=xKUx~8sF+reWw;`siA`ON!n7Fpc;Jji6w6b^wFjNxGvhF1vQ#Fk^7bSr5q@b zJs?Z7`pM~o`?9dMysT)+@^(BDRqbf^CQ|x@e-k@m-*$DFRr%q?WpF~Aq14AzLSI+* zMgZI{!S$I8J3X2VjUtFq%1Fr@R(67LzZbo{+07pXw^#K5bz9@>!y!&i9^#MFw*Mjx z6>M)3H_-U8-eTuFHMH){v!SUDxas}gyMLh1EuZzwUCQgXD;Tsli_F28|xer8|nVg3IeyP+oi3HsNxfLrJ z2g9yWSlm!jO?(CH5J9ksi%Xwz=p8119ZrbVqK(!u)uTa=j{N!nMF&~^Ymc5MLi;So zluiByold+CrMnJ;ORFQStY0v}L8rPfLpX7i)>b)aoR2@kL zTy5j=3XEciqH3tB>KyL1ucQLg|5^nLdeg@Mi{hz{Cw%ZS8%|6|#K3_J4NuVsb~St{ z0YzaR?FTBZW72B?PKcW_Kv-QPD2r!&q)w}Xe6IX8*?l3+)NL?yak%3PkgPbyS z`?fqbh*xYPq#};5_N8*{FPd_^RaxG2NhE=E8^EqV6WuXAi>C?77A1XgfHAXQT2aAK z<`RH6AT%{OC6!>b@V6a#+Ko(Qss#Hl*4eRDv4+NiUWcpANduO@7pKjvvyA9cN_+Z+ z+KlkqZa9BJG~<0&+ZvnZ>dj34W{QXKG#paI!NDP0@IpAd8q+a^?CEueAi=m{;=5ItiwL%?FJP~ z!x02LQu>g`Jszhm3ZB5l7HbQntmT}Fr9V2VcH<0coGX7E64K(7;$2-Q@jF&bxl!le zedaGE^hRVLWy0iv0nw}w2FAvL(+|Hh<}H?(>Y5JySEslRzsmm*P}Sux2p#&tGvq|g z;VqDp;MnRUB;IUHo##5)B`O1ffBp=q2USR3datzX6jW2~)ED3QOifL;D?L7mAcszG zJA-~HTsW7UyrqcI^tHYZcjNEi+0G*ptutk0WE|bM5n^Hr{VLGw0kj}C=~(??(K`kC z=k2x$g(CUu{-5Mq>NCqSHF_`O+JIThS7vC?^yIIYP34W`w`rp0OcOGUm@B09BpRZS zmS~kED~&!MzrPQj@Y`aRzmg(f?Axfc8`}4$Vu`BK&9#qW9|gO6Vi1~3#dO;S5ioue z!U5|k3yy)hUfMM>-gsDR`x(R?gqyZ`Ho5q0dV5$vvx#=9^Vu&=#pc=2!Ty0>Ek8p{ zVSM*Na`^q=@yA){KHY>l2b%t@_(V9_{xWFXjX=;-PsD?YDsnABSJy;_UE2#snm$o4 z5+ZU*=1>+30|S`STel4-n_1Ld|8Zzg!2bGrDJvKb+x@8Jm0dYx2UXn;g2u?dmh3R> z%XT_ghJ7QcygB60@yACz$@2-ox^Gr`ePUTXNn_us2!UvLn&e+Y=!71XyHh=Pd0fILKb(2|J17chM5pxOB>2^@n zQA+jKFIR2zbb=+{Knv*7iUcU2`Ka#DgwjaJ5NJg@(Hx8eXnua5u9YoJM;#*&lMNfq zdIW6L2KQ9Nd3~m23bYlz)#DrNuIJ`8rrIlE1@7#Ega!A*&(6^5>T0Ewfo*Y#Z-}!$ ztEsjQm`zj(#6x;mbR_Xbqxz5qs+#Xl6Brl z=p^1=00d8C*0*X+O-+|TljDDc7u|NlFHCo|IQk!_gI^k568T`G@aQ>*#Dzd%kzu_= z7N||2);KPh!T>3;aKc3w09sobd0romwM!@rqQ#bqh_J%72@q#%VJ}6#4m0Uh`nDR? zv__&`NcOkn<~56KIsI}2hlD#JlrSY$q{TTntSr)9`$w1fe8B%HJygOOrg4wj#!tV2 zC>geUO2TIe$lBSK`A~Wt8(X$OhVQrwX{7IN_fg$4v{9~3-|=75Ga>#W%#RF%Q$ISr z^Hr)K_XU-g#S!Ux1C3*r4s0$V*3Pv=ZygUCbyzOHW}duKmLzq1@gr$*E2^K@SB9r% z1K$Qk;@`jD$@4f)Ld2+`@7D~SAw|uKH%tS#~|8jP=0su&IC8T49 za>yN%G6te!tc;isXv#UWhJqYmVmA%=jjio9BO!FR0Q(v+qD3ZF9q42pHM$}j!Z}4U zyq8?`FzJjQeBp1`#L{@n0YqJ(-pL?>V!hS>=-RO&+VRbvn3-6N$H5T!739r;#6mOA zyE|t1pRD)z!PI@E)a( z(~jLAe063X!6ilpF`vyS=kQ*3$ZR+?eI7&I3U!%D`|*Kth%!C9%3)Z;3VE2o6(&I=H%35Qh%s%Kb=zg}R67 z*ZA7GeY>@bZgdsfcY8w-Jx?6P*Bf2yH@L9ZGX|Sf;FJ>FN*bC!)(T=^R-yr!ntV|c#-R@le>(hw0>0B9pkusVfH8AFI9vhwC-Ml z=Y2Oh&mllp)%s^$x1jQ(;oVLRCXrV>#iVj>Pb>oA){Lpg4hJH909x7EsxRQ4n1p0* zclS@xzo+0jC{c%I;bT&lla zz-5?^zMgVTeHCmj*?N5QCd|}k$ExzE%E{*dg*CC%pAU{wX>tO5_v3-dgi3YdA8W{! zxF6`$;wFkeHcykBNyXY-`o{GgKZWq66cVtC$5k zgxobdIPPKO<)uwkAY9k*=J@;Bl69@*1<>$vez+)J=#muMILgmW$8)@ZxQ8o`H9Klj zP_WG8URx_PFe$^v#VK%gDI+As1`WfG%bsauH4D4jk7H2S^zXr2wt5QaDrtHmB_}#A zfj|-4Jm?z_;}Xj@Ehmj2c`r|*pJ)Y2R4$|noM7jdWe)^*mXVRE(eLru(0*MRKAd_U zS0m$3ucgvU(T#BAMO|e<4*ttblK)Z##XSc^_+e3)22?B!fc;woy zVj8Ttcjmo6B?!>WYm%}Z`ykRvmTSRUbdS&EUG3U zA;fcfimN9^1Eks0FOjjXmM|+eI?Agv_ zKXo~#ahDhXadjmOq$b+k-L2z!xOvnJ`PqeWSHZ7OJlN&k8vE7mjcx)ouUO*pLI+Tv z^@KWIkyY*98wYr`>PXTZIox0Ju_OIMlf)4YiexumM>^se7AMhY6>`QK6E+rZgDj-$`na}s`J~~xmQD?e!k)&28G4U`t69`CMkx-To!>r zyOWgib~u6I(Sj9y0)STMwccGG_rAW>&;7trto;y_^P7MQKRHkd0El9;T@5E(>CkO; zfBD2Z(e9+3P0Nb`!^DXL)n4OEd{hzLC?km8&quDYtGw4xz{Z`b}%76{D$J#*011w__;iP7Db#{iF+Au zTL%hL)5Xte*(v!ERm&4wt%GRG$w|)|GgR}93c^2GPe9`d5bIjEkazu!lbPE4poYWFa6? zaBCMxE%*XTI0oE_F@Ip9Ju!aq0+*ZzS^VB|?{MXmQu$KzJOj(>gy*V(KlFZtWD&O$JRQLNMUf3@#G!*kCo7X2GG2%mHf0?#jAmSAT5-Nt7Bw*&!UW%C8+^FDHlt;N=j*RH%UlY9G2j)2C$?3_#e zZh6mM0!sZlew|227k$qI_|_$K7ctdqV$EI>%Vd!l@8mp<>tbS^Y<>m%4?_k+Ovq@bFXfO0{o@|CpxcL?{lTQ@IS8`ToGrNnH z)|%DLCJ71mG>wQ!K@odYSmFDPKik(@3V?uN=gk<%aSUi)ba)87tY`xA6fj_gh+Fpl z4Bf-d4uXk&JHnQxx!vkO!b~2r=}L&V5+o4K)ZBICl;@Ye_jr}NPGy}xBpqh=-Jss2 z=OOF`BDI)%&HgoB$#=w2NNX8R{k1CJqt2VByYcDwGr&xGmD*9a|Ix)7dw$0fbzMAf z>y7|PAe;(UCmGQ_NZ+iniH6esLaQUx{%dUPiLIqY>*ZxKdddj&>2eIN_M{U&P{u#_a+T)-&20C4EGIxM5k_fSt7nQ!Aj(-1+^TP#nHI}BOuh4j@Xmy{ zEh^ad|COpq3Q*r5&*vYx(h6aXEBL;zRnz?(@)fWKm&`jrm%40`?L-g2_sBv@&|!|V z{PXN=lD1yx{pir_2P(la$PDHGT$c#kGDn?4d4q*^bb8k(F)gnZ0q`C-%pPPu;ke>{72uPTZArt;1+U zv@AC@B?n zFRhuW0q$;; zL3qdq%mi($%d`u3?s%rBKQk)}4rkWT+|2B%{`h2C^)CWYF4(hsMR_0=G1#@UHl_i_ zFT`jadL~nQCTs4tAp8geEMb5J%~iRs6pf8c5--oz8FRaQ7qK3!=n0@bNp=J!QoO*T zL`{VrWe=jgoHH$cj?s=-n{AN&P$15@0uv+_^!{3sWFVGBxC*2zzknODf)jZz9dZc9>~9 z8KZ#XB>CA0jMThEuT)9%&kPPb!hlEX%qc_Xd{9)fLbBx78T@1+*R=HXV!z;QTYsj` zUr>zMpZJ?~8Q7SJJ4zngNFwK=kr|=KIKT6w$`H58B*EW8l6H-})wm6PNAI3ROnO zoAL#yj$lo1t615;dF?@7+kn|IV1wDJ4eXquE$mwK3HhO0t*z{ccwgSwEC_)+hc}45 z9DoV0qT}Nu&|4}it#eqP(TZ;)fLREgwt10~lZNIEWj&OfWfpH{GK#E-pC(D9F z$2}C^NlC<|Z;qiB;$E^QElT-{H6ScvUWSYFQZvILINE0maUDTHgI|3@Vn{6abL%TB zr$4_@fCWpAjnyf8?X}|{SxS4W+PQn-S={2j-6i6l8fh5ZTv?S6QoKKM7TH#)JZ#r- z4cNFTlb)WqUuALJa50h;wyn0aPgQ7KJ(I2i2qOcmIZ?>lxs~@2%rGWzd^}=?WU@Z| z*cUJ$f)y(NkdO!xu(7kx(=jE=nWT()5L~v?qVlB2>xH4ZXy)XKq%S>#tCD?Hs!vj& z3Ba`r$w3V}Oz>(qBZ#354C*X#pJOsOSjaa&E%h9uB;N97^LK^tnu9>G2-P{63r@_w zCr$mKf*3z`7XfB^fdE(RCrGcS!Uxr}&AgD42jzxj*P#u8l_HJXUM8a z%Z?8aUS;?)apL@b<9|;sh_F6=SNcLW>^(aYZ3!Q;>pM~RG{r@@xs{BJH72O_jwN-h zuV*NW8@W4?z;f=rnsGm84Afy|VNp2@v%Nfn>71e8-&4+ixLV#S2$V}1&9Eu{XyMS- z)WJHVedn?lBbEYcyn*U_CaU5oFE2l}4p~r9X$*ynM98BAu&99$MZ(3L8-9?oLAc5X zA!lZQ{8;ss4zI8g772KF?$=r<<`R?eGCfH+pkP$D{f}%N%A%h>rp-4o^!BsD^Tn12 za;6gCWU3ep$(kY7deA;+5^!Ep>{iG50qVfm@vnaswx;5-O^<~<8XG&h0z zd3=*NtCP2*3X7AQSZ_spuC=Sv9l)aoj-FnA@bFvCXvvoYVBqkla&DuS4eaVdvP06h z&pTf;<j1>38UyBjN6G-WT>kR8mP17Gq*y zwY*M&<(@tYlR##<8bd&Pc~;wLJAty%CNs;3`?nq7nc=rvpjG2`&#s)c2F zE4;F|ok_^es1};CW7tVi2D~Wzr)xS!eRMl?D4nm4;1%)MSee=Ncp1x{$rpuW2C6j z>r(FWMC!hxr153pE z%mCjRN|>+6#QTQZywOi68(Xrqj1hYz^~V9`B-3v;W>@FcGPO3IdpJvuyvcZKsZ-bBbj!*&|(toD&OPUpJL^y zOWzSY^o=@je+qcm?*7S4^nJxubIoRn3);G*BPB&;pU;>=sO6z2+GRMAoMjn3DdQThRlMND8x?{rI}hlaip*8_g7c?` zitCd~s~=@I`3!;J3D&>^-j`iw@0TD(V~O-|^GpEwtdd4feF{i_1`^ER!nfTXV%&^DZQ0)}{Ou*U`FB&rN;Zzbxd%QuI(ZbgV(K%UBt7sIaC4%A;zak=Na}@EAYQz&lxsn+g=vz;Lic;h+C44Rumlg24Ls|LW@-fMe^r zZDZra#))m)&WUZ?wsm6Lwr$(CZ994S-+gue|9tG>8bS6z8NQ~{kUm&RwQu< zpUZU94zNJ$67cqJxvcSqL$!GyB4bgl^R9*CATrAhoW*e!mfjOn8#1w+ogrfFwvon) z7@@)QtEz&h6I*&Vw2X`_1%%vmfvIj>~3owx4^)_f~~Xx+zZ+7+A7GQDJe&^}9}Q6cjP%y&Q^C(Ed2@FG$ALchU`2tpEt31Kuot`vo09E*FCXxu zVnDBurmja*aS%At*VeE=dC5R%(=k9n%H?06wWY>zZ=izdsQFs$U>Is3thR1DZp1c* zU3+;$$(ukOWcddOA{-+kG1|nkpv~Fl_wxH)TnjwSQU=bm0JQmebR_P1ICCfoAqAhS z@ss-oYHK^k1JIQe9~<8%z|uljZ4qq7C@DE=uir>zwE(7T$zQAJR{qXf(w9xh zOmT`To7*=7G%79@0l<|}=iz49lJiZ7xNv!3>$-UsV&d49c6^|$F+FvTZ$#pn92X0h zdYn~9iOASmhI3<6P>td))3cA$vzZVl?&On{9QlMNL{G0DW~_|mLNscf&M-ELeO6-f zYQcJhl%Qg1l>%t@U;Z+8NnC$ZZ#M8llmoj0Qe3xo`{Iv2 zB?TPzwYnaFvIl(bd*O4kwkYd+k_*!d2<;#Arc#2hH%_K#Xk<~me`$`hW;5kC!gwYg zzaL9>ft9D>n{Z!}n=C^b|8p6wo~o~?DEC=>zp{^*yV9F~qcbU}cYv&7Vh8n=WhOhA=;ca)g!W^De_OjBG=wrjXxcqibBr2&pIWg6z@YP?vISuRi zewwj5sw7?h!ddn8?!2li$XyKS2w#cK=Oki#aWXt`&?{y?&@*RYVeu5`OI21msQQDw z`S*^2720B^+1sgPf+8E^#5K1cTQvTWMAClrcX;t7{V9}-ISEu%wCL5y z22BtUAHj{bwh6*;(1&ONW~pax>DoI-lS`+OoHj47ubD0?xXjHB*z^tGFSFn*DgJ(X zclrGJ1(}JBPTOm)i-NF`@U7}WNO18Z&au`EE%4I1X^RAy5y;jNan?WUV2jIa>F^K} zXD%`tO|c(ZqRTVv0zV+S5R0?(_z(S5|4ef;I%e2nZf;(*)104PJmM?Z$U8mdWRs!g zP@uzLerpRC2QB$BTC@(i(r6|Czo6`SYAWhaWJtInbQup`JkLy%Ysw?dPxh}_#>C>h zuX)-gXx!gbm6rN^96jq}RFcs04ONa0d@AYbPd@Rt@7 zxk}G^J23R&B;QsP1@QO7%-mevSqUM3%QEZRR(f@0B&QCfYxUh-*-I8Kms}Wk2nD11 zoJ80i!`*HLCM|$}%FbZrbG|$=8>HcloY&if*^ai5?5`ev2$Ja^MS^s0;ke}V^pU=O zZxKQkiowaOz^en{_lxPwL;j|A2EagbR8-XJ-v;J7;uH(3Hnb&91Dmp9($Xk^i-=_M zF=q|!JZi44t|w^P1BDRD-9Hr{d2_fk$s(GgQL!*3TiKYj($Z25=E~2*m#B!?ScJEH zTOu5k@tHe>hkd=zm4H|(sCCLgt(K|W{BmGjtULn)a3frO)sg}I#e`&w_pFNn8fx0) z78|Kbqs)5rKv3FS_S+J`iB^~6slFq7(K?~cLu_vhi}MZuM6fq&o^^)J?QWlNvzXT) z+LGc5cr;2rFAv{1&zGBBQ7SHdXox$rcIqGUVc7Yythn|rlMJl1G|__%4*-tFhwCu- zzPI`UXh%fGXk%3y4c-JPynuP6dvx;O@`T8vfFzE2&-k1S$WTD{Jpyhoei5+uo3e9^ zbM7ovVb%No#NOW*T%a&0b$`m5B~J$98ZSe;D%W6 z;?{V@t8LPWxwgo3$DiUPOz-`QWqY5qOBbbE%-9SibLhvId_`__TDb)|b*(^Xm~oaf z1#Up{nGwH*lIZX>-ICuR_0OP;vEd+2`dMitGsbzz;7v|VdE+)sjgHULu}Q0V>xaG?xE265ZdXc;1Byt?UDym!FG@Q|jbn+6^%rBv!9wGsPRS3P0 z-1b~K^%o~}xKxouofSMDErf*XN}Wt$_6#>B)N)m`oj^kG-mw)JXMC$46cb!yaw z)LNJ@5SD&=O10ZF5Yp z?V$mD^nJTJ#*^x>!v2lOXGjV$&l=EGj43o$I=U3G?B-fK-LwlzGVeK*d%w7Q1<1pU z^x@jN^Rtm*nlMK05+i!ucQ+@S z3LFbHJ<%!^sO*z2ZOGKz*;7(aY6RI4poFa)ia-6G1=saz?`Y?{)8wDDd$-N>cSJ4V zc&Dd_{1iLNL33z+O1|=LZOqHNnNMRTt1d|RC*k9DB>4Q}2b(e*F(AJaHh5{*m5RTt zVH*5%7fLE$Q8XUtfL(Zkd?Yd#2s)DpQU0U?gTA$9S)iy;X?kbsWw}&g7GEzdWiOoa zN6N;<7n|^oY{0g8D-RJN`F2_z1FNyf8z*Pvd;|(9>?k-W#1H-?vSGOxR##UErdfDZ zp@*_tvwV6}2~IiDo(Bj+QeqLfy8onF<(;ps6!KnYS*rxdyJDY<^18VF$l+*_V02dD z^0t+k>BWMYQBhN0ejFz&4s-IPvf9MQLvMF=y4tsBqGMxsoCa^L(6hd~O|dsw_6f>0 zPq3t+5Dookyi^`;ajH__CI5m!Htx2PSZuMH+zKqt|5X_}ZO4ub*7JJXz+FHo;|cEZ zZcRtkx)6Y-rrR-ENW7U;1NaZ2mS#P*GSI6d!q+-$%q^jp$mw+RCmn z?y{zg&kL~ghE|-Pmj@Ej)zvUL#2;1=Eg2UR1JiWBblB-F4W+hBbNe*9raD;kekn+o zZRvuBh9(m-p9>%(rP&AtJ2ambmI>@SK7Ha%tgVmd8vPL$Q@lZYBBWJ1iuP03rgvrT z(HA8pmL8GSrOF8#HhR)dT(^tD8=sYxheOvub3AR2e3r2~5z!4xiAdzMwYBSH_7Ebx zFjZGR&dlr4`Tg;NGao(BQ4)R0-qt||ESQoG2jG25 z!FXaO#oPU~SVp)*o&>jth;y9C0THV^VMo-q$<02F0*&-oSQpSb&RL}Ie*VqXI|v5@I`vzp%gz9_2C~C_5DH$`^~$T``gl6K zZJj2Qdb8W@{z!Xt`Z;fo@M=0+=lT55pOOmNzFCt_aiz{nLCXAWPB>CgbNLhY0-{Tk zgZpZ6R0amx^O~(*!^!@S@k`#@aAepn0B|@8FmQSws0bKvaBDO=m*49*T1~&PmW6{G z4^jw5{6!$On~Te7rPled^{DNP72rJ)smbw?DoHTnRJ|tRG3zM+mLI3VQrIK#yHV`C zV)Hf_>WK@UQdM_V=SSVq-8k(vo{zfY-Q%*(Ha|6sZatLhCr`Z$a1vclC=39 z&DRQ2b5?kG`58BD{Xk$uPza?jwV*C7GZmF3CLL#q``EIlX+m5a!)Qy6ZS#jWbxk%K za0+n$l1M3#24=|mtIqf9>uT9vKaj2oIk7#aEL2K{fPVm5xx0)9-~>D$XEC??hwE7e zyT#htim%e7g)YM@sAXtE$M*Z@!hBwGcUMx_SRPRrJtAL<5NF0etwAegjupV2B!ALb#_WiO-Tuvgy`quWeW~yRF8dWc{gvzPg=J9MaM~L05CaU zG}`&`;lk~GT^nHK{{9ek85*#uXK6b7vIj0AYslvj+)cvxmYz?`9zb!U8*Dp3d z(9dsJ50Ft12soY!7`VO%Izs}Ud60pQssy|VVc=1phKp@w1N+?Rx3fx^@sI;^PMKOyops;R~PNgQ~(;zcqj7pt0w0%SDQ7zbGE;927~9T z1$wjG=|@UK4+wzq zqwV$Ix`Y2z9K7`MaM@>Zc;22Y-jSQnklt!kDYy#T$9+vw!W(y{c6(-GXXjx89rcuo zg`({$xOBOO0g&$i03oxvE3nxtUR{)6?Ac+tPwxd_ds#kiG~tS)vj_ejPHNq@#u#w% z4?3^*wj{o_w^?~x?yrU`>h*U;^1%qS-~^N3SI9cNq3qWsdV%E8!9aUT`&da^jW)FN zv0ocP}`?uwluP%yXOJ2t1>S zV}{fMhXmFTU4p)ZfgnXc%FG%k+4T-GOL;yB#l{i4;#fzcBaCe0gbg zg201OkgfUT;%4V{;I_YdJIHt`f9Y~ERc}RT>3HX~)8D6nVTm|4ys1IuDln?q zCMBg~RUfJ~5k;b9`mXVDj4L)N{5zJIU>+uEv=Bt#aG54#KpM=m*TLJ=p?fHR zCb(Xggo17hU|VDY6OQkPvB2?RWYeZT-A+trIS+gejCfA_rkbpx3H| zV^>Z=wSMQ&=WS1j7No#L!)pP2yh{2-l#Iku?rY! zO%cI*4Q*ler|bY-FR&yc2uk9H=Mdm}#xr=)PQdqt#6#r3 zlfwwi^`+fz&;3E(h(b%O)LA(P_!;Vmjh6{zr|7mws--31C= zjdeGDCYC+nuLvc7nW6UqjppZcq97b&fVf77wkXy-YR$##Vion*QYYcQi98>`` zy$tJLqfPKbg_N%Av_J&a_SYxFkk#@7RgBd$xS09Og!8c@5OXVXeu=R(L>9$WmG}@7 z9p4n=^hMMLT+l+Q(Thc8kEldy!$P)04XFW8)V#;PiLyMs>7$5>FA4S~gj`ZuMU082 zC=MxowB4r_V{6Q3cu}-li$@4+{dYY+Bo2O7)ba6t{)Gj?{)$~Ft4ElAUDo>U z-vN^bOswv$Xl~IlM$#OO-WI7#@Vbq=4Vsa;KOHYD+R&hd9U~6d70XTCrZ`o*c+Eex zyNtEp>RjN)V;2**GE)?n)w*PwR2wE^*iG0;n15>*oR@y{04g}M4Q zU#NK8n!dxSZE$G+>eazD-PZ`SL9jKey|Su>3~9eiCs<~KxlGoq2V9TLUkzr%CY?bQ zl7(A~!_ycZxK|f34(@?|pQ6eWDOf3`9!UY`RjeC|n=+1%*BYFhS*wOJqZp}R@F4L< z4uRZ5snb0o8=KCGb$#;8`12&3!Q5#Z6bgxo^I*DWD*F_Mw9JZy%0xKs1HGNyI-qeA z=IEdRxE)T6(TLX6$Xdf;SiOGr^lWLu|CEGd!j735s4Drs158~|q-#B~_kLHAeVIwj zBMev6hKzo~;G+^i*^0DSVca3LCyhv0m*!jP-c_mVc|*TYn8CPQT9nfChu%_u=lBTL z4;O&BQ-d_4$J~Iv4QHLVF`GO%+}V$26XO&5jOQ2_{lQJfe=>z-%;gD(t^!6#;j>9Ec#VtEJBQn!;F z0s;3|S!&fNL9PTuh8xBLXs`kvPTPLq<0WBiEJp=&Q{X*5uzWUIO43-MCu7XIMNfSt zI7GeowzmfXo*y;Bd2O3H`e30dN^FA_sn_>rHe63|CBy5;dmHDH{*eSPLO4q4nbUAA8Mo<%TyQ-6vLyO_AYbn4!Y zppzGFL1*BnIyr>*z;*P+N~?)>v=G^!k+z!&Xv;2+K&{cRO*=HC~N zc6MjeEsac1T$rU0JmZ4kYFNu;!*)CGGhTloxjbxHNqghr_dCe!7ncBIV2R`tBDr1b z0nbHL6AsW}#qFAJ1f5QC3)T>NU;td*ex9YoLFLU$TpJo#%t>_9HIodeob+!-7<7nk zG?b+=U=tzpkrBK=p0Qb33PQ;qv+dJ8E|tB}RxYk2n`%@GWi@8IOUIhFKzP>iN)XId z{mp=SrKEd}{lG^~Us|9(n#_GD<9mQR;~0)EN4?#63xrbB67Q6y{iqsahzC`2IUTYQ z9OEO?9QWF5Ej-)@TGp{?#8e}?l}5askDgnQ;M-KRNTeb{*&y>$v!e+U2$?U8-#Obt{6KNn(tY`@wti}Ca(;e6FEk?x*e^)3aAs_+&NX~=liE&Rmsiuw z%m(F&1m{w9%cYrqju>==1GyDA+{mz+jf5BtaGs=){KSLB(~3=HWk``6$Ehj-qw=@t zQeYVEjrY+A-|3eiE>)KS-S&9CJ_p0OQ){W4>sa=n^JqH+qb*IPgpEkqfxUL6SAv-p?lUFe+)uKwWeD*AUe_D=x8I z8u<&xTUJQNd)iwM!cywjiN{&@9^bmbSE=rqI<_9*hmUM0?BlDo!uJ9yqQVS@9Mby* z3I|_um_lbt3ce_P#)SBnSW6FyaO~kVLV6R2Wmt;6_B*P#gunyF6GTnE#t0_)(PqB8 zLR}eub6KUazAEAe>VgGJF>PF>TGIMb65^5o*oyHA$4A4r1Cbq}_HHC};7<|dxL=@- z%OGIG(|f`dH+LjXjgA*xRuxz5V&=oaMNWnbwf>B9iwPr=uUa$C%QqvbL0j^KRx_lc=(Wk3 z_1_j^#>SVbZjAJkoFp=iafnB;+J{A~2 ztA$Eu%tNmWF4vd8w1Jd|N^6PSJRBGpknr>lq#YV@x}Hf_f6qD6>L?$wH(a3~XTkq@vWw^z_{ug)L58=wxvN zJ;ymg^?qVvLBh#fo(gK6Tfi$-vxCzwz=z^`H#x|m68qUgiC`N)O;aoTo)w6BySei& zq`I){Qda-s-@#5&3+Kg1ib%^^;91EQs$Rap^!SdhLQEaMHT&BdsQfZerw4c*yA?Sad*HA@fkF_(dA zqc6oS1&l2fux!^r@9e}a(n*1#&65|k{N0;!xvQ$}vvSKb!ILoAZ?DBqhK3kmmPDlD zIUXFTs!kn?l}l$gnAKUA3H5L{C?>I`sX-<@(S#1j?Ce%U-TATnT%3TJ4`_0)#_x|X_c2gkKei|+?q}{UrrtaRX}miP z{P$GX6xZpMf%q>Z6&wJ6C;#XQ)Q>2woKo6t=^K#&@aEABTff^cGmH0#^k8>Qc#%9v zPS7<+%Iee^jV=Ma(|RSW^1VW!sj=E$(#qMs&v<}u!d)+*z-*qFrLMdPT$^J>oDB40 zWy|WryiMF2gJ}Q^n!l|3$!PEEhqG9tcq}SaBQDlm2GZMR(~K$3m^buJ)lFv=0!rzr z?LFBg*CP;QRLm@hE+~9Ybd^`Jm(I6iD7pbcP;Y$-0S@CjZ#TCQ(>B7b8@CKOBnXZQ zk%I-xQXC{IJR-RFJL#rmbTF^y;ys#C%0azb6+V)wf~ z`B94xzW72LOuDcQ!WLHlg=G#9->eS6eek*DuOqg2)3ZoJB#kxM?Vw=^=mzFM1KJyp@6%1-0ERBv|W} zD2O6JY;M?IxQM2w`q;fHm=eLD^Y=UI>=Y)f-iY+$5PO+C`VGL)GYdVeJZ@XElaYST zDPl|lJy5bT`HUN^H=4%AB@l3Bhhfnwu!C%xutKd6B{<^EE-%Iq!ov@gR=G`7InJ{n zIxBXFYq&WiCz&Fd@$n`aXBS)AATnyKW~hUI-UN9={5ttm*IX-_5F<);rU;l;qf(s3 zv7@I=tC{T0pxuvMi;7(v4ScvKSGGktHGLZn;2}Q&>r}B@Z&4tlPpix?;rf$You}c9 zA09_YQ}~u^A3~>rBUYJnjj{hmrbYy~tG=OR+;}fgXC3+K!DXlIV&|{RaYY3*p5K_Q zaSZsG4vb<6eaqATktO8PHlRYaf$E0^Kp7{eVp}J51s`t38phSrDOuqG-}M)MSmfqz zcS*iw-dY8@i=;M4ODLim%Sdz%%w`n|JuY1f>7`3Jp=^4jz&V}LsZE}A)YGKL1k0EL1rO%~X zD#2!)(Q4$rFZ$-7;gZR4Rvu&7gORy5-O|hH;8biGdo7Y~b4)y*iA&vS`-ZiT5LMR1 zeQmLJk67_SSi!+*qg)Znj9@%$2+4M9AJ*)w6>O&Of7wfLr_vbiHz))eWb7*zrYuvh zNIgxceF}nkRa}MyHZZX;+WNrG*A3-=SIaeN-?Y4%!{f}XC=~lmpjJE^V0z)!i=@^7 zXC})}56Suj1$GdRQ`EjAi@y%sosT6Ee2(>ygzTkeN+aRB45`O5GAsEMz;Il+LR`iB zq};;p`XkTD#33%C7H_ZD8TyAyz0E#=XH^%Z6q(2N6;xnY97J6+D=4Sa7`#pNX^PHZ z2cEq)ggBeyd5jAN86U3G0G7iX-L6|8fF-gTrA=`V#DbI{lFggo%P^8q0<~nUcRf)m z_bpejntSyIZc60X{R?A=gttm?(#|LYrPoE;H277KX8hM-)gRAcNz?AK$eG2Zxg(PXa6 zo-pWOEmOqHjYm~5I*%E^7N;%;b_LeIBYTpG&QQRqTXPG(pDAFlCh_;>qwKJdEW3l{ z6mt)d&VD$sXG|3M`}xIxPr{-}6!K&To}y^l%~)rwz_1kP%+qrv46C&pnxXdRm_1O6 zUSH{L*tBhlir*oxMO0%Xw>@UrkW_qtvrBAQDC^I^6d|VE09I1#X5F}+(3t8+otf-q z%AcVHRK%^(Dwc4CnBe1_A#Do(oDZO#D30_p+;Ry<3Tic-1AnIOJPnntTqKF2+d4gG z;LUdk;s~r7$r7kH^%u_rPxe9@2vijKhPuV}X!tUp_}7Pm2j4D~1sqeuz+dZ=244cd zfJwn`_4S1+?7ej&Cl?jk(01yQW&qAH(iZb*Q-gt@1!`a=Tz8V#4MLPfVhWgw%s9S-$wENwxdN*{c*EN>9}fXidIMw?-BWW8UwR zh4Hl290?qM8O71~aL}wSE416^+Y~lHMOhBzvabiG*pXHp(rEg^H?FeP%@C&}Z$Q;g6YxSuTlL-!xCnbO@-dsnn=uhZz33HgxFOYBK?V$PYR?#i zX;zmF^Uif~hw_vX+`t71oVkZ8zvAXr2>B(8&C9Ij99|0~b5b>e?oyv(x%2Z2I+rQ_ z+TYQWG*z8nJM}_e?cC?I(*fNYhmjF;pig7ba}HjAq=R*i@eKmGwLUrOICWg>d_uzf z?#^*Z)h5-RpPH@;*o@3H*1np^#4Smqx?!0rlq527uz>2BPpc_eqMFb9&8fq|bZUyq zTG=r$`A6EjPDgD54!T4UsMGr?QJ77BTCo}cGug|DT(*ZT!TtP)lVAKw zm$~+b47T?a#SW=`jx0$VviSUXS%#L(aM=%Znlb|%K9HAUwkE>j4y#ChaYGOv)_W$~ z8FmmYz z-K|b++weTJ4g&h%RLS^-w({;C=0g*`b4n=yg|?3C5n%WY9WXB}$wF&u92QRH*AmwT zVO72P!sE+Snmuw($dT->TKdH&*Wp9_O3Tvj^0JPOC%4tewDYOo?b~_j4TLuiW1~ev zyIJE5e`$|SHuEtk)&d92el;nlqVy!Z{r2Y7oX*1C#e%W9tur=B15<;%Tt;q>sz~-+ zjErA-Am(Grn3z&@`GKZsB|@0wtm);k7XkI)coKsLbFp0HsnOkr)3QlLo{cRy9cg6f zeVD|A@PV|$H=mY!-i(o-gT+u|f#K|$g)so?iBHPl-yB<3zoBB;tcKSRjGnrk#+E%`G!zr z;#$U3*_{Ni`Sh^c3lUdLvPk+x{vq|>zaWfAxzd^fIrv>q*f$>rIJtnlZ{TETXQ5GU z({gbN`#}hbU+Hv+f8T1mIv>Dg_B+_DIK|9Sww|R9yF5^F=q2-ADRX!2#Da)7cwp%A zw-EIAyGRc|r&R=q_Ng#N-TwJ_NL$BgQIM6Qr}ijXtv!8wUN6Q?7j@`C-Vkg>@X>2L z-78MDl;KYyxn<=L%cxKUdZ+4WqVA1BitPUD|@Mp*Yc#86) z3#QR+fjV6@?@8uv%>W!`)NMFUW=Xz1j~s?zQ19VgbHFg44ChFQ4Pz+%* ztk_uJ3e}15XAL+zgV8FJi_CrZ}1tx^5^9t^Wf7#&&`MIccQO zYr2+~B-Q-qI|H2ZD+}3YgnHQtU4C4h!(C? z+K!78KYz5J>vTea6T0toG-C3_l9uavX^dg0KYrpXmsTZbVXj*Jmri+hRCQI!29=4F z&h>6x^hJ%D(ae%%BU7DV>H2JFX~SmqJcaFNG0RdR8o1sxIdiVC(>cd}kx~v$)#!@c zLITgthIKd|;IAcgAONFPyf!L@yNSEc5eLt6kn=_oJgG3-QMA$rd0!wxlBkGk66ps( zlhST4RaMBeWfANL-*Kb*S8gmFHTN3|A1sK*9jUNvi7EC`LPY@|5g{t~6-Ja0s}kfp zC`o|>f-Yp^OxJSCw8U1gllheJtAzqm<1pkA7^J9r$`+ypT;l zImbE*8NB2y+(U#mn98peAar5%)*g&@=%x=<)$)gzQ3;uNc-`;j6mP(>in4~M#e z4z)La!aU{|AFR>}qsgcpvdJ$%O4DyVB*Y>l#FHhWynK|`*sM1(IYIRe32=Jh+aV{|C1u@PHa3g(aYm!k#`hF+I0|$m zaRZdfDE6?$w-2DjTm$YBCdRM^sI5t=;qRE>oE{3GTTHVH$mdh5;O_p#r&HUmQ2;x{ zTRm6vXEVOSq{cYq@l~ut)F1MWp$_?b#QR(72EVhcj^5f}8DW=d2NJArnRT@uhQG}{ za8EX^G<>L|!xyyQsb&%>CTf2e-_d9I{G_uM+%?RpTr{b+J zqK8NaF?7$(aP^=8haFJ%7CD&)o0x5Dt&d~@@^YBWbqBmaK_^N0Ow#mk3B(%k7KvwK zD=XM7lo7L+Q%W_t4T4;|@EMTQD?ptWjP59}g zk$2zg0ZGW4HJjPgHhIJz0TQ4_D|^UW-}wz{9>bnD$vO53^-+(gFUuL;*DIFicj1;N zqzCK6%(4u`fpXE*V$&?eKCt!ENWDiY#R30|p-h~_Cjf@GaiT2!kIn`p#TT6I@X}ZA zPH!jFkkxvSZLFYY$S%N8jL(>+t)4KU6_-8v>D*zqCi3>|!KAwri`T=z%A=fRe}DWJ z*P;Z9=Mpj$TkRl;iMdhHq;7edzSSGxb!9y@>KDV(OHj&MGIo1DC5g0jSlzLQ&3Jii z;^wLo(5ux%TMqjKw=#I*V?q+g)j1r^UTc-Ebi7j~57)y@jNiW4y&%yy`ku)&viiL48nKTum}doZi1vYeas7RO_*(9}u6+ukVblz(+#S{0=y zmO7NNMkjr_P;|gyT9@pstU)0&aD;K^~1c7%~JE}3Vwyf#J(hwr|noKh=8y*}Yv5R5EE zMw6dp9VAreDfk-pV;WoS#iX4RIT*ql&i^rWlEa+sR{(h%~;EX*B z`zwU*XMLm{DJO_ZUb;`aCQHj|)0FghiXMn9SyMtRQk&# z_2U*+XYWbpEM8x67y15cDsLipLWNXE&0iUi*NV9rmt-vY;&awsd1I)bi~xtdEDh0cSH$S>q=+pEN88+&LVYQ-m0i%NeP8UC49|3R05 zgm3=dv#g1&5AeaVI-OOhL@uJukze~~X@9HKR;=>^tyf`3Wt>cNw+v?V9W;p8UGn#~ zMx15yQ-bJ*r-{G79TXz6E5>o~j3%Ie84_EL#L(r%Wfh{H3zik;?TOofvwzpZ-e) zzTS(5=%S4~Su(&pP%x60?w1lb zvQlWUU}|CQRC4L18!}+?sOWT^xu{J4iHmIq*v3&b$6vd1amy~LLvsOj?Xm4I7FGeo zR%pJ*4!=)cv@b^w@{iLf#hYTL!P9WeR-|o_Uk&QYm;96e0}Hi{;Spjmo+ngQ&5e~ojE8N32x{A zV%awt(E6XS*e^Fb+OT#+&IzhF>yn^`C6_dv{}~yA);=NJ?L)%H0GEvKeTG#_x&cYFL&wSeHoMl``r<4;8Co)y$O7roHttdD&5J1|Fo+3RQ z&)Nk%OZu@T%+8O&?aCAX0As=MC0tq3z$+nq66Tvuth4D-2g}{Xmdru8jxee!7i;8IcNH?vNbDe;U2 zO&IZ>>SWLmM#1~5bS^$8M|HT~v)WG zNbLJG3I0L}DzyC+u#N4@3cm9V4z9Zb#->)}3t?;7LO5y3qf32lrI zCL=xyCm^l_eWCkU2`0SfGebLm+xdrPyGrFR9c5>HEl zMlqvbMz3u;e@?kfpIxNmIN;myZ(mHMZ@SfU`cSvur(yS8*!yMdwti-C&;r-q$E8A* zN76vzCiTN_W|`{<0U5tigT1)tcK*JGId2cLsNVY2kDKxc_^vfM{Qh`ce+E0+Ox^D4 zs=aJz?1Ooiw|Mv3TKf`PRS$#u9Bz7IxtAigo7~&k`;BUORkNwgtfKFg#)O7t{*=zr zU|SHPt&?>1=o0AWKNe>vU3?}Ky_kp%`x=6G`kEQY|1jnAb+tM8&Dibr z-FAAgNqpK%{+oq*vcQ@n-hC|9aIA_^kJ0S<$k~b)Ya1hnD&GZd`*qYpCD_F$ox-o` z09`JZx>FJIAwi<&Og!U5z(D&aIVgyDUl8lK7OfabH?OR!HEFz1eznJqC(SrLA7&^{ zB45v_nPir$)F*?Zl;viFuc5|);nQBy6UQnz_{CIR2P8&zmEJnmXnO7)f z$FCCxO|?pQuzr{JiW2g)J>Ph{#%|nHtG5Z4MiqY);HXqz@m*-5_FMVJ64S4KcM^%9 ziT5TBgmUul>7(L$THGI3XvB|JexR^5vMj`b?|4LO87O&%Fh7rQpTu&NDOun*q)0@09(6)sZgH?ul6km~6UEr;@^G zb91W!oUU>dQwbsf;#_I7o$V3xdI2UkHIoLy0hVO4a-*o1%`eqgBdmG1G zQ(l)|=rG4mApyw8QEO!iaG$hZq#~=}Zh%4cOx{fLqC%&~a|Xj3g=ZAN@NuKVoxf1I z-CSxz7!MBT?nVAS#V~nsQUCPI z4isAxAU0QD&=L3$(w-QI!&~G3P6$z_;beRCl8uuOX4>Lv0!1RT9n((7i_C(yZI_&px{=5$V#jq`*7SH&dC$dD7sN{SJv$W-*by$Z*Tyqzz zENb;$N-6k9afSwJZBtEFjSn7_d0jm&EnjLOSAAh?Do;XxOiJ;ml+)N;KP^ALwymZd z<}hH`*kE}1)Li$pSO4)9E_`k7Z;6>jmgrHwcJa^8c{*Lb3S`f8-lCU)wbHRph~g| z<34A@S#b(W8JQJ57t)>iQgrLzFefFx(>s#Cv;TmigoFGvaDVr$DB0$k3ulTI`50HM zFCtLDHD*17RcXj*%l}!t!1X^_FdK@>S{$X$=FWVa*Q75JRA@aao6F#+AK=hJ4A?HK za$ZWaCeD>NQI`bdJ_iCESh4#|i1l*9y-FqM&Lxu}UU@RdCtN?wQIMboFgel}o~u~e z09x5xaazok7|j?9ymYCDRG>Yhl-6plGDs~!gdq>@h25==;%hgR>ZnSB-o*ISO<7WA zjA(mAs$Kt*ht6NGtUL+Fh^Xk|IG7p|20CfF?~H$nUoBmHMmb-XUEnYr6-xB&mRhhB zn&hu6sNUd@$-EpDr{MQ~6*o=%+==Ucmf#|72ML5MMKizB&`=`f#;sFw>dy>H#ECcP zJG;b(ZQ_Yh5-|qS6Gha6=dsL|=X&wQTH4xG`RE_enS(WDy@Bt3`-h#iDm6dM53Lq} z_Q%c|??1V3wDy)wK*6Fv4?0 zUZQC*Sk|>9W^Sz9`RVQO`BN7?S3rt!8vD>?-^68slG)RQDE((nSb%^IJBZ2YPe=Jt zw4u1kYP!)RzkSitkY6P_I7fJ1MQD?h;GZV#>tCB|Or~0rrg4LV$Tz#2)Oorr19Tsw zOWFZ;S{%O18_;J%oWHP;bGpEG19_nzrs68tXEo9u&6dj#Tme~{Uj$fed^nF;%@`Tsj}YgY-A+?f8vJx6E}#T z*KYu!*f2`kpJ;~uxXmN|eDkmIv-|&shM>()EUg`N6y0o%9JKxw&wm2^)2{wsGye-f z_vhn3%B#+WAUV+g z0Q&#=bN>_SpPttLhEfOnU!eYn!}UKg|LGk4FUZ0Y$AztKtVw2!9YOp|2?%ebuu(DbTVWzws$mD zbhLLcb#$^c1v0wX+ITBZ$PX~1d?bxN54yw)*3PSfEutr(K=`-fnr}cE@zu+1duEx2hf0E)FkE zorSuNs_tzCt6agMnGkLI$#+5sdeid|s(WH~-ff{h~`o0~brp8KLr2d|;wa5wxjyNnRWk|~1EOqIx%r5q_# z#377VHz8*~0@j(4ZO!SlQ_9wpgJn0uZ=*OB5C(YIgqX5aEuFTk?Rb0Xy zPE7u!#5&a&`t;D&6p^Y|ZJK6q@m)q?!tJs@n`;&j)=ZE2z2UQi0g*a-u(>zweoXyW_rkxR{Qs(7%+1Nv4rpm__aA=hMt30vFk^`87qxYK zl1dLMraHMOK5NvE+pXG>e#r@g=nL4Enbs%F7NT6^q45 zHDPajyGxz+pw_m_;Y~fw+)7;L8O6XKl1C#tkiyARvWkARyTP zY6Du@I@?&t?SF=rN>e_E`VtC)wpgc0$^rBG>KwS>F#I4>Lkwha- z*!p1({glx_GmlV}8vp%v;VLyAZL0JHhcIxzo4QEqE`&7G<@kM7ZP15Rm>+=Le=$+P z3_=tcbZ>*A&fE3ExAU(Ql;k zs9#D@;gHi|lhhKYp%oK3)ss8VeJS1Emz(r~)RYKv9d;u5I{f&9Ri$pi%=B)#k_()( zZZmd8Zt5gEDq<^%N}mA+U#6Gna!y1@zOC)|1Vl0*f@rd%4lkK1S5ke%!Wigi*5GY_ zZR=5#wP{OT;l%T)Q9r2vjy=7xGUeTs?pUi77pLTiqLY#)P?|K|S&Q$>z2|CY+V1jZ z638ft^GFQ>Lx+xm{^mdZI}t;5MgwX&ZOB?Vcl!4PY8fo>(1ob;a}bI7_t+J3{LEg{ zjP2aasFxsFIvtZ=U>p_DTl*qa4U+fmQsw&YV`XBRuviL|(|3ue8k4%d5Ko@u38lKQ z<9)dKLy|iJUh#(jg3yg zxx9%-;Y4x$=wHPfnA_X6#@bru4iFkMoHwT30yyfwAH<{q7FmxVWqHQkX+VWC$RAY7 zzkl*L!HsAfouv4334CssS$^Uwg<7MD_9`#hr7PE)wIaXaEo-lY!xh)%^OpP`{iWkZwh1$E^rxP*5tv|vjYxKx|J$jB@dN7oSipVpDd*iGsnDbs2 zv9p}F$6N#aSPr5>ibwDCz752p zmKCpqS3Ns_qKnRl|B~#bw1f8?P%rt+t7z24B>;~Q=ks=X19dBYkW^(A$0zjkq9pLI zi{TG+(=fXD_z7R0ab+YjU6)0LARuj{@4FqafVmo>m|xp%y`2cZ^J>d;UpV`hO~=sS>VkiqQir;+9rb`i zhsQ^JO3qK!(J;|P8KDF!yCwDS5jG%}!-c`^6HKr&V|rCYGp@LHd&?fko9eKT*;laJ zCa=k>>$}wv}wRJ^tt64b;m((Fc}q#O2lOkU=v z*X1S|x9BP%cTU0aRi7BqmP*fUD|&Wvsl6wZk7Cd`MoaO<^1Z>dLd z7vw=JWtIn|tHKWL-l-vC!vXt%Qzm@2%0Es?GB!%$qkH?0C82D3L;Z~-w&`mTk~$HdauSw*V-%pEuARG*3d%JDGfRv&(=&)&RV5&E?jc;N znP+%(oYYNAgmA{%J#Q3)8X9YQJl^6zBM;ANWPQQ^=d3@0H|e*;1p(=T0|oiF{I^+e zV{dG2VrgjZXlVPNSJ#!UCvc;={<|k9pa6K2M*b9NFCA6DmS{@V{zR8d*m7pV&kj3Q zNuw;H?9&MBe%JLwzJrXf-x8JImB)S^GJXAXEXd{<`0Two5C8|XAUuuhC*}*n`d1s+e_ZxclZ>nyt7&xR#_>HE{4iJ3((8%nV5Cd02yA_rIl4dB+ zW(S*RGi&C_d5I?VC7Cj{%4&}MX|*&|JB(8^7;n53@0H)3Wr(!?-I5d&&rs9|G5L*t zM-09Dyq`kkdTaY!+)5a?Lc$74V?7bnws-pXNlBt);10Wis-sNf6lxP{wDpxj1J_i1 zwJe^9`=(c$Z1(brQ~P~-EZ{m< zmo@n0DW}oncnW!z!O?<)z%3-${LfsiBB)xI=QF9}KrVkN1R^pqj(Gec9a4ep>r6J+ z2DHS{YZ2`rL$La}8!N1$rk!d~+$K7PTB7G+O+|k#mVwjA&X~aK#CZ9T2t1$E%uqiT zN2R(ow?{3mfKZ7Hehy3_N9$?BLtC|7vVK+{yN z*N_%E8NY_dI%GSMjvF_qzon)jrC6G5S6TaIRz>a#NgA0ihm1QWauZN>LinMx-jIUM zFB{zhIRivrizOZ(Es;Gp+6f)H_s)F1IoNO85})UGwl$8Ya3Zzmo81->t( z#M@Vf>7FmXw+-DN@3<>MFSAI-dkr_8eazE9e&4{qj#XR%K)vf!EL$=jZ)#*?=$SBc^cG~vp--z$I^ z`a5)k^jH@OO>KFzw6cRG@nLQzFqb0%3$WyJC8hdM4(s zzs0G&H*J{g(c#=|UmGBJxAwFdE+D|JV;u289wVtOc{1GwxiAFH=g0E_dLMnc^5gb! zE!;be>}Nk$T^xtoDpi?r$#>7UOMgphhOfdvi?f(Pws;kMR<B-M4@^Vr1wd4Dlz|S` zdF4qso^iGe`I2tumY4*w!~fFx8Yh>8PTIJN*296D+`Pi_8_!WT7Y7X5PH}^qQsC}` zS<3E$hID3##h(T8Nz2&t9BeBH_{>h1_^}+2IIF`&Q-MPi)M>nIPd=mZ!2Mx&l|p$Ahfq#ek`Joc z^qrZ(4=b+$e@PGu9BP8n5Wuj93)hd1oF5a1y9V9VKqior-wVV_jT#2y_Vr6_Oh#x& z|25Jb>_wZBgRsN7i2_xL32EBo$W$E=N+Q}oI+@ta7vhj4-QyY~O9rb-nD03c9%T%h z`WlIF_z_!u7Gz+^{Z}8x$UG=mKmdPnju`K2%1FKdhZ`kpUxqdLk|#-5*v;tW7IOY5 z7n4V`>TCl@KhOPWc-cs+rmCQl$2J3mf?8Vt&EElI1!GkBV_t&Scs|O@cQ3szUN^pD zR_dVKP;oo6!yrCXTv6q2uOIfR!Fp7`QWv?tu~TTf+L4TpJne_L$TK626vR!N`z1SH z30|b18}_bJ85y_MWOb}BKW?Z6(IK$b6j;rIT;1hRn<|^D(WmGiTT?*LXK-gPrK};9 z(kQ*#-zpoc(3&hVJ9K?D$P9}h{XlqlM>7p9Hyw?d7%+(7n-WV>d)*!!E)!^?!C}G9 z_C=pWM33qUwgVp5IV|bY2jhPMzd84-pdE6SN%eZD|o6DNN=OLHXfl5ju>R zPCuceWntv!uN%@JEiUy-$805I3Ss5^(fsyk9{GlLBr~Rq;d<~{`#4rhpk zKg;L`fJ1hBJ=$e;x!v@vJ|9)t zPcS(TqwQ_JR}NK!S{vl0Vpe~5mk0JPpc2xlf?;=H&!VE-M2;zQ_m&VU8eW6}3vY9# zAK_vs1b`c7dei^ujnAVVf1R09OK%z)3=5?O2w@yCTAFNGSb5`pww-Kox}Dt&?4>#FkVcx> ziP-%)iSPhcRZk><07s&0w&wq$XqMclHm9;fM7p%I09e1tuj<(a3JiDK?wf9%d*Jx$dL@WUD<3wH9A z-p>M~?9d%y6StWPMpo#@2&0x#!%fL~4Z)YaC3|5dxKIl|8O5`7zxF^{D70Tt1gq)>*8&EJ;cKgS0hl7cU2(Vto3Iq1>E9# zwU{t~7%5z~s4{JL{YkwE-L&k8oOf+Tn6ozaita@uSOJg#jNF`13QVpNB^72{ZT)W+}4-3AJDbu-r{BN7)A@|?(w z<(`b16CCVr8E2VPy&ZNAL={|?D1X!VKoP{XU6okL^y%^UX5e+tW{+IVdigt z2Pbf&gk{?}oC;>jeO7Ilfy#eb6J#W0XcGs}tBW?6?M5HnPZ~}vq zB)WK}9z~`>`g)Id_b@wPkZ1#>;LGQaHnk=!N?MUMs48dm!v37DXcM|(*wn(J`hlZC zD;=?#00~erjnd%RV_?ki(rDLpW86cnx!2N2hLA$XY=d^Da2})wmEv+AhZjg1agDeF zurO%Xg`J?WNGyUQ`cn?XQC;8etuxQe{RBl*vc#mLnAWc^Yfl38)jsME!;AFl_5 zhPY>`-n#l&941P5yvr0v{C-iiaBo;dSX;)}IciB=AE%-T5AN9SxCW_fNi^VJA_dI<-2d`<91| z!D6cilI&3o7(8@tvrt|f*rEM8B=}>9R!HMR-YFk3gv7VTgI6Lh1wZM{_B^%8 z1>D(L-RlJ5Ondjy<6IfxT$%S?Ilgf_Q5WtxJ;Ks9?9dH;jP8g z<`IP&HiPZWwggL!>fq`B3p5qIMHIi553r|f&b5N08Y80$tM9jrxDO6E7p{rfbbIB` z{la5zU``zoYJrUqhsG%otHA&SF&X_~flDGWh9k6dV3-vF#3mwEuRt0Be)MFc;qQt> z)%eJ33O3SlmYapmNAt2DXMF`IW5%(57SU8uqVzeiW(eLyHUZe$iVkd$#XM)Nd|WX4 zhM$9X)=e&KmUTL%_~K~aXb0EPhqSJmKzt}FnHYw>J(UBuG2pH{ApXKl49o2w#op-q z3LJQ0tST*zLiuYP&c=@I_I$L1G9*NY6?G2M z=g8;Od!-*O(C3)FKCYZiVfP)stsZL~FU-J(vT01~@Boh{lYB8x9>7*jsz2PShpHh#0TY0b>-Xvhf!X=-LdRqnc!LDggRF)ULt%?xv<|)DbpK5PlMB5Y;D5 zx;hjfprX>);#NY@N{4g<-_U;FG8;_;CAr@`ZXa&XL6Ni9n!m?aAV(;8Ku3z~(7CIb z9X^Tz4iNULWh2EXZqR9H>NLqs&K%wniw5Wk&CZx*LVM)xyJ7x|A*_TxdL|id4#)jz z@aBw}ZMWFDPRIQkW`qcaCs$J8!#K(~T~7i4zkjz0?kaL7{6z~LW& zCN`Ve0>jQBJ8&N`(VMLcl?{yim@q&?*O?||)Z&iBh18Ly#mjFy5HL7W&uU5(U<@K? z;n0LQ81j;z?#z$h|G^*u>Wj+7bdzbf&L{tCqICHSx%^cu>Ue{bl!HOq?f7GDnS7X7fjgs^vqj7w)y41ybX=Jc>*kbIC&ma+1%DbdA#~~X)!3`OWM@bHxrzl<~e&k?cSG{twY#R~!VH{N#?ub5f?B>ZY2Uj)YJu!xS`8 z1`op^0iQ1B^d|md3-JA?mAi+}PyHG`wzxUn&9Zj=Bx1o?Q*VTUv5wcA;G_j(VaaV_ zTJ{wq6J8U6dl*XVXygENU`tMHr^DTadU-n2yt0;g8^VOCCgXr3Zt_Iue~~dKa{^{j zhq8S%ldp-8Z`JuHUD`|xxq)EL>xWw>YtD?9;KS?W3EPL`S!~7KL}9|k+{Denl*OzP zz@pU<>$tO1vRU40Nt;T=K>3i18}qvt@4Lq3wRi!SCUn4utk!BrqOXZXqK5k}$`pS= z!xu(!eRn@bj^+LJ=m{!xN1U?Lr@hvTtZH3cOE6B|Dlz$<@7|v7%Cwy+`F`RpvsH_% zdYPW_=yFAEa3}!oygUF&>3GWr>nlFfpH8h?fI)0qJIi0T*H!K!VBGVt%GC({h0!(V zcDci;mKg6r-WR|#lNjdZw`u7nmxV6Q7-qn20Hsbpp+EW@pmF_~p&=mzU&EDcCO^GI zc5RyHNKt4Y1t%z#E{hpIKkE-Bmb)6yIm2W=EbtrxUW3!g>v6!C&CA-$sf3dwk*O(P zBSq;&QlB4xP#f*%NX=wQ+&rE(6mrfvD@ z!Q_z56EjqSK=9Spq$DOA9j2aG{Gdyn)vGSv6Ph%~A2h5kUcesu)l4t=I^jxyNUe)S zZ3AKI$+u8~fN#1=Js^(D`R45LAXeKZ*xZ)9diu}}lF;_(7{it_t-}jE`%Xpo0JULF z`Re>Hf)>g6N=;-b=!^#8=AeuDUs!^bjQ6*^RY5t%;(GW*i0^al>b_Y-|G7o8>6b)K zEUvt-swXEmF07-2!EqbZV7*d{kh#pMk6v(;8Wnz?>p&9vv9r+`z&&iGDL@ujwLk?g zV88jE9uWge0^c7aGBDyjv-ZOIJYJ7DlA*i^D`txhDEnb`H4=%Eio4-!SWhtbqX}1M zBY|1zhpDB9DTkF~zPOyVYJn`z#>r7dT@o66yPossfndiM;Me%C-=88^R%34xG4>uj zWzxS=yqlBCp;H|wH~ARXlxAB)!gi|HBU36&>MYwLjJ)ow)EZmg&n?ik#|O}{UBk&! z&ZfiE10?H6;@8Q;q+gx2!zXIJ@PNnj3a;9@frw)=!aM-P6JOV0b=I!&!Nw(ONB77)EPz%9wtV z`Wn4}zF`XNuKK;XXv@Mp?fqiha{Y?b*mcBj`TDH9?YhO}aWT+Jl&w`!$FUhP=wP2K z?sKtDn`!_iuELoA`0ym!6GG7o-T5eXKH%zsCHqd&`wVGPTY~X&dBeCRooXm2M&FRx z&2GsQh4voqUOSWdzQ@{QN^FeF$o30=s16ycJv&A2397r z$EmQFg=nPRO0Ov?XlZ0Gb6;cEDcD_WJk50~*Vcw<)M3^{G#@6luXv1AG_%&s9oD*G&?e)G%Q=dM z+$CQay2BYEcnxsW(DlKL!-%z(S3;Gs8;iA?ZXSAJ!*?w7gwX}Ql2ZX z>!gXr?eUP*V5QtkY8sXLsipL*G<~StuZ(+ceieh}L#a_~OuY!6ZpL+~Tvw_39zlPK z?rdUtQhiBlivGw>W&@SZlR}7k&?()NjZ&1TT_)zlcpGEh!YGKnVJa}2XBwk*tn9&` z;Oy^?F{6w&k|^;@Z@SIPR);akC>=Ac3ckC@Pc% zNA)K~5?H|aZT`!`sWP_345pG)kwr&Dp19AVd2~3F?F!NbxN-PB+xg=A3c{@9(p7!G z94J#F6`M+=J9`RZl|{Or7Y}7kDUC$BZA?Q`B4gj z!YFSQwvbYITCjKtq(Yr?rmR?cIvKr7mWc*4XN2&OI?2}mReZ&+ti{NpGb7kQ`kfls<}kde5?IQ zCz=oiGEO8nD&WO`bSdul6m^y4mHrgC4q?`J9H6V$!VVL!R2IKs{UOD(WX0pXj|X{0 zoq>Sh$(vKZc!H>3NiCd)qVqwhz1!jB@KXx(FNZbM4wL*$CaSr1#WEs*Q}PVSMXS zl8)F9F?me!8LVeDeLbkaTa%#nI}wm$F@toMv}DEh(td>gemN;wc`!!P8Vsn;!c_2@bAiRJTg#5nW*;ppNT)KET<8Wt0cumH_3N30F&M;5 ze7zq~1_=*?_O@MrlMdmFMDorT{tEu%_0Kq>f&qbYtGmE@EJGH!zEWKQM_1gLVY-%E z9GoCpC$-g}E}(?K1ZpuWXsE7MGYY@hnlW|L;C-pO{T@MjybT?R}a^O@OxUt!n7|zAwFTMNv)Wi7dCNzghSn_03 z@=u0r6K$~Jaiq6Ltqr!0Bu{LMqhZnbu`7MMBZDb}bB}T?Z^-qOh|IzSFYSKyZ;|fH zq~zu5U(v(RI8qg{R8R0RWnfmh?BAEgS@nOUbn1A_t$6=dkt@jYcF3hU#uH3Uu2au9 zhhCt>5DHWVgraa!7tMocVl8rG*Xn-)$i;9clxJ&l*AxZmQJO=VOLCKbDskSmO6MwG z5^g0c*F?yjfJV9YPHkVwMHQr_KrN17iB{Up4MFH@D#a&$M)s$Bl#oJFti}5t?bm^v zxynn-`H%+WxmjT`E#A1DX-Zv@u0z%4)<(K-brIR`;W1b8a9WE_u5-9}<|e4~ZXNhL z+ULju`8a;V@fj8gT~<)a!@UciDQ#NWa%#?EYNCDeWrhw7a=~2ER(twK#Tes96Zxr8 zrCTz*a>(cs7#GSi=}>r}jmSVYOpUWg%dHf6K}u3_;4#$wpkW+Drq}Pf?DI{h+vn1~ z0hhzJQy%^*subVwBb11;6)^)FL6uXnUM)=rz->J+eY%PCdbb_&7m+iC3{fP|i7!Ab zBaQL5@_cmWni{%zQ!mdB0^^(|F&07EbCUei=Ke%}Dwe0#Z$cd`6$_%WE=#Hi8n%vzpuzB|{bdrt2|%Es1;A{l3`)=(s(Y!S`>NIHu-N{XNMnLd zg_Eo;EvOR|2z9Av{w2N-XH+2)Vzf{m9)@hRFd9gtJ{Vnd5x$i`53!A7-_jQ_%#=); z*;9NlZYM(lDW?Fz2I$NquKCL#&xPq_+Tm34+u(`xfjI-g%c&q+aLsk4bMO&XFA~vV z{mmhr$~XmjI|%k8^Vcs2w8%T15bwhTbn=vuq@k0oL(RCj$`?&P>4r5nxZ9%0u}!X4 zvnCvy`fuS(w&b0X>?Mg74&w)7R&v4kS$M*^e%@J+6T^%X1>f=)R|Bl)bMtbs<78Q01T2hvsNfGMiEf98bQ|*j*CjuB+1|y<$P}kOk9)Rf4jCZOc7vcVX`NhJ( z1NQr|Y^yGD)3nlPw&pMbethJJiLj4V=n%fNk*mF}$KWOfAEvFlZqWBmGQPCnzGg*= z!!2eXksz~wHkrW6c>$NoEN6uw!dO&1@&!NUhC^pXalNV4f*wwS@UXiT!PT!gCAgw6 zQ+1+uQ&N@qoio21E>S9}`t>I7*<^DHA(_uZ0Ou}Kft zm(mX_@Z+K9Wc*p`s2?;NETVqP0$l4!UeiM!>p+q_3h7WjYID8qn;>)vD_KWprEz0>YsYzxJp6VM+L%uJVu8q z6ry$PTYm9(4R_GJ$G=P?Z0VsH^?f*8?n|Dst{%KLe*jBuPT&uGS>;@Wp!pa6B?!m-*}&87wM!a_db zgZ&y%bD=AV+O%o*+x-g=DV5opW43~ng9%ez? zG##q>F=l(<-z3JE?>VMkHpju>nWi7vLugy#(VG0s)G`dST3mPXj{0YHC`;D z$(^=s9WNeOv<}Dl1IQ&2676p#8N|S{W>qK)sGE*lwb#@ODAB5hAe=#u|A;_C0%1b8 zk79Tm>$31 zVcS83kKWbEi^6=i0j*Dk! zOU-&O5YXEzbJaKh`i)6dlL5U237BP=x$7YAGzvAiIg%XeGu6OJznBNxTfRVC4O6M$ z+bJgN$!fd&SfCkX0ET2^wXK5zoFmtSPQilnujM7XY4BQ)8*1P>rGqhAQzy-{+X?NM z*ae)kNfWbnMYvwvc&z5~a#kbpx;sa`#^X}{3~lR%;xKvLpU?!}>$LC@CrTu7wD$RV z@(C85$flcWEiCCBQi5O^v~ie$?LB+rAsXiC$K;AbV3k_*Ge40A4HjoB+DPj$$RR^9 z)JYvs{WuN~&G^s5yyThT(IDh|gUT^6GRaSea_YW9=aAL5h0P1kjkj|-8#^GDaZDyo zyKqO;9q5GBW6ccAem|+y$MnJ#?+)2Z`h?X6grLqjId=kyB)ir{+!IHXEU*V=7W=4| zIx2aDa%IpL#3ID{S&7Dd^I*Bn(1h)z*c9~!)eZlsl0e+`fkC+V4K-*A;O>;|N6)-A6qez4H)^*mn8sd zM`zBjwqJwaG5+D-Gos|ox^F2fqr%u@6bV3D@$;Zw$1qQlNY3%^u9rCp?!NUi*JSj3 z@U_H(Pq>1G#^r_EboC=XVLZYL;pkXOwK<0D?N=!l^frVuZ$?O>VRwrXx7A;xq}gFZ z;{U$wTebah8t~42T}B}o7VP|?sb#9^8ZQSm3=kxdmYal9S!wzt%k)=4_1h#L0oB&! zb0JlS0aaX@#e#!u0v+{A3+()P71vd?AW}lUgbRd`Q)xa7*UboI*-X%3VhOW9aO9Lr zEZAYU`hUxwVZ~53N&HMN6hyV0UlvB2ua%J1y7GV&Q^}#DC8Ta(<%+&!HJaX2U68@4 zvD*J(D2m=xzau!88X~)WBnA`s9<01D1-S)BB(fR!#E$v^w8#IA?dKNam982naa;2> zyEv^JP;u!`87|)$ZXMB$LNYkp#;A%iq(b z1JAfzShdK49Fk^o1uJdd^!M!I(!yWfX>%#OA^h~+%@y9@y;31HXc|J1GPa-5k_Y9C zL%}dVY+LfBJ#~tVx0sXaTc_lhc~7dXYql6BLnh8{(X8L*l$lF7t$x^#S4tCMQA(a7 ztIE&O5^L9oM7;@%s+mE`osm^QJ6~=ZyE6^Q6MV*;B?p9z8$*%J4yjxzm6wZ57geKm zZ?5<{sERgYhaC+#MIC{+)O8b`R|{2@tZ}nbVlU$}&z|`7 zedBoOf8bp#d|A1Vu#}F8cd9CB&J#*6Ipx&3Nxz((qu^gQCOAlm4Lg#xCE4ZKZ&T6| zhT%$_IQ<<{(GmDlruvk>wsMz678c9{7l`t5a@DEB{Vl6zh~9TNjBKqL&vo z$*OQWDD59?E|ipzhx0a8RlDUWEl)^ER$LteJYcfNy1=k@d}B97YG>%2*;f>Y#j(&{rx}V$I2e z^!HY{hZGY;XjeC>f6=T&Q|T=96+Cb2veZcw#BcLpyr+e~a6)Qql~T(^cj2o(zoqkD zWqG@zM{s=jvyZEO_s3L>y|*?3Uu~I9E440v)^M_CWg62)fg%ae8^0=uz~g`;bxM2kPiq3cQ0DBo>uuDFw=3;!xz{JUkn z!m&Grz{jB&r}zh3n*0EgXVAe&J?e=uXiO^Oj3kn`>1~WBX^(Tl@+7z_o%hKKUhQBE zii#S@ki`JM(oh$;<$XxZh0|UDSyV1_K?F;pyR+hHHvzGOto$1PvtvtIyoe7s^!=82BSk1@Yn&MYccSKXL^V8}c>L}7et9(A{5@sDWX7{k7T-559r{|225<=TzA6!)Oi^GJ@a z#U5?kWX!92x$L64R-pWA_o%M19=zmQ4&K>e$m)KRFGTX1#Sw={==Vl`6TYD<@ksIt zn?J65I=RXdhNAE7b-%o3S8K(+^fhNj;jtBWbtgWC?Mq8VV%ZB$PHD)una|=CR!UB! zVK(rWz{2JA$zsSG%F13NL&vqQN2h~!RhRKu)QMGfs;XZMl3;^o(bw+K{?Jt9FJ@(I zSWuHYBbUd;c=%%axc*>QNNaV(n;~cB@_GS04|#I~$2V|Nec5TeqI%9S&sHIO(xf~d zpj4takLS#6ZE3W4ABwTU8+f_^jkgza21S;CaFULlqYm+AisC#1!#Wn0=_7wA!acuMtUi5=K3j7Cc30DiK z+vFCc>;c|I-qvwzXgQgjj~3kN4xbUF2;QJt`)L*)QFYBx*_aBs7=tT6@g)DiF&TKi zJ|1kDbq4vAl0@%6>#P1zWrbWw<)iZ-D@?+Lr zN)J@LrDkE=Pj0`zg_TrBQBG9;&Jyodjd`|7-z@!J>6+}$;$PVEhq6$uzP04qusDYY zg5?85LQ+kMg3ml=#?w?dkNRqV?h`<2ibEBJtUB zMZM>>6ux`M=VdOs9AnNot@y2S3;odesde@#LF^(9HKzdh_7NUPD1Rktz*|_g@BQ21 zeOjz&Xp0V{n^RNoXIkJj2}=ByU6IEL368QiPT8G-HcI`5kb`mg6WfYMufT5p-L-2T zk)F~Kh-J;GhsTjy`$&qLlB?$m41o7^HYZ$BL#8v0xo^Vu=%}2^|2Zk|?+`{wcI?Wt zx=iHuOsdhe*qq8{EjPhBxgi?cv7 z;g#<=HsXvf*9ejNN!EyL@bd$okZPxyfJGBnY`wA1DFeZN$p5p8yzEY}*KC(y?`gkZwKFc$^* zjFVW4z|{w#ge)EXgMV4 zL4KBEqnw++V$N%)W~aj-2*^B53mc$RFPijuw;Y8^XeO-9`oP7-^qJz8kyh1%o+yMX zQqa!aHmk&$5NVg|C|ZbaWi8V0MNz}-HQfABN<(;o%ANRu9v-keLR|^)BLyEKS06rg z>O@-(Fd+SRF95c|GDOpK8r(9;8Lm*RMc-be$&x*@=jrSr89l_PkTAajDV$d>h^PqX zqeX+sD~AtCrUgH0GBiV}=35F|)B49&1?`$=mC?Rl*2SqE-C=!2^52AL8^mj;`0Zb% z9g#*)U3qj4D3D8}-AT)$s50G8oAP?IIQ!pz@jfJd#!+9pP2PS_*!M==d`dt$X&qtJ z=;Vn(K3xVaf!;M@@|{Z=Xf41x?ZWHzgCnQ&Rv;+ahoSP;?PHS6jo<|R(o> zm-_OR)lvI$TLqH!BfKx)301?Q2eBl+zJ=;YoxFO#g338p-ONwIH`ch^_0s|Sk!gmS zvCgGdGe6#D4V!%}NReTKTQRtUwv%)08hz?%iF#~x8`IpV--0C9#$4dWV$cv`xh(Kb zg{`NfRvc$fnf4DnzkS-^e(jk25oQ;bdoL{PdS6C!OnNP!U9!>Qx-g(~C;}TTui014 zG406}>a6p!DFf(wZ@f=ca3!yB`{QJJB+s2kMiHr7dWCEQRJ}vD<=G+=G~ZR(KIQJ9 zOgA!{Y_{U2?bG7hgGGdR_c-NOfJOGmG46kpuaMC=7My1HbxdIP-*AO%`MV;YiS_<6 z`nA}K3={`wuKb-&H?MmaQw2z3)WlQ7VCYk|GV6z?>n&T{?7)K_b>jbPXEge z^nc4uQg&wc|52P8>S{)(l&EDGXs0J<8q`=9*%Xx}r(~uT#wKN2)My!`7Mc|6TMp`f zPtV9q$Rf)#+)c_%v>Yfa{tTmrAje92p*SvHr#-_qKTbY2IX1nZ8Zbe8^uwa2;igP6 zNd{F*#7HWpT?0HOV^Ji7*f!0D;zKq{5>HWr{7-$%uQw{~Kjv^pa1apU|DNza-;#rc zrLm!nl%12QqnV+x>3_10QqffiWXAB`sUeA~K{qqn2RBw4NgT8_86Z~N;9FirroRgK z@_UdJcb5s_)nZQI6QS7OlF|gdU(>{`WH&UN5*WH& zN>`b$?`PxEMmqTBpXu01Ilp{Oo$9T1I6N{(H%MUK{3uW;9j2o4GnPYyP75}IRV&a7 zerg*U2l^vVJPH|5q`p8nI(W^r%967o0RKWFTHz%6W62t^MM!b*(<%*OqG(tAqV-Rk z6oluB(l{g6!6G-+S&~^Ax5qMB6}BG{uN+PqXbq{x2^G??ttgY3KORFmaUaeTdbI8a z{hz@6_Vnu6{|5-$KX3^CJ21BPCZ;x`mc~y1Jkf@Z?*BnWZgxsOh#3QNS3qHI)B7x@ zC=r&K4;`D5au~XGmNho-r-jnV)es|+eR)E>MWI6ea#gDdPFX%sN z>sAR{aar!^`PzIoTQQF*zAUPmIEIL}5uHWSQZg(Ph-z6sqs5GCfA?M|ttuH4`(M#5 zj$!}Lv&kZ=`1tkjOnm+Y&3``|2ODQ|OFJNw=s(|`ojuUW(pbgW!NJiK2()+n|J8J3 zva&N1OpIf)NQ?~B)Y1%$(@gdMdao8%@m6M1u1T%G_~#g-(2TecNU1JyhtL1kRP5O^ z*P8$G^86Rn|8M!p+M8P%1MCcK+&xSk75^*g|3B&7jVa8?x=j8bot$-49ND_Xfk3bT z!5tb27Bsk92p-(sf#d zIo0*;vn7Y}izMg}`lzKxhbn(jM_Otan`)d%3^7#{s>BYa4m!yEg)B&w0cZ#OPThNJ zdn3F5hpIsaaXRFGS0jX`QZYDMIgSEBGb1X81g0kRV`YX6NQEwF|78IBt1lI#goXd_ zV&JcA9Dk<;?)Cik&ipPcMD!1`LKrYDG-|IWwLv< z@2nlHKp`5SYw-V}O-35w{Ro2?UC5_FhG9u@T1Xm{e{Z&qVyXW<#Z033_$tAq?w&Lq zW`c$WC0GaubdXdHVQbJhZDp^c=nRsl^~c%N9j#-&$cWM18Zf^9ZTm6|;lhqc+wzNW7CEd@|rGb);DtZ)pc z)ckb&$m~oTBdVQU*SlBUBJ8aVXX1(_seF59irM%!0l%1dfiSG1@3$0gOEKg(43ROY zQ+O-`wMv)o3)Yx#dHTKm;-s-bW1t<=>NM=WIu848=4=M=qHkNPO^*&mN+4*4s*g76 zy5;6|&la^%{FoK%RJ@qA>dA{XEv|&NmStfvbDI9T6}(n@ErV+wKUNKo?*{L4+Oe~h zN!H0Jj&dwcsISrFTVQ(WC-i)uzv&aKBXHQ}FaLbJBXe*DSKtavT{@)=MPUqSV(ABp z+|<;(D`h-x|17>mm9HJ@c)0ix+9oXd#WcKrFP`hLT|SJDU;o`g^{I}n zk%b)t{U4v%Q93dmfApoqt1fz(x6+Q@GZ|Y+<7pv(pC=SQ( zjrHY6aZRvfUCaSva;_Rz+mG$;FLlc1a?R;FdDS5=FE0wite%TKba6zA7&xfzJJ}^) zp|MnHr`^Hq4qQ4sqT9iJI}_K^=f-Yogl-J2VgqiBHuHWu*K#Hs5ww3XN0Mf4#$10s+&5rX3MALD~snR@V~acT3@onGPTUEFa#Z@PIp7q`{f+FG$2y&1tj)jY>#f+U25h9Xgx zQ=WqCJz7NJBLdR17XcBFpPJ=7_MUqRa9i=K>Nurclf0x6!W41VD!n+Im&V>S@@;&g zspZyWf2Z|{Me)Pj-C(bX5KLu`9nI4EX1lEyYQjfmknvDa?nBnVE3BYtj0h}>S(eb( zjHte0YleWV!T4Ya0vdqtYvZ5=-ue7GKayVH4N>Jmt^3lu%uwRY@zzhoyYt)`Dr7aU zVV7EBe2ZO}N%^Ly?9YVb4x`KBPzC1@fE;yj@;p#(A4n18VesDL2%Xduw1_AJ_GIdx z;vtv9KZ+$~HgdxtBZm|xClB1ss2N{8w!9=HB!qd|3LuSflrW=7o|<0;PI?a|UTp4f z8jQBTeM~uM%MgrO{INC^)F1PT`m5D|5atn;OKW<%ph-z}zCWD){%+hyPC+p-;An1q z;p5h>Sa++GXo`!L&10XjiQB!^eJMAIrKfJwD~D5cIYT}~g|_WZKJ)cwq%m2;rvoZ6 zF>_8Nsu#Nl4O|7-4B8mcQM)Xs3$JxeMGp?^UBsFAEb~P&$L9?Wo(CyTO)a>%)NS9& z85p>jAECj)e1I|{PhTQ#RL>j~`7zVxv5);3=~(3~DK#~57EcX;w8M+vuOFrPG(32I zcIKG)hHn6gJld}lci@81H<^lhlAcI0-O5e1YxQu<+QRBXXwm1wLd3;+8)X$0_Un^0 z3@j`@!hp-!W&QMyZV>^1j{CI|z%x$Eo-~ySdSbVJ*z!S*9gbTxZ&;I&pS8u+l%cTf zL~R+ZLF+NJCaHuqr@JÃ>;1AHq`wA%O4a&&1%Ifp@~WlbV*orbgx$x2!o`@MaK z$?BLL!J7l|@GdH!qSD3YbQv_C^n^E}n>XdcshZC2A~MqBN^xnb-;@)YP!OO+II3+Jt(u>W*Yz1`OoJ%SlTQ?;r0Nf7_d>jK1st z0mC{JnWQLp)0Mz*T-GuEyn8_@)>`^W`I9Q2+fBLK5K&Pa=LF-3+RU-$S2%_&lWJE* zSwDn8&SBLg?Q`0flfaOyrX~Uv^a z9;GGf&*#N=>Dk{ER)znF?y80n(Fiyf2?{N#WOI;|hOD$li zCcZ;sU3%Kp->^tKMW%VM*JVmIJytIx4QpBfGwHvO=GKwg z<8dVgW|LU&9)B>wd%;&#Q?ACrL3VPpYaV@jR11ZL9Zv%Vd&aH6b~Q@;(DL|ru%JG$ z=YoB)OXUO4&zc6M_H`q%bvf0W->X$b*{8pMt`B{!Ff0)!(1hRhDoBbIRzfgA;Vr4F zI0DjW^~C51GWj7iJ{>8AavyhC;*KT6jfEshyfXG4nNQ%F;Wdic0`nUk1f;w{w-dFS zQ2D-mx$}o*s%%)|t6g7hY-o=GNa)sP_)qY@sfb>0ojB0Y(A3EGt(8ABkG)(cOi600 zsA%wJA@_<4%u)No4oCw8*TXG$XkeHIp-p`Ff~v{O#m2_YjXzi!uD6afY@dui(&pI_ zyGn5a7s(a6XbzW?=W0nZZZp!;(?gQrBlF@#UVC?$_m*#C)^(0w^2&r@C-%~-E3P9` zHSuD%btZu&I1YDg-Fw_lcm+2@YwOUud7d*hZm!WroOtPrjww!QN|ZQU9P;1*LYJ`P(-{ID`>~b_K#|+j~D~`irALV;8m(0XZFb;wLKmq2qcIMP6bHvDgePp z={>dLy~kF^<@}qGJm+($RLSf4X^u`VXr05uglaq6)RSb;&pI&GbQVoIWY9C;$}oqs zwJah&Wdafb-+9hAdqV|gj7Hp4IPlR;UsIeP+P4k+-~u3X-)iWIz&ttbyq==Oa#3-q z@`;RmH~oW`2*GPxvcILRN&W%|bfQPNbi&-3-CudjhxhK?um)1PHia);gR4u)wMjEx$x$^GY`ac?bqxhkfw{>&yR^}eB+Q5Ns!xw}64i!ei0LWj3hL@y&|_Z# zIx|z*LF$)B_&8L&A;|?b<9IE{E>-EdCHdm7s!4<%lu_!LXOnX7cD+dl7p@$ix}=!y z&RS(ZSk3j1So8ru?6%R!d<`^vO`#mmYJ9j}fwHu)z*Q1REIS7ErLD7*EX2R}pr|w< z@(rx3lM@FAQ#gXxsjJ*Z^OTAT|`>f{qUAjN~hy%Ic zRk=W_7a$~CHelp3fA+i^d&7VQesHif(?GxO?dNs;hB5NoTuK7Io~G64mXM}XiP}R%N9Q9+I+6B&8x6oW4SpPTD-os zxZG*0@PB?ys25jN#p00%g+xfmsZCH}G9OOxgxq2Dj?TYzSi(flxaL&ZS?cxpyi5m- z&sQ&37e&bX*$}sHX=5To!__zfD5hUG)=n55U;Acxa6KQ!5E$M1B$hMc5t``No8fYn z*vawKRYQrB8ku;PHntw=FIDC3-3gn$l)7ONy=h8B`5A&OYh>2;E~J2ef3ZY0+7uXS$)p1Fz467&g^zwUMC}&+mgfm+QHLj4bh<95~h0)7ap$Q-m2$=Oq{7-Q9VYeDJpplO8~Qe z?jF^CX4&A_@L@dzKWZtcL3N`RgABgw{!oK3LEXhmwo5qia_}K*2Dui}G(^Xh#^1By z`Ewsqb_<6PnxXiP0*BGNyIT+1rg$Cv0i^7Sg1wiHd3T;QZh6B8b}S#t*f-fAY@u2@ z%g?cPhVtbmJy1sRqu_r$4~!mFe@~K#0oc&lAI9w0qH0cxMAz{#wcA4DiV-c%&Y`I? zHXU6Wmun7QIwmBV7sRhVjUMek;QoSUCx|Bq4-chkNw-?$&Bl&3-~eBmWzTtDmAiZUP@ONe@nAgtocWVxVvimYDZ z>h~{Oxvf?An^#20eVS43l9SdXs;%dE4AxOkBWO8oqoWKFkm-m)#qmX}xuu2)5oqV?DR!sZRxf5Evf|#A%Idz;tUU!JcUZ%7h zrNAV5QH6_7Nu%$?nP96K^A#46HsF65k zMHZZRenX=e+kSgH7xoi4Q>oAoIRO=AWe~Wv!*Wq!Kz<0@G7{l7H{M!~Nr?%x2rT2wReAk_`_hCtzm-S7Z4p-x?+KR%UX=OyoGW`rz$=&4QL^EscY{ zy`hO=*4hrGWk>48VIdoXl560q2-L9|q>-ZW5))-V;X#XqqX?Ng!P(HLI64imMV|n7 z8ChGK1wa8=UQ=EDFn<3C%>hlV3v}eAaJr<`!{6!DtI$@nv9al>NNn~NuKtc|zFSpi z^fFoA5bluDXoh1n6=TRW-CT#tvZ%PVSx8u}d0LYg=hityj@S*eIs*PgNE>f=$3H(i zv8?r*fAIENVjMpQeo~SYYQuT@9XvooU7gCx)aK|s)^$4>#>Yq1D7rWbIz5RI3bCmv z0D(&NLBFcE`_z$KgvGFS4+6m`tWJS*B=V@;dIl}n4qL*r0-+hFziC86vYYGK(QHQx zzJ%BV0rF^`+Qxmh_Qp%3g}_aVXW6FNo^;JMGCstWdPc4=P2r?IDxnG*rL-x+g|Lt1 z0Yrct@5~6l#moX}1fAt~GoK2ne7Uv7pBMR7BzL?_aHYwK=%#%tecpVABD+mHyGjdC)|QGJ33F3n)QB8=kW#TSFfe%Bf`X<*@WTWS z##*MCe7n_iY5005Xl8F?gR5O>+%KgA5BYwWZlT83qw0nb&TIylGlyzDi+_4iP(W}o zVHg~2M^gS9gXGPwR4@nf4k@x!Sqm8g`~beU=FN zH`u+iEcpHmGGp!*n|0ZSmI}@+{Gly#p;b-k`WWtDHYqlM_)hCRIZUl ztMc%>C*i*8U6|Co;G^!DvyOec&>E3IK*tYfj)uF&)b$K;a6Q!T)v){+$u$ODXbIDZ*qG8 zFP;_2+vF`mj)+T1X^If4IWjfw6MGaE5D~f51k!1{#zZy^V~l+qh!rdbARg*#Z@Zyvp5~VpN)@aLL%m;&7hr*ivR_VdfnKMbW?-)W_MQEPh=bMPb(MVV(}GXS$K0Yl zQ&~B=OV!ZI%mzQNgF?#9i*SmZ9RFCB8`?J|P$fMfdEe^yi5F82Lox~t$Zcvmx=7*M z6MPQ!aaMtH^#$TpuM+d*YJD4vY(e)km_@nmPZFf+d+ph+v<#Res#g&c6WIQ6H4j$) za)sIbYxEW+RPL^j9_IBL4ACproTe*UKwOF4YMl1)Huuy)OMxF>`kZSZ*&kIpR)l4F zyPuV~Z;yA2h%5(xUht2?Hn!+k^cKibKLse^yu*tQY<&nkcKm!u;oQIF26w~?3oH}z(c<(!}>=gud8A@ZCT8> zvE}eZOAt}3P0k)PfsTY#rNyROiBp&zm1n2v&x*qO$(^wV=inpM>@^a3O20sa?%G{? zI3kCjx})h*_cr1%Lvzj>yv{gU#f>=4$9dZKmLd=nHrOiN@G{hsF zyZO>{#`?{C+T^MgvDGW3-aW3C?9Y$ex7o{8h}qF}1yzektF8DEtL!peByHAAF)=xT zBye2I1w=RmxcP8bC6@B=P$efN1q=*uQBar&U_R3xNpQ@v{(yyOePNj6 z$CppQAT_GGwFg^G?If=GrKHGx1L6rISGvy$8IMugbp(;yC z=z}m?HKx+cFV|}5f`WsW*^JU!Tw`IMNiyl%W%1g5ek7mK?UmAkblwv~t?~aV8hiP% z(|(}7KYiV@Wb3IqK+KRJ#}5=EGjJIh;p{BhSMmJAw!~05ZMzS#2P!g4?{RlqEsh^P z)9B^XPKP93rA$IRcLJ?YKJ$!@=7YfrhL(Eb+kers{QZ)n=jH2HwDf&#o)Yymyj&*J zi&@>*+aDdSZ{bdaD(9>fH_FzR+FBV6g`2d(2kx#TYSmr>wTZl&k@*CHEqzAV^W5Fg z*OYjkqDzW+N!J|j2m%mdo|~#w3uM`p#A;qakHaxZ1-Q`HTKljNAPD54F+a`((f<5o{sO7dp_*Hh1T0%LkE$5~%_2m~Bnm55t-{C!s_pCnh6l zlyB!=;${m1*pUY-m9)}?zp}2}eI`B+y2=-!v264HdPqQcN56uZOaAJu2G@p(E>q?F z7stS%kKesWpkY%b?n`;E?^|JMmo%NQ{LtBA0VHeLXNn5tLhU@jC$KUFyI+&c9}(A3 zpg3gme^ylfe73C*7Lxg|XWh+Ha+07Ba@dnn`7~bE}`YD}3vRB1L6p*U*1`x}8#g99RYF076h_y!!W! z_&0mPzk0$bwB4+e@rAEW_ihUnVJ4`6RNG!+eh1MbM;NKf7BgFHt&=R#8FK5>BJ=bS zC|Ou9A00Q}QL^9u@{UIY8EYhWH1-qY1c8jzuoRXRxi0eNiFBd43z;3QK1PSWo>j3M zARv{RbI+D!X86_YRze7?u8oI{=SlVPwQ4GEnNdP%zt^an|GJoTD_1>U93*O&B{xa5pbHE)gKRJYa z|KR-I?*a}3cV+y9)$soV_8X4|I0@Xt@RKwl@DI{omFNHDXaL88TMvF>Z3TaW{SQV2 za4NWE;3xH^&~K<<1_E#zc**}KE#-H#Ukw1@Fz}lFPZ*8xZ>0VA3O+a-yvqI)J`IBZ zrsfU~059qO1muhSQs4zY5WE`s$AN!tl%oF_B7Uh&f-}G;-#?E3bHsAf{72LLm#G+> v2tJGaB*K7v6Mqjfe-i(`y#Kl~|23z`O2Rz<<1lp4D-Cp4aQ}zl0^)xF_w)Q$ diff --git a/examples/acados_matlab_octave/getting_started/simulink_model_closed_loop.slx b/examples/acados_matlab_octave/getting_started/simulink_model_closed_loop.slx index c09449e59b3040c176f436537f08157c454d68f9..992e11991d0c1116bed4a066f0f549565f8554bd 100644 GIT binary patch literal 25071 zcmaHyW2|Vw5~YuA+dlW$wr$(CZCm%)wr$(CZF}yUNhb4VlKIiy$^O@0Rl0VsRV^q0{|fZ`=TXeXX|WY>#V2jVQ=E3L+fs1-JCQoJ4lZZ zdg}u?4Tr6lA#U6XA*Te3}po2Y~ic#R$ za7ys&`OAVM0pzwNrz+bMo zRMr@1Fr=7#RJW3PDbyd6u!QtvLJ{#Lk+5hqEC4y|_jI1gph{FLm|k`=M~ z?=C5yLN541fB*gXAe%7e7zS|PA-D=8sARO z_T{BaFsXcfFn+DDdt3`==no-aBzOThO$X{;1QbIrm@=aH^8dv*FP?&z_#fZwe|(|; z@zrxQv38=P{m)#SH2EK{BCqdIlb<6J{1Evh+W?Ag5Fzk?$%G7t>DFm&e(}(Sr6jRL zOe}P!(+evddbT74c;`54R0I~;^FdUqv1>L0dsJqlyd<;0m;L9UpgY9G#v7s^)KK;Y z<1gDBNaT~L%}={M^rx0_CGG23%LE^4szLB`T1>z$@HCAz61NyWGX*BY;{j8vG6Ge% zMQc#)1tdUQMD*k%@UZUszU0ubmqWen?)>1c=rS4{YMbX=e#i2g3FIteCu8<1%=8={ zaYqe)K|A-BY?`liw5`Tt7c~;e%uIuP(6g>8B*ZKeH#N1q>+_*5IXc#POA8jRq|L;$ z5GG6^c&@oC20?X=T)8Yb@@6b)<^=H8RDx!G-|C{AwH1g`;KZXMT zc#`~Eoh)oztSxM<=>8W}Lu)%DD`N`-Ge-lP|K#oeglrQxX$i~#Bl7x)D*72qR-CMO zFismU!bbWHu&-dv#Nc;4K+_NDG^$KiqM*N7bq8^nJ(k3XHp|m-oJH}{tXX}m$`~`Z z^c?k)2KYpv99A_O4YozmVMm_5c;s0yR3N)t(>AW5e)aiF^G&vdx@}im4O1qTg4Y*3 zcO|aN{*1LVts0j1Y?v3Dt@CvM|4JyiUG6dSe*h+d006N6L9j7#HZV4DHlQ=Ib2O26 zw6iyHbha>Y`VRn~L_N6ydX(YacND4J))wq=)9oJ3CIEGNQErm-cyNVd6lbxdr3)9qGAAOT3+eK977R1q@hZXOOT!O8lY* z;{|nfO{KtRjGmm1~=hq6Ed&KhfTB?0Wt=ZJbte*3*mdZN=Lsx`xg;49->}#6FMQ8vT}IZJ4&D5 zyc^`OYR9E-HurB^+$>WrJk*fk@PT=l33qi2FJmd=dE)o&%EUGRWHic-Xv(W~N?$dP z8C!f+C*h+cWQ6&In6+o|q(-dLJ4)UTJnH z;h$}N5LJ%tO|KZbQ{sJ$5Q^&5!bbT+a**f^GDK5_nZfZ+Nbf7cJh!n>EH?&OFUg>b zzOi^L%+BF=!lDUGP6?JVuDNS6qo-BAD#WW(AT~%r!yK>eEA`?01x$ruSct_hQFdyK z{biMh#M252CxKpbqO)Ixq%MH}-fdxC0In`A0018w007$mxm#y*7aK!c0}E?fdt0+> zFK{&_oz@#L6k$+Z=sa-`6>DXYNPiR&e&NWbIRynz@+Vs%0bzj%Mc{=><^*}vdct5d zd4k{_u3)5%?9JED9;aFOw!>Ji8-uaA;OUtSE8a&+E1Fz$tBQuJlZ_Yxh^UQaYEB5j z)?nXT!(cZoP6~rN^u|dQVmAf(=hii}Oq^Z^+ zye|>Ar-bq4%H6Xw+HI~FsrkyUi!_kF7SUqQ5YoaC=(Gs9!DIa&vHs)JI7TtHEldpqByPNh!y~sJ^WWjV=`*g$2*)9_+6X?c zh=82_?Zq4~1Q4rH#39H-rdVZTLCXYOMu>qoHzr(1G)M!Hz;jmvMB`eb9KiO7xkmzs zK|Lb~M~XK;+V5PcUwBuHPORE=K)-O4Z53Z#i5YMNL=$e{8Ly-b3934IaYskJ zbi&1px0a6$oO7!%H-pcY*Vos@kDJDnHn5vwxZQ{}NpPghXW5 zv^ejoU+X;P$;44@2*-N2wrA_G-hayqADTOf52B)?0y5OAUfzuB5NhSVgy*CtoKM_C zz{8i?{z015PC@*I5Fr!1Tl*^$V4f{epE8071FZX2%OEoIPA8T%TH=c?7T%^7Ut3;2 zz35B_#s?iziKs2K(e3r5f(;FeNCdjRUJWXc`BGI30~u!T;cTjgf{I#*eY4%U%92bQ z>s%kgjqFgO3_d3g8bB_1`C;brMLr>hylL4|Rv^I6=^Z%9mP0q9(o@_HmV}fYSWu8% zZE;a-JGhF4Y&_`>f`&Y~qBFtg*K_dJDc)|{tlKnH{M#^c*_f6!LDi#leSNj@C_TZc zds9PtGk|AVM~%wvV94&eP~W;B9(kaLLu{GQTVH>#%lv9j%JDQA6b3)KDw@O@b;Fvx z){zV_-t~k(ZQ>Jd6SS;9G6CopdEMOuV71c^dTS5_LhtUAi`%4rv z{IQ2eTA@Q8MT9u|3(bZus8R1MaJii<=|l+`p%%UH20WS}By79FW5_@!?>8N9PtX(= z3kX(mt}VhsA-lxF{I#s6+}sr0+;=_HIK}js<)iEOIWnE;E^)fERYPk;Oc<+1r|0WL zl}RFf()#SvC71m>Vb)9k#B+7N_(F#$MuO8hRNSuNQM6stXLiQPas2v0XSw9}&d89H z(9}gF_!^Lqb9%LSa~|vXs=$Mc+}}2mglS`D4?8GItd8k}oRn2P?IOUykWJN_f|0mg z8ESwlGi}x*Wi*bA?24C!i`8noeF15JdAJ)=%_AiycFL8I!f>SOTRIm%nX7%0<*Z1p zK+(lpY#5q1GcT`qMWWfAK{SMD`~3Jk-(b6c_hDqRy=c`Hsi$%e6W6c~wq(VOY+PnA z*v8+}`*T5!ekEH-A6@M!*YMSOY8mJGeC=0>wLY(6iO+9Nf?%AOwKjs2=iRMD%$LKv z9pwt1*46up_}$s{#Es8eI|DBXV(Sj<54U|?32RYh>QLXc2Oz!dLuu`qo zTMW5N^>|_j$aq8)wPg|=4gD4k3-4&}41-9WZMgN!a=dtWWW@8N$EnzwCM{*>OB_Gg zYM+lbS6g3K#JsXfNwc>iUmyckJ3+Plm73hSYfrox*9C2b!KJMGmT!UFiw1$jfG zG{)gtn_p}YF42Kl1~RG9aO4?&FX!&z=y^4=c5+9NRkUd8ku!ilWE#TrfN%Jix5w^hVsH4t5=4oG^{Ju@}%XF8LO+A@`gbDELY@8=5B zI8+_i%2r}iTYAG+!gWAhLt~f&jY&?>Kpuz!f-6f;p}V_@?E5Hb%v6T1+K`Y_S?jfu+`VyjQwA%@t>YCOLSnm4~e zJVN;j0BA82gh}Y)tZuVNft-RxfZk3HF6&|=InN8XrY!O?ShVqysvh6lCc1QHSwhysXGh+2ZtueE?ynde}% z)owb4H!5X>x)+MZL>t^SxUS{~+F*Fs-(o>U)pzZZi?S-NUmZpF>(g%7uySO@jxIK7 zr=sCI=S6wWXykFA*m7ps>YQPK?0hzt4?UWa4RxI9PnWm$^LpZfj=ecFdGn<+%lUdnUULXYpaMZxUYehE1Yq!-|=OZ(g}7)SAh zumK@qccmKsClJENhi%T@DZW<-gWo*QbucV}a|m~5?O4-l!_u3Iva;30855pHP~b#J zozZA)in}bfP-ErbUvwOycv|;5&B>Q3Y%L<^NH1-TUxD}a9R#ovq*@Ipecd4jX^Ru< z+pj$}pS&!lj`K=XI+jI6McU7|x4);u`}U;=SX@1IUhKgQ4^1bTDmdKU?%t|mB}f46 z?YfOR3XArsEWs}HG%U-cr7?%iYuoe~(-0AP>G_jWQ&;Amhn0X3sC^GjMaJNyu$MNw z>G0dtj#cj~!*VHZHaSdV_O{In>BC9coC0i;xShM$?515_rLcF#``5HiEn^R$TW6BNvi6j#;W*rhB_%JDde0F2U}ZP z{CLmKF1v!S*T%+EGqbC~gGsM9+Zysu*<7yfibeqw3$fn-FUZJ-Jg45QQlY7`!B4~i0_m^CCs+%vMn=iPG z14XiW#j+7zSbCi)Nf`9k#chj6apivr7{hp32Xiu$2-O=fyKYZ_YD8 zT)P{S!N`pi)4gHl3jVa(sR5^IDi0D7JsC`Zfq{i;)9Q=);(^h~M=_%IKZDtTmz`r^ zb_2%oPxObg_VAV#p-f0#!$r=Bfd<#^pUUyiqr*t7z(2m9T5 zIB27S62&0sd(>PaP9;A7BSvDw%cCD!E)oV$1iQmn*sKDc=lO9UqSQzCB(@Zvz_GO! zW&l9V6rEylx`%OZpa2YBR9gLwlOeXd#N66Tt8A(AvsORpl5dTnwOJ)r#l3zf_jz4C zeGVHn>!l$C-iZCT{(vuZ8^r7|MUqFj==u;+jL#&O#Y20MpYE}4vt017k5VSAv5VRj z4VKS08N>!W5VsNZu0AWvbU%p=*G5tuWue0r)|s+m)FR}&FT(>sC6|Juyy-9AX_3>X zYb1-tH}!JXNzZ$w8ZH|rvoUZkt^JAgHn*;j*s}+t(I@aoP zOL~?TLYNGcOoAtXjhx9bgEmb zm>9kA>QXZawGZ8rH96azQ%xo?RcA|w0L9fZlv8xQ7#Cnbo590be zY*Weu^;VIf?d{k8r$87?b$>25`<$tI*fd7Oxi~x;Z)l+(8y;bc(w|?hH>G%PhMFTw zaSl`TQr>}2cp398Q?c+WRxWwQ3jEs#6;d;}>}m1|-1;wJkgWMu@D7w$s_m?^ws&gM z@(`A|G)DO(r7yPBwVu}0T}U-+=;Su(=6ZmsH)vv+MT-UbPKru&AvZ9NfCo~7^~y^o zT393G6FohpD_PSzO@PeTK9i;T%Nr`BI#{|}ff}VkK|w(EhDS_=ocf7Rk;#Bou{mi1 zA9@}>jxXFg=#^Vd>rJZ~o! zPORfW34ncS3JMAcXCpg{N0v-US)0pBK-QHI_(s4F1i^Ym3$A=~6HNM@i;nF(hY^in zN*+z;N;^2)w1LQS#lXCpySlc#lhj%uay@7;>z0s17N&Y*<13^GFfTnfNZ6u*qlT%w;U%$Uzq+T(3%r92^oPxgLxtdFCvmeoCOZ02xW(RhwXek^_w~P?mO8JFvzXCqsTGx~6Nyr0fAmrCVw<#$_TsMlKk<5SzuXpP4QSV$qC3c-)8xWN=x}rg!Gu& zQ5HJ&2Cv#?-||A&)$scoNND;tOuVCv9d_KbeYf_%uLK+{&iUw94XJ--(Dg3XOY6a;L+T5oVUP3=# zZwyvTCqagXpP!y(#5mg{(9vtnNT3)D;aP8VH+C|%S;Z6K%a~Jq-kf)nlS#e#LuVA_e^0Y9|jh1rT-|+6PZI~mG(bsVv#1MH z0(=y|WD~_WC~D@QntptItb2HToSdAHDSev!jIb09bEu{H;otb)n*T1t_2cu*sM;F* z5+d+7G$S>|M08*vnNK!loHT!Cq@scG-b^y(;Pj?Ds>IqK|NQ)1Wo5B8A<5rJEi5d= zvUFrmUu(Q#@t<6$jc67KSLC++t0k-K+N|{95+0u~;;2l8QOp>X88HU)5i@~BSkLzy z=>o%FB^!2;GrKyh*v4LD68Xu@r4t^g2a=AC4Rmwq5ii^0+I5q2d)bFJn2wUNiEzC? zGv<>(Y(;A7>fs-(aEOc2@*K|HpbZ?+Qfe~Skd|D=X4EeEr$)zLJB5b$sLj2WH;Xyv zmI`=@=5rYzPq&6D^A*Kq3JjoM>HTzwHu&1$Y*QGJw=k{?csKih0DB5!TYa*PueDsq zFG~}|NicVdrO5~0#sgn5aI;bJ5S5KUfY~a;vZX(AB zr~Eh$>N(vmTZ2d#uDQ9lZo0jm^-By4^JoXdcxE}iI4|EWw~HAY%(DLeRp)}hpy2Fa z1)f0@Yw!|jAnped?&4ju_&OR-DpNiK=?uj%7}yZUnFvSdCh=CU1cE(_S1Gm+r2^_- zJL(3m2$8@4T2rT`2h1@@GUTqxH^)^{ddJ?)Qsb$4Xu^!{?+*f3iixaVDu8*AY3%DJ z(|(!{0+u(Zpg{hn)f1ie7Rf=b7f>WK048D1)!;+Hyi5qhKw18|JmPcDqR94 z{FP;*V_L;$8_h`aDV5^tCj>Tt_fkcvQnfqZHjBl(>lM+$dJVB3oEcRE*lW7{eL^r@ zCfl$+>2g_2T%aC07G)nab!tG8bkAw*_MoI3O;%9>B}8tUl*-|&wK6sq&ZHvyz&=8| zdwkT3sVI?wfQyUW@bm40kM6kep?P#VoiQ;p+3qux!g$5I;Og@G4FI+)Ps+B34qEvj zf5|(iu#}~7SuLIit&w=$AP>LZXtugJFu%NND_UF&yusvW&zN;4ML>r)SRvI+B{PPp zE6cuWW%t*$>r%OYD?ymTsz4z>5C1@ZzlL>Vku?M*wfEt4;2nrUk_S7nR9@9pdR%|_ zQw)GI&_gQ1;V`2^*6(%}LNzt1m~?ejVT2U;H~nY`c1%4qHU6psY=b^iSi49MZ*xka zSnPn&>IMJyS~}}GXDeP5VQY_20K|a1Q(oCJ7xV{UL<$(qkrmyV4?jJZ-E?a32c4GOv$A~plPK4STIcju7n`e z__VDH!|Us8UJ6Rqk@GpXzt#}_nfdB z3VM2euvId;8g{Ii1EH+Kcs^qUI0GZ)JUl$AN99k42$NXI@Bt6>_wT?KvG^nCoL^{6k3?SnP$3~9&3JKL?$R+PC!V9c3Q0UwRk%yC(2{!j z-D2&WQI;-YXNITh#HdPPa>d`fj(9BQ;fBZw#!A$NeCKB_-N_H_-B=$H&K>%}iFFM9&{4d_oO{A(C@r&b7tQ@HH!nf+f|`4>+CgI9F=91AdOf?eIGN>ttMT+vMnRXU z@!37LO=VYHEswiiw`7*aQ8A&0a~hRBHd_`BNrsVPoGH;4C;OcesJl4#5!V9<{m`{4 z#Wd*F)jhMbyuS`d%m|&4X7valSBc`a z=xm{n7Pa`%5R}t&FZ?|Dn2?n7p$!*UlER@4iVbjJQ3Dk)+{Fs924u(Xn@OGVuS(u% z!V$t2dN;7h@JONS2uEi*3eb|HzL9vqFDUFs9t*e z`}c__i-@$G+{$)jjOgt#Nda;ro|uPvEV&b6!Hw6zKT{q6wI!fQ*eU`>8Z;UThwEkB zM{RFl&Mg(r(S?8bcrr}@nkgwoklYtlb0l&b7MMEAPTiH4oT+i$&?-P_t?dY#6xJ(+ z&b$yo`}Zujn6vH}x~Vv5?BW>bqvG`zR{!Vuc1wb%9Fs2Z z@-8FhJm<#0r*9$EI)D&VL^xxsm_b$bywAzeRH)k)y)7i9cXRTcmIZkGG<+Ew~kq-ei1~R=`CH}k3F;Yi=1N& zpYMCyi50IV8Xq(U313Q^la9!(ENO8yT(fr?s+o<_lMU)FqWrk$HGl~o4d8SeN*M+se6HXUuljAO)kv4}q;1Kj@2f*%0vEGeYX>@uO zV?Z+E**XcK@vnGL!mowKtT)-MsLQMH7qm`#+SQ)r5u&_dhQIvEW zl)74+hfpB>)RjRd-)BVdxn8aH`+O#TUJ8;|s(QNgR|-0( zowvWAtS>&o3DRcsw>FB2@^-yUuwHf?VkI{xA=olI!_` z^WD5Vd6%~5g0Z*9_KSyjtMmZy#2Z6=b>!sS5|U)A3h{X~SpkfYxZf9)3&ipQBU^FJ z*`^hW6oX(uh?1)LM!*|;>FEvp(^ligyBPmDx&=}<*?W;)m)obrkVPOJH{%ovi zW*(?qVbAxd#^eyZ=^%n>ohvvrmn}*fuL0dK(O#KB=NSG#;;uhGDJUO+T(E5PklAPg zDgoF*-iqk<0Z}oEimW~B>FKP~r@ugWmu3_cMKX7|36H~Q<6)Mo*J83f9#6%1C=lghZoM`2V7gpYVI%kgq(&OpV^4ZNY zBKG?IBFlTLFnL>r3v-9&MAngMEngg=_cQTMsk;FXtw$w#0H^mUbY9mE#d$i#lCzg%r6NMwcqzW zhuVNA2wwR056&?u-d_Wg{_4`gE-%gG4xC&RH=b}0LH{I$psp1!uGjEQH;4qDPTYAH z*Obz*#C!^U0(uP%jXsRpG1=_BlfH>k{#d&!Q{o!Ewe>x&>Dn5sW z-FIkNXhEgIO-~?A{Pf16>Kc5E>tLvHl1a5r_t}ic{#}}-lfHmyC27_*IaY4#47?vZr z&V*~?;e}aO&IdygOn$%H=&mO{<;;P#REXv;86PQM`-36>%;_3aa>rV7_wE#z+ltDN;a4Ktre_FBS73sM+%s$=qQslkGX3YqlVkYNI@1eZeaptDO!- zI?!mdVHV@7ZN5O~@-QZlXjalMb8C_SHG$QEX)5m7vB}pK$l)%JB+&4=gQxo5zVE3% z4|#AT3AJK5hTdT5Ak2NBTJSs6v?{Y3>30gc=wH2^wzZ5kz%P!AW%~yie+}g;lFAffI6l7!Ni!~_Q zq>p!Z`r~vjRBYL2wAB37H1?1v_&O$=I~Xcwu1otX*Fu)X(dC~Ted2c2vo&YX;4RAU z9vnZOulLCpzUe6R4N#!`b4E)UFc`lTR`j zasy))o*?t$EyjWfVE5y|J@VviY^{<;mxAkA%giW3=!B$Ey*V(6{$VfKRKiaD z0meVJAUY^gK)K-=R7il$OtW!Rn3p)_H+JCXM7ciz#_-1>UGfHI{-yex%s9{K8K1!B zhnV`cDuvipem$}|_+Of)<y}sK!SuV8T zig*9S3DWE{Fz4-{9P+=0RP$3KUWn{+C;#LGWkZfXvZn;(rMck|_;0;wdy$Jv4#<0% zoT5pA4+@8|HxmF(0SNR)^{zy1INxBLU3D53gQj5-OprWBz|)nX=RO`+XY)sKq@+eh ztnm1}+{9!hM(@Yxu!aRVpS`IJ{t38hu2ZO@I0-=2f;O)7WVs{Spk4eLRu7(-xZh}j z=q{nwfwW<-U0=*sp&exu&iNx0v;J>wxL-3W1EAE99`8eJxU4r-`7SH-g?%l>E5f?% z#ipdr{h!B}I=@6rL!t`qgu8tc=MK}Z_;bmj&zVdzZo3o&zDo{dDqYQ z!iJ}vS}sz<5}ED0JXw{iRf(2Ih_^VzuxjBIp7L5#Bu~jS?)J%4GEZ>N9Cov|c2s#B z9Fj$pJzz(;QjqEz)ad-y(oyG?k&(`Q+J1e~C8fnC-CnQS3M@job|4nKS42bKx80)L zE!4lej@^qM{bSQe7k7saq0faY2Zkdc(?k;(|KgBGY@P6FkJcCEo`ias%@_Alwcg0q3X~dt@2mK!IVd-&JHaJa9A@4IVpI~6F`f%#&wfX7q9-AGF zhOR_qxA^UUruA-Z=SP|YL6(q;q2^5-%HFmGg&Zd7yyk;U=ybdOnoou+%kQpNb=gTP z%qfRDz=r#c>i`j=Xk3#t#RTE{g`_jjtY(9r5d@kp|9VkF9T6c{z%uV*4)l@S@H%c6 z1VMIIntC~*Gh{-~7=FXs{y>)hWri$tI z&jA${kz-j*Ia0rVYHpl_gN9F2qDMU7U&n(9)&li-tSOBwccLYXmp-rI*Z5B0`?Zw%!u1EKr}Fby-Se;#?KL&P1^qhG}#M}u1tFcXHe}D zYl069mQfgS_QZZ8>sC#0irGjMy9;hg7$VIqsj!jnuXqLs<)j6#qXZwP7&v+q4!*c< zsVrnZ4%DohTU~G77vWm4n$`GaD0y7c|JDgjOMqYVD^;ccF;8rvpzvAR zDY<-ldOA2TQ1EuEsgZw4-qO@J{>BHP)!3F2ovf2cJnhJTUyp6RC2rj0_NZ~zItqin<;2(V^eMXLmLbo zo*5A_j@QuckptOhe2N#FIJNlJs$p%k7tgrc{h5?*I);i6@EEW#cbpA(XV>1qgntoU zdp6pTPk+!n&H;-s*jI3VdC?PMP)JSj&ox&Nvg%BzHVw}JyqvV6ta9JhnVO>c*Udo6 zA6V9|IvQ~nL~{$P&d^9@_IDBx1N?hDwj!a+e?#4Q65*LMY-S~g&4hasgYNq0bgbyN z;JpGeWThWZqV&v+&o1Eils$20zEBL%-IfuOk=92%ab|jg?)6}X@tK2d0q^hS84M2X zQ^?=5?&rTKB!`-|SSe6AH6=>OZ4p>iV0xUw_w3woo@ELHE_;#!VL0xcm;*)OlCV4r zQ&4>WYTuk}iUVdzpSHR_`V+x!In7hJ$`b%l2LqAHW5K-TeC*bu5f$6-JHW)iHa~)*syHZFD&_On+b0$PW>>`>h8J-Uq)y<~ zU*4@^rDBL|b*Hr##aas_P55v4YX0GzEdV;(+-6^MXW34wwz+EH)!<+fc!&9Xzdyl9 zKasAx>65WpIkE;=aYtm(SXfeUj_(wojmR@+bUT#KGtUQ#OU2)CTDdvXF3T^h)5lz} z>#M~L!bXEvo{OPW@rI4jIQ)71<2BJn!2sp0*&nQ%hHk2BP zPOsA$_1)1=zd&_D_82~`GdyIv2!lKE?iRMYf32(k%!?PBbF8NN)`YL$9UlQVj70Xl zvUiUUtQu;bn}o7oHQ=y^roz60y;VA)sF&=uMTAat2r(3b0l^rY4h~lWcGEPd2kEfE z-MtELZ*Q_OR1@H!pavz2hBK6RM{z>&K+$?{#{h$U6aRSN7q+RiQT0a!B19<5uN%{` zpYP8Y2C>hocc4_=tDSD_&y>;0P(|E5RoloLo)pjNj37D8Tlixp{~_RdM4edpf$RRT z!Q^@eJ@cF;D)7kmpUa&duh*M%1A0tS(gWkwTJH6Key_Pc1oWoN!KDy<(bZ047$?%{ z9|B5?K1#!lE7@dh+;4#}o_e-WT0YDV9Y$H;4v0jL*sqTzkGr21?8?Uo2kWz(WQlZS z&pK#MRxU2DL1iNL-vlD1pXwb+d<^r1e1xZ*Tg9l0y2%i(;a)mf-L1csLYwu0Q;)ok zVs~~%FAPsOAZ<+gNo1pkt&--m8gADMs>;e;ek}k>jp*y*HShtVG7eY1c2PUeK&}3D zb-W~!fvZ>2z_0>=95L;q1j2>ba%VTh-01rKUn4Qt5s^c=e9r{Q|BkQ^Y<7N5eb@oQ zaKo%>YTiVt15GO)UpgkIcm87%uxmYW;f5OvK!}+*_1HfG=QSerftk2i9msXftW2(_Ogp}ii8LPWET z`Mlk^KaitMhS!2pxUXEn7%%(?E55d5K_HcvjzxcO2lC#2xqcT9|Mfrro-ef9aQ0^M zEIixFaU;GdDy?)S`n{iY|APK^NBW;jp`Isl005>t007K?9qIr3xy=9lQ07|4!-=@n z@t5ldMJYOW(zUt?XQ#=exNLG#I2k2jqk8RGk_9X9NO%y_6R!G*WS3_f-~pwk3l_9ZR0^`{(FtTIU9QZKfbGvi(A zK(;#t-?|z?JSNT~^{^HuC}Gd;?lZ>0#AaNsy8T%=wG^AW?kXJQv}v=xfE2nXEZaoAE(MGA~HQvLwH$ z$jX6XJjjhy)_BK-3*SMDG%}K#514X z$0f@po=LIDh=jgf7I`4?9Ono;AGoY=g*G}{)aKgYuGZ*XlmQe}OOBaY8hWf0EpD7P zk{zy%=BOz-8_+8Juf)|@8{`-6A%}*qV^{Gn#}MQKhd__&(J7@bis zZf=gr#YRPuV@b*Da4D0e5F)4Rj8J5*K;DBczGiK|YkqX0&4pA{40{0kl2RBczEI31 z&Yx#}=QUrmy4;gCuDoBn*Zp5VIy4mv1RWxBz@`Y0V^x>o>WVt!uHC4e zl>OYyquKc!XU^b*vZb4NPbMMSwW@>>6-3UT&V;WYhl373xjVa7qb+Z4=e0_^S{#?@ z#~M~hBoo}n(kiqV!zn_k=DGjnJ!O8H6KvWYj_3I&Yf?`clr5Q zVk@qzoOsUDKY5$@ZY0kLX_+5IIXecLi%X5b4IzZz`8s7!XFf8Macjn@uA?|Zj*yb> z7lVm^(k%)1haSJeEv-17iyslx`=QmF#Q=c0Ry{3n=`g9(BhLgD6_C}He!-+!<+B1n zbk(&u5_jGNlJ!G8Czy~pt!`HyRw_1p{Lpp1P6Ys@h~m5+Dh#dzbh#W-WdV?pi<-1u z8c*o?=^dS8mXK)0F}C0GN=sR-dgZ}1NtnkS?Ig}R86xwI`9VQhVZz(IKPG? z2#(V=`i;7h6tdd%vG3J5HD_cu0F&>)`mR&`m!X=+46LvF*7L(3Df~h64m96-ybLA6 z&{Z8!!dO6Xxaw*o)BL@aIe4V2N4e}I0@G1b%j0&UQtuF(v6~)`t++@H)R%>_4JQ>;Y|1+6hye7z`2#PPxrsXK%ugFGxUJA_zs^W>tMj0 z%X_yqcN$!}Lilci&Km74!Oicj|583eZ*p~(HT4E&*w@?BQM$}Ko>`EH%B@!k`GS>u zP%^q1L@~dTY9l`Y-LAK`T!tBY+y)}`q_HFNq9avKNk(zh*zBUVlL_c|A8A4p*dooi z4V~QiL*WT>m<#$^W+ScIV3t1vx?OrTLck{!NFc}a!n>65$AD(dmYRJ^rVrHjQWur) z)lF!pdm5k&h0Bg5V&>BGsf`@8KNzs`pg62N4Zt1{s~h6|k=!wtzplq!X2VXUBcAkw z%GF~Z(s_7ya#I6bHI4>wGr@&XyWSD7)31dSJq!yAx>>+^RMi`HT>*|TddszKaXbr z3%8OPh|mv^pK+f0$EYH>Pb&>jOP~F9&W9^TW0w~ZvwXUPfLQyD()vR@zfqa&C^Klw z7C5I0k3jm9=krOL7YGyavcAI-W4Ku?-%*B6q0(DQwW?KfbaerVffw0dl`B_=Y-m9d zv4Ju|_G?WmaFW3T0=-8{1?Y=7bcN$V3m`9R%lfqNSLA`~gwXhJw3F|8er9nVf|q; zvC6wT#{xFzYOt(w?8R2`C2khrt%!gp8u%Y?Uu#J)dHyVyAXKrQ!1JJ6+Hz0%R9W*n zF^NHDMLgxAGT_ayzwKc4MbLCvl? z=)Tzp2TQs3m*xi@%(VM{iQrHRthNfVO1qDFpD4)gKWA4nU$c18j}2ogTHySw@!|@s zFMHc?RoDg$2};Z)!`pqY%f(*F=3)R$0DH#?&@kAj;^p}V_46AT>HapJIbiCn3Z$nbJ*Z3gQ`N^34=U$$ z*k>U+#${zVpR>m5wRsG;WZ+UsC`31@jCuMqs>p!77>ocVLI1; zcCMm($UEf;mFmMI8OrPWfZ(gLXY*L?i zF#LsKe6c*mC;m~Je8I<=1oksI4Dve?37FYPx2wW!4>*UmPbkvXuoS-@A`kGxdNcU@ zAd$p;(MEo=0S?>=Sm<-?wX(3W@4iX}<|cy&G+FP^7{=1;(gkrq-$80ss*gz8?E(!O ztFG1@OIKCb)vl*+zyT;XCj$rdF|ZnM2!rVv;2UGx|3eEj$>o5*Oq{!UJ{XV9{-7^( z!_DafkZ1#TPFgpl$$8IUKv zR5cw~T7jZjO99XkB{M&3NRH%^lEx9oDor?RdY!c_{IFrHaG7yIoOJhY zEG>`+vO++G61p>Wt~Fk>ybw7M`T9#%ALa>LLxK@y zZW{>$hb{)aeqd5-VR@ncvq62k5q~unR^~vwoEVfPiLlU5VGdn1qP}ge9ID2;4P%mV z%W~YM+XxGWn?U~aimBtX()2iU=vh;dn*hgo33^NziBipmX?~d*c@0{t$KVemTFM^V ztX{v|Kn859LotO=6Nmf3=l|&AtYhlv);)|<+`UL~r?^AW;_mLQWn&w6hvH6=;_mJg z*tiw9;_l7{ZoizH`+cW5_pJG2GMT(jCcl-HNmkzVFxPfCVdn5mBLvY&^A;vItm0=6zP&%JY*JSmbqeQk` z^=_|7SWe7>kPuCL7t+oo)Ib3)+D9M>$hX75=jP}>q ztpJ`a{4*VGsF;Ka6K-S8dEj6ImldC4+E`G9rPe(IJ<~X;d3J?v(Rw0|e*tdc$^l}N z*^diaf=Mw|RIat??;~Ooz%zTsN$OFoQQr=Taq`84G6mGpb%?desy8EZRz6o#d@<-> zPV6{XXV>)|U5I*NlxEj-*3lDf!^a-xR0vdUT?fgqcI!rjqc5Vw;t(&^=LTwltYvxt zYbBy)44usPAGrd}vToYWqfhctZY3`o`b%OXfG>bRo2@q84ObVGax^D0xL{PYh*0ie zTdmX-s_@yZ^u>LHj@{#^n0=fzcaEu(O6gogODVgGG8P-;a-CVe=@6n2EJx3~%}^xL zQI#fLhfhDF7;3%Mw%tCRs4}Rvf2CU)gUWIX@z$GJ-UIanE}Oe*pyQl10^045V8rTjZ6c_u^ece~vG^Q+c&&TtN4n&O^9O zPqWSemNSgnj<`ng9zDr559s84O(w0;m(0Lgp^P56qt?t7G#HIb;ILo`Hle$>WVpZ% zxRFLTh6*rk^{OrCj}^%o_*np2kx!p}@2d4ME|Y7@|Lr;)pZ$GOv+ojK9`*j7?OXKs zC%lxki9*WTVF&=$Hq;yuuO;LsD@S=Ir*JKOOI~CC89`(iwjJKUA5!v)UD|wk*z;L`nW)Gi^p%y_{Mjkv{;=wvYy8=U+CHi(2&0FvfQPc zRtgG7zt@%hBQ1fdZDj{XW)TAB7LLlCVYqZsQr&u__$V<61&}Gf zjhyORQhBQCKzQV=haW+Gf^9Wh* z>#PlvjO8eW#&tEm2(i5a>`<}t^yV%_=O|L~=AGt61Sy>~5=Io~Yi|~?iky0;!7*ec z6jq~?#~7TcvnGc~auNK@0SYIA@mO?QY%3%7_*w_RVOvH&u?R}EJOkHhUl3?SfUaq` zSIgDIGM~x6nWFLg|NJ<5*IWEq^^PToT1Ee{1&-B&=%^=-QXRr)Yn-M;c__xLeS?Ft zq0TM=*;jIRn?NTV9pHeqL~*DfQ#h|ybT%7=fP$J^VejsGdO zTkhj^v1@cT$7M>hnI&(dXrB}WO1*6@TzZ#9RQP;h%dJ)%sioQAO>SdjY$gHKv)?hheVdKccAP3>9Segc$%LC5A74%@TNivFj>N;?usx zcn2&}a}U`8HO@=rh?pktfL!OL(+w;OcRU;BWpYFu8+UkTZu3<^z6_E*qeO0n$e%t~ zULS&N?bt*JWUYNnIE)HqxGl0;&4K$NPcFLd*=$Fd{wF;I+1isWo&4^AV!QZrg-pkD zs?lr`rcS3`#tpCQY-4tuBB*Kc|K2Hd@Q-DM1wW&n50_m5#A|1L-aASk*qisbU8K((v z%sdQ|eF|aQE?lIF<#t|Z;qv7leA9!V%+&vK|rVNil_^qsnaW!rIro4sE^ujofH6%ZU5MXnVVj8Dd@f?|_oh!Z4`L-bs*Cx!T+_U&HW>2jvL}Zyp!4rf zC;LX-x+%T;?e}3jF`@I`2&7PbOU$#iTP?Vc?Ng8~&Ry=j>@>)rt2p}8_i${j1*MSB zbSQ)q#i`u0vV@p+eNlMolpw5=IzGZVYqd`!pC_@WuSw*K#Ph;WLYd30xu@*mkgz`5 zWai13RvVNxqvp>f196Ie?9w^%7&uZEKNC1gY9-iC`b_g+X!@<>lsIuQ%$C2Xy_@ib zSxm2)_7u2G;iv}Hak43vyCaBdN;_ay%%6pEASJjVI2I}#4=n7f4fnELGK$4e=%;K$ zzJzQR4VEpQNPfn5a=q8fnW+#X7F|+TBG+l^FP?=S??o^bt@!RA?it^s>(6=MUmrdc zayeEOQ_O^n=cq#yx(oRLCx^D)HxrJz_0)+SMqOyBX;5=L4sn>Cs!Cdo8wBmzRtY=e zULdW<8hF$XXD(QI7c+3Ay`AeDf+9!}wst5kvr!_0S$aQzDG?nu5Ci|<4^@qdKK`>85KN@O9YtMRzT;i_c5Y>x^g}TFeWs*j3{5TJXWhOfN*^kEM zFrOn#t+ZyB5AE>}iMPF+Hy+ML-%oyaVa?z&_6=CM{fKh+ z)jtI0lH_2o0|Z#^zGEkR>(1v+UmtXsols;6T8qox(?1)}eqWrye91hKEJjJ=VvE>Q zpHWlN#5kM(f!~mil-S0}5I4sME6xDEj0K1zR zevZK4G^`w;;A2jk`?DtfRzRPcrOH)8I~ewAhnyWOTMib07RHfeartTV!h|?|e)b4A0qBVWre6T7dTE*jP(S5~GgSC~^;`aJhB1hfA8T zYP-l-JshIf0P&h`uwR${cd(xoV-NP4uY_#U8UAF-Ss={VGUsROG_`={$SE@dH$2yV z%n3(4JL(yuf?dc>YaQ{xgvAIsS|`b%!|?$RH&FqUq=B>}GSf=*cfm7QB=WkV*<=D& zS$4oJ6cQm}x3J>W1J_6o&{WQhF;x7Tcbemte+!EM|ChLlU#;9HY*L3IdbeYV@IcJ$ z8V0iaa*~bn@E4xdZsZy=z4%|h+KaZmWSd^Gew7mpd9e5bL8m0E2((3MDS~}=x;IzP zC)z_rSh&|yLeC-zfwKbmFezrxCL!R*pG9qAhW^59)7wNlP40&RoAnrZE~J&KUTrjP zr1rw34k4hMK(<_nyU2i#q0B}t2nRoGdQydUD|TR#dmJ?D^HGGIkle10fW~sw(e00| zMJ1cWJ>GptUm&c%#SRTN&pzI|+t4)DDEAuL&Ouv+b7mjF;z4NIHgxjkR2lX4j1(QB zgbFaVDT$AK^AOK=Q%veHJhLtHWieTJ`NZfp-hjD@aH><#A~KDC*y{B|Z);%vU>K>K zU<&5qYWxVULdv@{ssz(&SgW78CIapw#1;Em(x zIP)Z2j~5e*)N_feK#R(ANdkMT4pHAA(C6_XAk3FY+Zfre#;#t5T>y^HE0xrwauG&9 z@Jeq~{GlkQW91x*xfNe{D9dXle^(D;_!4evMd z(d)%Ml=pofT0K&CEKscAz)YP)h*?lzgaSaP^HK`?F|y%H0J|HO-D7Eu#^E7LXKL5T zc~+z$JwGPd%!sUQ-%!_KLP!d>Xe6;TOuR!mcFUKwl&D5LM}M9V)0Uzgk5QKO@KG|a z9G#2E0(s#k#?j$Rk^>AMyVi6jm4tyZRktdD%OrILQHks+?0#tl0s>#&2o-Heok^c% z7*gw|(u;LmFHv@Y}0W ze9)C!WQ*WNpT**)^K9k&l?~fS-NaV=R`IJ78iz-6l}Z+-;u?Xo7tEMTlJYt~~E zaRI@W4UY#t>cbQr0~+-ASuG5>{=XCrqlN%B&m^6yN=;5*H-VNfxUzH(rg7^Y=kQWq zvMB*n-^7vzEGHe4&tymJP}XsVtsf2sFM_1>IlGT5HuK zAk8)Cs(j?Zh#PnP^;B0voBWq5P#rhAfTF4-n3|-BPxhI5q5_)@PLY%NHLnQ()OwAp zoB~6l8=8%^J>Twk3%g9fhbLMxD zJMXAHI!g(5PG-cE3T||wbvt|ur(~}XmnOd48A%1m>(F$K9^v`eGN(UCiQ*YcEq3W6x(vAdWu1 zyb}U)5}Ry^vNS$hQvuE8de!#f8ylChgm9ymwo5McJ&Is{#pO!y8#0oUTEmEdL7*%7 zxhSC+X?U?kZ9=%iWTn_R$K``9CqD7^b%|n6MIBkU$?x^J_aHmqEqxpH)$U$W z6zv*sW5Ynp>Wh(+{0D&P{)NZJCHe$p7*$xW!aMoB*?KgHJ0WfS<{Bvf{7!JAV_lV0 zGwhkpV3y=pNfULV<$!+DeB?utEzPUiUSn1&4JN#4qWc~q(zwA3)5SrOde~PgtB|U&qpap%-CgAuxK{>;U}NnmXwYkmJ6bggcIcaoXA-~86Npw z#dX?6KtvuSGHg=Fvs8D8FASu~dQV4|m9bKohlfeMTkR$zZ@C`(pNi^UgZhYJY=k{z3O8PNc^akmDk`4JRNzhbV9!tihD{Yudxa; z-uTGhi3&dGa|{wGj;}TE5sa(gkBBFTik7qCTTg!0FcTbGFL|3dxSyK8<2Mq1Dv0g# zj;7ozzz<2Z3YqJf;zu!wz-8DpitsJtgP7c%$tC<{wT)+VLQ=!IYdlw$6Vv zH=eSZ+Wu(8K~lme#&*B1G?gkLBFIYhS!8o=TUezeSSx%QU@mk>L=F@acMLyxLno)# z%08YY3R?O)um|X>7$=CuTJV}v_goLEeR$^1yf(Lw?b`u7u|aWg6Cd6?7VVACb);v! zsTNU3P12Ne-y_s>ukmqL73_z&vliMcLdu%l4W=bq)60XDOehh(;8N_FUM_?HP4@sX z>K3%bM4Wpg`jlFtayRUW;fY(rgZ8ezhSQ{=Q~PZSlN0p7@Eg+8_=OnDqtyE&IP6fV zJ?%ZXhB(wmr*{MWkfHm@td9W$oOo#ahDNjupoX;2p8LnE(uTSrcKU*~Uoq2z8#C z#;(^i--0VU-KRhLjNVNgESxO3hhA5yO#1P@{eCs|k}z`(6&`81kvcl*LWPr5dGDRNC36lEnaxW$?K0nYhL~@}sK=`Z>+{?M}9H6W~TX>@;c0U(jcyIhb zC1?mMFjzQ2<$qbnACVc^+=2wrJ~r9M!rOzt>yg%=T*JE8l=H*pvl909-*_><)A=C` zfRy`5#PCVl+ z!^R)oS=~KtFUQ;~!{d4kd9Qf*)%%Cky|v+9ytT@jH}!S|@^X#+ zDzQa|JC!V=6nsC3U&CoX#M}Q0GhRrB}eZjrd#`;(mi@9;U@$V$z#LN}G4-Xe73agnRI`XnwSYFfF5K7lT0y_poQPYZE>DG zErPwQ`lA=FE>V6#u6^sekP>~iLnNIA$()in%MRZaEo?gvIfwFDA}m|ku<7^4hw(bu z7*X(}tO)*?F#gSuz)P;+`-K2}JBgUGR3LM}8sAy$>rz|+DdjP*vR(HU4%#Ux%ExT@ zJuM6VNl~q)wRK_sb+iy!*7beZB zV#UwwCgkTS4fnxC8<$Cqq1(zc89lZ6lpGD+v55Y78FLLsw^0`Dy$%dDDKgl13hsTBf0 zzImD2QRvQh;xm@YlM*cjM=2>4>mdvBO7GcV>z-*}!jCvj^D?Z)RFv8as4B+Kw}LBv zg*BM`Nq8D;)$F&k@|CjH28)p@U}z?r>Z+CAvcs{1AXEdm+A!zC+hiHSp}O~?7z|~? zeKt2hnE+J2wcd4$f`#Pc)e5-y%9QyJWrY>>NNS}hJVr(lF9PUGq7mNut6L`Q&8m$D zoC%R;IKsrr$xXmCdFBu55Qtk@p*U`-J+EAL=?^%l@euh!=tyLbYAz;#@)C(q-KQ;Q zCd3U~B$feYukCaPlpN(qJxq@h_(p>;E1>IO-opHKGuveUdn^B7Wz*xYG7P-nA-VJ% z$OAMs>lMNo)VoxTgs!0{w_GZFmPEJQ0NiIUq?J@yXct-$C|dR;|7ktyN{-8*Rs|f2 zMI|k5YJCN*rR4U8dcNw4?g`N8?Xe^mx0b&23n&QjVPV?qNwWV^6P#Mwod4nT(0y$8 zli|O&3O>nS79jri71R8dN|XMxRq(e<+sVq>)Ywki0bu^c!r0XOFGsIy{HV?Un#&7( zOxl%(`!o)Ml0GFyeh1hF3%H3>#>5vUOjg_78{1e?vEGl3b*ePA+Ti#E+EZm@eJ>9d zH-br-nd7~z(y0Q4GS}gaX4Yftokm0JIzs|5BnYBV^~4lV4M;K8COEA0Wvvo^*r9q{ zpW*hlpjhI+8rfF&wp6|Ed0WLg8sw+F*9v^XkH*TV(mWiSAmI~*(T!(*S;0*qO`Om9TNf!2Z1`rSkzpv=u Lb1rZA*VF$15$YQy literal 15164 zcmaKz1FRs;w(qxX+qP}nwr$(CZO*oB+qP}*nXNY`_a*n7@7;I1lJ0b+*S~wMRb5Hp zryvasf&u^l00AK8k)ouoUm$=42mtT^1OR~iw^m!&-p<9;&P89v)4|kPm(Ii1rX^`g zZkPc^)Sq+`jg^uHLFi^WXbhwgQz<2<&{ntHM*k^&#n^F3lcXNk!o1-WJSK-0edThWv9a04cm|xvpW4(0oCP8D z$zsXmCJ0c<^t?3OEd=jGRa0+jRZ~bB)D*t=*R3Ngw(Rxo^4uz`I@wjv4X5{d`70#D z+G9BEy1?0)9n?6Gna}dO@_$&?zeEOt)Wr3{G4af=i^$rFx#MBK7Q*lGCuNmgG ziAT;{&R=jpB0HS{q41Vi5(k-GfdcXdNzp74^MHZVgJz47`lfw-XT~*N3PI{}Do;{vsRfzafkU1^|Hm8$x|2QyXV`x__NZlXazs7*N7~q{hFp zTB9PWw_>5XkSqB2X*kOYXTT*-Tm9|J+BY_&Ht2??JZFcdCVV=UaDNNnr?8^ZHs&C) zY(trvi0)mUZSay31|iOC2BYp67npQp^cjIO&V-0{2p|oIM6oe;f{y~(D+rHQ3nwSJ zU$G89KPPeS(@L0_=It^5WD9=CG$NtaWXI}ZE8vo{dPFu9VG%<_^dHKnaS|){x_jgT zd_4yH~nmZr}C zYL3223v$5>C|{_H?iSF|%&_$$GkH2jD8&yolW=bYMxYBWX*x(#n=qBK-eC{ zrkNH@&F-~^7}^cs6Sl{wlgZk)B<{T|+}jQhBklplLU#2aW74-Ax3EKIZX(5|lx9<> zTg$~=(c|DG3H_y0FDrtyMuz>F}Wbx5XjCA8QfOc%A>2vjEkWDQ&kByK3-X&t$mWy)7ECa%EFfEqy@oCOU?^lR8GYV zPZBfwluA6uaTqk4%2uP4Vk<}veuOSh@S8iDzT|`En8`o{-ERf)2eNKj=lmzT z@c+|)h%_;x?!SJ6{PmygfAe3|<8QC&Y-w-zuO_M+H=r~`fFkN2Y6uS^d^kjSQK{Rd z@P?w7Edy*4u=(drOn2pl_idW7%Uffd{FJGjLtz42ilOuP%meTIV=EAlv~=_QCW#2{ z85)|jYat|C8dKj{i*XGTdV{9{#--BL7qO@vGMJWUooh#_7v)w7p~@BATkK(3f)wxP z!+Yc%S&+T0CYsmpQ#3XU*uLGS)Xt0H(2-^B>Uzw)6tE?I+Lhd7awszJ5(U_|d0zOc zvzPS{eFx-ca?Jie{l8To4{3n~0DyxB06_b9|6MFxZH?>f3v69-67DQsil1dxn6Lt|5HN29ZT4M^QAx7E^X*8Llo5nY?2)~*l+%4hX zf8T$<@||_$e{i1h{VlzGYiFIcAElw0h*O}w&;$q(Bg5uaZA8&QcUQv^eNxOVqePzum`7oh5BgF4{eW1Xa41VtdK=-*2Rw{}f5=ygN_B zR4DFKqnn1pTAUljXcw4w zi4UKpu(Cr-NJ&+OUDuiw)McN!`F)IL$Bq=y9@AzjP)2oQ{33fX|3ekZbi*&Pp*_n^ zp^}I2ssO8V#LK(E*p@ii-m$o{LRwY%hb%x;Pw`=!``xs%oCtq<77p8jx=^K-y^|BY z^8sm^n#9B}MOIq+>h?(ZZflzN&(*!vUdvMKA>JLAki^sbd(RL*qCOcJ6a)xaLUk0B z@01TIr;bkS#{0%ae(-N3JE1?WPa}(mI#Z?K1km0e>cX32(~i|bx-~Tf33P_t1!ZE6 z@_Sa72uW%AS(AFo+R&)&G$bs2b99`7Ex)r6j%8t59BPWLRNM700w6c)8(MB$pCiEE zoJdg455;@c5&YkH13Q)$BeU{tT%j4~dEad7{q2cKvt7&iJfuko2>~m{&)ODz>bEiE zyCt$;gQR@DZauZXCac{QKGYxo{HgxgWLh1oUe+@%X?ntuZN5eCc-EV}G=ZeCk2);N zZ&(egtGrS@ih3DJEQ^ba1GCIh0>&t}io;|E0h0AnS0qHsd*W0M1MvT_(rLH4ln{jH z4{~ut{BjidQq57nC#po)TYS7Hu8dgBFGc9y5Q`UMSex_elajI$pFCApcMdRFS=J-7 z#%FXP2UZAw&dV5a;drzLo1$ay-t@M+emQHIeO-hiFFfhgZNELo^w5*nDf71&7q{K$ z8`vY7$;~vRWMgN4GixK-(;dHw#$c$WEJeykL?N4iMNa;(2SHwibluAP{TRwj!h+)V z!#4+b`RpNFe(}z($a<}imK{EJ=g~7;LsgcLEw_B^7wY@`{3lxT&Mq(9@ESa2JAycV zr3XP2HFfxz=>d(?a1>mK1eca}dN9a1<~!AYW!|W-FDpB{OBJ*`4Il$R#n8iw%Fqbt zdq!DPN{sB`V54AEj1SL$r5VD1OmPjVBIZT=Lk(3J3MAnPkvYirsmwu3Tig9RdYg0C z;-VEk8ZJd`moi!(VU1=DI)-VjEHC)*q?J6kviGe-Pc<3q_YPVM*q^g}e*^5SWVX;b zsvOrg*sNQ^GorX2Gora~93?7kkW}DS$M~%XJX$!&lNsHEU_ynM?0u5O;*JxG76HCk%Ve{>b&Nzt}Uqh8X2t$ zn1S^U4%gQ;?&;URy1r&OHLKh%W&)~K^7e!5@6BTaGco-rP_*=CMzVtvGRJEJ4>v{@ zNzbGE%g?iCrS2{i;+Ng9&Eay?CC>D~dBqCvFO3#ZO1mCCsYpQSZ#rv+OkJKiER2eF zj2Nf8TchCA`HyOl9J^}=Cx)C;V+s3u^VBh?h;k>E!%T)W863#-=2FC(lPB2iB&2|B zBBEn?_3i0>G0JA9WZJL_;8I}pH7B_6{RkYyV~$!YDsTigM#WqyEiZtG9vUkuF6yU; ze(#MhJexV(qQNqo*WXT?qX>_9W{k;u=vUC-lX;+`t0z1+Zw#E`;;cX6z_xdH^Yk^V z1Z_W0Py@ml!ofF7(bj-cnT@KpvNF>9EgaF}n)+*SSn8keJ53ua>2xp{Iu%i}Wcv4~ z_+QiEjxk4w@c3l%vb-6P7+u!h%yIUBD4;a#zSc;{PKijhN^|+qJkV=A8G`1ld`!Um z0GpD|U1wI6%nB$1YbzzUxQt zmrsjLaiOa^H40Os6X`f>?DB45QSR9lUn#u5(ljH=3W|yj=ib0Vx3#Z-<$nDUDC%Ea zOd3w$#!@)CaNR1;y)6953vIJ&is6!J$s6fuaWWBaSdoRkMg_S|yqkIV@14dR~_%6VE_c_&9aAg$5m7lQZO+cHjfDUey45oa(gXb!7Cn3zb{)J(tK z?uiDTK3gajmxmXV_1aLR#5h@MrmL27mfO*RNQSvk7*^h}6O+ph4i;W{<36`_d{CpN zwf2*ao^P>U0~mT&yBW<&Oa!z*fcd3K6fZNd)#lP>w*xfq$N0UjkRgj6ngp{nOC0Q7 z2rQ-|e2RBD8)cjtTixjel=-QViq8~`x0uB7fv|l&&(E>$dZ_R_m2xb`T*seNU`LT} z>LOHM0%C7>H*arL{F0m%#CgtM(YBmiP_Vf(6gTA5tj1j<8TO)55c#Y?NPm6})p{^O zWQYdQK|506EbX>}l5fA^`F4(95|+g&MZWckP;mrD$PX3t-K4=cXQ^wPgH!W{jfrhk z@FnHs3s0guMTl7DEJy?@S-C6yvxg40@i&Xfmc`FTOQ*5f>uQldnW7{fh?A@9m!HG7 zfIW_H;;LlYTb%&#t_AT(&1Akx(#f*7u+U%#R#Y$N8Baqe6KQl&w#@Nqu5?3Qm*Y35{1X~|j9R)C@3gVF*t+v78pJ8w%uHEu`c|_=pd&bT5==o!D7bNJ7Vy?CsRj^j6~~w9JoF1 zPax!Q7Akf36vZ06solM9fupN)FeisVC3$9K69Rxm0F)!{Lg%0&3DQI!xv$E5vo&4F zBb-XXK!%>^OR>-A<-Fi864N`r4ZN|Ps3%?A(U zbnomM{Rfx=w*ycsiE^#7`i2amZ|HNnTY)7tp^J6UTo;!|>sZ*>qL1w`vI^|GgOCjl zg97@VALr*5Fl_M?^hZ_|Pt8@rUiQ=D5Y=D`P@c*cj4X&f_b-Mdr;bD6l5=nHyQwLu zFPv#uO$>1J@d~d1v{J6QLl&-aXzPf^eM)?QH`Ed>=Jk)?E9autkl}sr_kwuON|xc= z+#1%YLR`E;J(wDFh~w%vHm|YZ@t;ruuq>j@=bsWWI)k;`(`a3SKGd{&E3qGLg~OXXHZMn;WLP#3_DQ}}?dK$^f2!la2YW-A=Bd0d@;`|-jZ zgqrPrQ89v}>)YE-kIH>#s>w+@lSv*Ks9@(je!g`KArWlq-wRqq2pBkUYrvhVJfj-y zu@yC{ez^zqZBZ2h2>JlI6~C&lkB>!)uQa$D;B=QG!jWMXEI|DrzK+J?#X-TZr2=|g zG>?QxTgo*lY;tl!d>!5I5e$dI(#sIn9u3UQ z*d&yc)ZAv=J!e>81GSy=z$@$0hU;Fh=N<-V2RYtuL(O|6kMHp@PyzDeO5C!nUHnB` zN}8K(*(~xvT_8vh`)2OEO3w1o($ZKac5sfWqrYJT>s%VoS}KeWdnkLFKZpRi&}5z3qLIE@XJUw2HiIm^WRk07>5wauV3T^M@L0n zx`VOKLnulke~FwtPBsg!bqEhc41s#*S$OK6z5_X~%esb7P6w7@3Gu80hhDDlsKyk~ zJpiao)m14sXlDf_#*K$0#hl(B`4%uOxZQOWbpsd|DYX7U4byQr#8PYQX0(+kNi=?r zeOzanTFax}D;zQ7{39mzY?rf5%1AZTE=*oGq8GxBOdAzTt-;(DQfJPKFvBRaG-~dU zM}z=;Y=?1XkeuSsk=H^$-xpv6;E8tsV$ctglJXgr+b@CUyKuKs@Y83#^3&)cRd+-_ z1T%+q13=eKfGo@$4CQ;=^aJpp1HJqy&B{L{005o4005YO2YSwywyrjocGmPpHulEW zCYFZgPKLJs8r$CJdOIJnCH?T{2bE5hHC8l;riS*e_f4Uo%8Fj<~@iEj>CTDYn}RanIi1L z;jHO@ElVAq>>6J2$M*6+hd3^C+2Q-JJ6uc6L9}#+%Rb+=LSR zBU_8}3i`dvCFeBFnG`+zB-h>Vm!tI{e8u6k7J2q3-rXAIdG<9Zg2P=8!G>@4jSgu~ z%r*0{h%Okls-D6!&d8`oA+eAa{KidN#zo8~9X5K^k$mqkK8Xz9Hw8AqR$N1Vn9~dgnHr(Xu-ORDUJ!7STv@IAGkT#UWOQU_=`{v87jlfCF8+M4-1y|_=@{{T%6I=5Q z57QH`G&~L>O&Ty0RXchORuURqlGq7T0Q!t1PViY`hI!M5CtW1Ez!hd?Ac3gRnm91P z{qRo#_OXuF05gTtNT;}_Wuha>Cj2e(fZ#6ID15#!JDF;2^o3E{yN%8ueHbOhx>sh7 zu3kG{hl{V1bed{gpL|I622Brf(6!#YDA^4aQ3?mnl`(jwFP+YvK^2<_2wQxQcj zs!hyYe$L57lcO4?x{{T+4plK7N~ZN12i5?N7g5ezc^Z1P!?DyE3{`+!Sjqq-5{6*S zk-njfAHzAk^Sa@WJ^tX&j~dOdMI5VOIUx%~$SK>)s4W%4jkJ8^muTz`imW2b-;B{= z=rP(bcF54Us)WjbGM7X7C&}H1M(6)qg3iMf}N#9Fa=mV!o zJc#mYgQfdHo|H{}fDyFnxpr=ZJ;x7%(m@7_(d z*QQTJ46ks#O~2On?BT*pf7h?^vvlIs1YOMic!EMfhzfvS2X(nbIzM^|Ht1Zv%kOzU zlq({E@6{a2yVC5}MzAf}IA%W<+UJg~dI2-Hukl@H1>1=$$%9QSdQE>-mo?ajgDmXC z=h_vn7RR%Gxek{FG$Wr8q>C#IF9DD_P4qxRiud9VFAq*t(C>WAL+KA7^8p2{d!SK_ z8%hi!E(H6Q*fo`XgF8G4ID#R)E83n&aDjH`97cOhG9v-9WNUs)sd5`ko^SHQ3uj1p zR;RF2P~%r0L|(NM7LG4HF3)5I2MIX46s;hcIXKxgy+WH7h$=O65-aYLQiLS|&R}^= zYB8NF9qNR`L8fXo3ccaGM4*62*Xux?KB!mHyA#vq6#L;_i)bAe=yJQTQ}pw|8AAPP zM-$x7)ldQ*5{XOx6&7F&w~Dwyl5mx}G_>#4{ZMF4uBKAP_lF{DSbTYJzBYC4HYS8n z2Y)7tJg=U!RJI6p@sEfIWUt3X5qSbj1l2hk1b#7W2S3|e||^0hJI(Rgbl zyML@vqn^e=)G&H87Bv5EJg@d-6M;ciKZks^c;oitHm8Xtsh$sdU1*S!-f>`thwX2< zD-T%z?a+rN2c3{}Mw|1~-=R18x+M^CLd+vtWd&NtZQohvc(p?fdUzv>QrtZCk0<$6|Pb|b*3g| z*oWEp(?mqVP+wpKu6WuCj#$y``SEjZsf@wG{>l={U|fxw-XMASCB}uysB#G05V^`K zNFGoZ+rU6+sJ|(GqdAgC8{OIKUW zs-y)Wm}3>bdW+?Ra47{8GbLL$x#f~oo9B-cjl;~KOW#&|_)iOB)B(=5;t@=9Ie%Pa zc5AnZRAXOO8ltRyFG~>-S#woJk)O}{M#qg?TSj7tg#bX$lBZKT@*O=jZ~?_Dp;ebC zk@!UB=W8^4^?yc}u;wJFdL4kGt@YJQ=6E1>Qe&9}s=gvOEKMd>erm_LNOA9~DCBy( zhy>F==%bdHOO7}Bn=y)OhGCtO`IJ?eQb{q1GmgaLw~*(r>4Ter1p3{6S0hC7vtJU}#Sb zZmy2CP5~aI#qX473^f@yukH!u2>8aKf-b;(9`lQJ?nWygkFoSa6T$n7 z_+vzmXMYhQL(esnfq}?~{CvZoGgcuEbC`5ps_83ugYz@chA32KgovuQWDw|yyO|Ae zD5+bphbyeg#8-Zh`Vj~G8^cBzD|L+u8f58Ob#3kTTSu9>Rg1E5mc)B!6H%fZ@JSG7 zRggpX*?uLiq#neiF&TkI$>&?hv`ULBag}qf2&+YU1ehuO*=h`r*koNP`Mr;ZyWO1@ zKjA|dATbC(oGaPp+rV}lWt0;#Ut+XH_qi&UkOW3lKMPuD-4%sC+ zI;a;)*+7+$ax%~_)_lJlz3?R8BmR;wX=>~b;3!=(wUyn3D}(qOH7T zVsR}mk~5q8P61Ib5L5ina9nc2c2H!$r{a{v(k_o~8a=2tl3919(zZo%N?=cdQWK%4 zr^-m)40%=x!xTFMebXW<4}65q#t~sbHsZMz|5Ql*H--6aRjIMLjj}YopwiZoiy%#L z9O|-680}Y8Q1AR%%t$bul7lUGegnsXmUQhEY?C;`Y=EsC-UO!FYk5 z09n$wiWT-(T-m~yp2pJ%aN-3$xCD+q$GfCTasaMc6jE7~d{94e8h&gMGP_>quyc`< zT5Fg$RNtHg(!(rnC^f~o?WgAPd;!L=u;#Sfs^B|Ul;w4*3es+r(s^DZd+Okeg5!oz)7vBwy3kJFqD8!KMrXD1@;5LFU@=sR zr|4k^y3KMeJcmV;bCQRwht#E7W4&6;7ddsvKKZO$9@Y9H;4HI>52#*6wn@kEdSO;V zT|(Xme40J-*>EF1eLn&O8V?l@fa`hfz^6vVS^t6zQL7fl@! z>CKPk>F$I8cgRyX{7`X*Zx}(94TFk{IP9fUfnSR;V&twZfgABFIqPhSD&MdU4piNuI6_joCv5B;WTs^A!q>waz6)H<`;@2*$ zlYodH#8?~9o!W}T6;_o|D-nd-`B_Qf+&TqA?$d-+X?xhKq1`hZ@tJ5o&YvWIJpbOij4-f5cm1!|~Ri$N&8lp(hfPm3167t}M#h*^F)&MqBa zh>pr6<=xe!yY^C<=44WxtKsk!>(jWpVxB-mV0uPN+Oc4jR3Y1v-3xnhO!mOHE|V(gE%xw#W}gr4p` zdx|{ME5XUYHk8Qjzj8B@!mP_Uf4oKKii)Y~E2LgCzd3y{=VD-`P30asu-SnRxVuZI z&BCONObxe#j>7*?UG3CDW!wnlGC1dOvC1)86B8XkB&;x#Os8)a<;Wfm>!2i0iK#6p z=gVf5z|5zbZ(?IWXp646WQt(<5fhVLCZz+cIFEC;Rqev;hX=v~B))r#-XP72wSAk2C2<-x8xo~BA+w>H?5;^5^5Q^9kCAK zE@U6dn{8~}3xH@~q!$Oq7$ezO?YW(7r+VC}?JwyFq|;^BZPP*vJQ_1&Z1c+~7E}bS zmF58Kp9S!_fc8VfJd1G15ijR((8efibS8h@J@Dt}&s)=ZTTykuS;6TtF|K=JU5>Hw zKL==5%aCFlS`KdCysCZ!Y?cEEYV$2;1~KP|)I?s$=*Bq>^1u$QGS#}Ix3Tetn~c~o zFY~aCE#620IR2OiuW74{Zj0mYW+uiREkZ$L0mVz|;b~F?0;EcOBC+3Wzo=Q^xprf)R?k zup%8$zwjl|#b?(w38G>z#5aX(HT$oNfO{JcNt${3WrppE6b18aE1tloAoH5+7HOhz z8AANvfTsi%HZ|0>A;nl)%{B9EcqO|^ihgtCUTw(Vuo%cQ!@h4o;U=W{q|y)ofSe7- zmXq+5N=LUqgJ>WA2IjU>;4`Kr#6ME28{F8(I-CMna+m#u8E&LYdQ@PT~jl=2v5^B0?;;OYnyum00^K$50E+o_L@I0CkFU8heV|Bj{~0 z);PW%Ud5(O@Vx%Gx3l~V{+224Ukn%lhPBfrsHo-HmzrzP4Br%<5mpf{bQIe<&(tt zhL6W1RyK^ns8Uuw#xlLCwlSu;UDob<;`jP$Zh4FsRub%R3i1``n)zAnZ36A%j>Xdr zkE!Q%ktzDDzcG(cjn&5!u&lpc5UT!ARTHwxNR$S$p-W6Uz*}v)$Hkay#w~R{Plm{M zr5jWIa9P1T3<9O#nP;p1#8ZSimvX3d)#ZyHBXg{3t#4dYxu61{MVaR8D+~XovHFk* z%I~@)*DR0-u{?AhB-dno z{cq%T8|ag?)FX6`Ux{slu7lRuxKIsMYkuR|DO0neF8d3Msn#zOV90`{Gyzk0@*J{6 z^@oqJkekk}b>2`nK6IA$I_P`NM-L?4@S5#7IP70SJ2Y-8Ged@8q}2Ap4R06 zh_GaKgdmmO3zUQ9s0I)h_poWX#_{MlrFx>qVV-8`$kXaH@|Gigx%Cj07Dpjc0X81K z`8GDMJqxOsq9Cy&QK{T3Nk*-RAiF@DUOo4O9zPlAYb)qMdJ%@FXg@Az3j_qoQveQY zt;OAQPSADeOnz&*-!uE5LW^E(iF}IQE&)4a01l}E25X;WGn)-pzGKZ_eBp#)vJYey zxz_yDq>klgT;2uiekvDF6pP#K;BbL7;W?WHbU#iYQ^~reKcA2*kK4IohR0shJ^a;# zzOB8Beib`%%@8O>sA;e`l~UMiK=9&EuF)FW>uWy=6E@gWj0j_ql|66H=W(-acor zuX%Eg`Tj}q*-Z{stvmgG8(87OcrnBdP3P)S&%XXCxc+q3+c8%ay=KgPA*C3Aghe&9 zT(l9~O68a-eueAX?e%7*EjjGdCK3z7d`~W9Z0stdBE4u^wki}(3iSK6h?CwUWz`YG zpL@5^>QW}*os{s!tKGg80)uah$Tn$oJQzyz=ESbD&k!vB_sL)!%iQV+8lB&D20QaZ&g|fjXbBZ z+xEbml@__)M#3Nwr9yZ zwMuimoh9Lgke!(}?U#2a0VI-zThjpWgD{x6K-QJJnCf8iL38Fy4{|@RigUPa%GT2P z6kc-+?PsOO8)I=<;c^4t{2lSJ*NzqUDfRfkcc7XZ{mwU!3cuss5$U@)I>eoktFx~b z2kq7Z7>aSs4bUg3zLKzS#v zI|(0r`#@A$(%VpPI3aLdC;?`SGndS*1bY`+l*ET}hQ6iS$W{~d3?<_b`f5Kiw60?{ zNcRx6UPTLSmKiTb8Ao>uD__@R&~d0c^5^07J_M@xM+f$oS6-UnKVgfdQKW+p0QCK^e_@o zDcYf5p(-t#K3us<9UEo&U@;tq#MCyUAh9!_0>to1gXTLNE=GjGdJ408I+jAwtFtZQ zvQ+}?8cn3aipX0C@ogc|^ib%l7(j5wQy-l)$d7}qx{2xUvv8}{vV8N_?Z-=EUK*YK z5!}FHOODBylbV-zEU0QhyETa5yo?SzHVL!hOL9*!;Ru=05uhQNFf?;|9!%vVB6N`w zfs>hSS&Lt`Ti{nQq34qWJmZ0j>S50Jch z4-@yh^0WX%C8^TG_2xNK@<1(7@}K8D=gNNd@}C3?i2#eLcq&MIkdov(NcvZ^ywh}^ zce$in&^jG@Q>Av%=3DX-pE42r{x7z#43d=PLUlGl3PfcFNXRs;h)+;4cWbNA%khO_ z<~?XfIHsw^5{l$3BMwW!=wb@HXJyp@J?nI~LGdhGDjw?Lyog1=x>9;-V@_)*LG&%U z7}v`)S}TlhE-b3>)x>EKjtAB7@3OS8k@UiE-M9Qf|QZ&p_C-x8OsmAJ`K+qc4?7Z~+z{5(Rv;^!N+(tbrHbw)L%`v=KRP zJw_b6^8cY@nZM6g*vT@_q>MR~e_C9-vSiQ)$`rcPUNd}Xy>IC5z+DVHK0scagAbT+ z?nJkIjiC3CXol55Dnn`wPxrjj^dKwHVlJD4P8r#JdIFe6?Me~atue?Frza(dK1>8D04!By>H_W2xh@`lipz^X;JpK0UWr zs1M+PD_U+4f&ZY@;yvX@bEQyLNBrlz+lPwH=K0>b`S-c8>Ns(Ct}@kIj)!zw|GxZS zlxvjy(9+bR2{JfjE&Hl{WXj*AlxLy)fL>;PVmXV6F$7Y`b!zU#^6>{G7P%q$hViIF zI7naZ5IUkY!v55ST3NaSB8cu#?e3G@m-G_njJBjVoUhFS{@+@b0!vyA5UYNHyEQ!e$;?3> z#YE6A(@j!;%(=T6X>ZEfr0Ypq9PLF*alFL*bBB?q_0!c@p!SZsP+0fM+oWcX7`mFi z#ETs~KimTtisdb@U~wcue97ouki24qvzU~eNKL~B1#W^aCI)PLm& zJayPj#sJJ}%ba(|C+bWuG33U172JePyM!Jta{LLO32$tuF@kBg7Uw@=@f>Uj$Nk>qO93bOuSKKKP`OIi zzf-f?Zp~5dIP$-*9dVlQ?Lsun`1n3x&hJ+TdBRbF+-_hVU6TC97k{(U8pmOl3pZHC zUC*D;Z;HCi++ug=DUg;8L!UnSgU?IuxZMRiM)bLSAjRpwS5<}|)?LIhT7+qgAbA%s zs@AfX*k$d56r!o#yV%8z`G{@EXa#h?DPt=D8JS9~}(08_&b9buL1=}UVj<^fL zPhNa)$A|}K@ZxG%V=v~eyEGd-=rW$@Q?hL)XIAzpS&)3BWfDvrmBe22&DKPoyF9a9 z;B}fVXXBsG3-y4Djihx-I9-U1hbVE{5 zV`t~5+VuCeq;RCSI-yvdyTHWCmy{aY$Viv9PBFy3lE2R}@DKRkBm3Jg?LClx_O%vx zeoFPM@BWzltR^lE6UaMb1^tw`%F=Y3QP;p4AN(YK{lcdNl3&Hg!7wXd3&hubui3j} zacYD)K2!7v|3FthH(+XeDBNxnnq%a7H@ME+82~TdvpuT2sU7rwCUvXIXh_xwogSD+ zzKk4>LS<+g=2Hz0O?R?Xh*j8n2j%GUwvqLWZBFnHWs<&}O|OX6ZfXzuwsd-NPYtbM;S4J6N3q4nb8zy z1H(^Tz#jyJfZ%=1zk~tO5ux~E(rC?{$-yI7WiT9?)x|R% zyrvk{=S{`|kJo|di_LED`d4=Z2qbzsIL($*&^evcn!8I6S$A|=&OTgsNU}$y)x&;_ zP4|9kuQ}CrTCQV#sl5+Rr0oZK;&)Z>J(p@(p+}uudIpx_l~Y~xB+jb^K^)^RCg?ru zq8Wc=S+d4DXZt4A9&O>!Gs45qzH%_$-C<`sAzA+()=a-nGG>+7uwIb6ZsB2mC8KN%+H0K3BNA=ck_ElzD+by>Sh=^yab%Gz1A7SzS#VfE8tea-hDVX*Kz?C5%BhW-#Jjh`1jP$JpkOi z_p!cVXg9C$Fisqy2q%l@Xg)J^bc_XKpl!g_Q*joY0?q<%k)Jb6r8`}za;*r)sT*3Ta|V!b5X zMZ?gqx5+N?*_3X?Tiw?1$hHPF{ld=G^@L zen5fi`PnSx&3Dwc(&CF~Ul|D48#jkx9-p!uJJ$2lK&)!YjjG8e^xBfLx}75l6%~7G z6_QR!kyRHnT2ufT=t~gSu=cSGZa-F%hjQ`ck=($lDfEix+`Yfg4zVZtY3#Iz{7XW% zYOGUaf5-_DY-zcc;}N{Mw2D`x~#@ z)1ob|>j#nKAs$J$EcnkFTo3vUGM?DpRkNwt7tLT71KiKhi$9In@^80sEYZ>nMN5-( zdxHmXdpcbv;Ko)Z`{~WiWKLr$sh^o5iL|P=q5^ZnBAnGICXXhHB-x!zigus@62{UB zQmRSqW&13x;)jB4*ipk88s3!BJqKVkw$&i}bJJpgJ7?XzXN-JycMjcD^t80oY z8vjKT8c7>g?B{T;V9UDT$T_fL%DtBW)vIvHH+O7O?_4|oH^WBqEM_IY#L>5mJ!Aei z5n?qZ=eL67tW-HywUu##q@vE39yR_G1hwjtJc|M)rU8%zWDo7y^DG<3jwE?hC@PI0 zV8+55Zao}`jgQZ?Voe8)&q zb7nlU62KrR0000Ge>VaE!MHF=T0j7RP+$N6Z_JPsFFPc;u_8j6(^Z-iHKUFNN(isU-$*r z^88XcVv?Dopuv!$@=$RPf2pIe9NbmKPLnqNmhb}0iNv79t@8_#zSL_uUy!Jlw7F|O zx-^p-;TZKs>&fW#s3Inw4=<|n={59-<;>kS*DvwxB#7sYqiV>-QexZvqIw{URc zuT%SSc(N@kVSq{I>4ouWf!*bpKSjR}0wc!t`_;Iw>VZ!__>3tfj3@ga{^rJ#a})jb zH|wvz(0~2au{W}Gpr!fOz9?bhU%m>xyg^NT42$tXTIit%yJvR5ndEwJW+C{<+|UwAvBNCQ_Q7bh+tHF5!sV)-jjz-&a?G;AJ-(ft};38>+=`(tl*|O@zh* zCRe8WD{l!`quTO`fi?^2$cEu!-S&RUpkXfsd)nOkz+KX&*W1-J%{u*z<~8BVm`6`U z?UtM9*gfEk=>33p>@He0U1@1rj72Y~#gm#C2Y8`pUY3iAn8$CZYk1b>L0z!5uW^^; z&tFQKh-M;;8$)niah4B&Y8yCnm^ll|kZ(tlPg`G(f3V-88_ViWgZY$4as&RSmk*K1 zD+7N$le1*-)@zIT^y-?7r5X}C$?o9!9Ln`>4WAJku+HSQ`{Q;u zX8y{Km$S_)R?sLr8pfol#*1^T_cP|F?t^p^Q7SE#-`AwPjX1;_O>97u>25#9B!6Mj zq&iw@h?!GzhI&B_e9Tt{tDJ=f+bnOlEz4Ro{KOy3msO^29aCSo^7N_xDqT$3x}&Lr zDHTo5?G2u@9Mfrg%G{As1u1C1*hgZ5?# zjkkxZt6}Ci1iYKJp6u-H?Xyqv30?SMH1#QjCzh$?jkv&Ee2}OapWx0!nW(*nT-r$Az?^FWvBJAK2Ee1uFP)uj#+6EhMYz~U4+W=0R+D0n(`KXIn4(|xHCd94Lgv^i1!3~d0_ z4~12sSCFnId1A^m-~YS?6NvHuS%0;)i$`*gt6?}L#8^dt8mDIuGj;H&X%y^8xcp7( z_&SH5#mG8yH}hQA2mhZ*x>W8vKm9i@3V&mS^Y8Joa{2#8$S9WH8i*c2=z}Zhh}(kq zcmAjaJ3FD^C?J@}pt?m2b&TKT)q&`5)p_qJj~|Q2XAUsCfqw2e{FN_pDB_IFk_r4C z>5deZ->pDw5Mj2l4D~1)3umq^WxIKWkwxx?bAp2bzbvM1#OrYAfnE^LU1f~+w~I^v ztU}V1GFLnaQ@fe56EsRG#+{o@o8q7BQZF`3ZjEx4u8vsTGCt9>!Kib|%Oq>viPB8s zAk^XN+ z2)X|K;5eAsSpO?$wW2wY{OM2tJf!2q@Q?s|kZb4H`QdL}Ik}0%Qi;R2w`EGcuBXqx z1|lz|xnZW0J>w{*NAoSfHj+N=xx-;i2>h*d9h1es3wzhP#T(P++}Pb%;$nb7pW)VI z@41J~$c%ckVFDLq*%6RS&_RF@28#OD8c#QEf0&fNcdAaxnl4K6^<>(J zbPEo1c6T81;Ewh)k1d&`9dQ@Yt6Oc7WpP>tpaqAGdjy@OU3oZyuMXP*s`?oQd)GVB z3F?9R2@9fQnI5jSn1d$5JdI0nVVxqz9PX|_nu2>hXwfS1@_Zj3IRx>uaF}`}!Ypl! zwv=F2TZ-kjXpx&@ErMUzUDt|@Gm98V)lkLH&(B9NaM=*a=OHbm~G@nSa7 z_c(&SabujW<@_;ek<4W^ECt^b{IGwFXlG}a+{e|t+jW%_#WuMj0>34D*uz-3CKb-_ zAE4k#_tZgNxCAO`gK7Pat})yYGh>6Qq@n^%Rc(;HFBs5N&JY&1ugB~W%SUI)Nj;A$ znlD0FNr_c_>T#XPJG-nKfy?FY*xJ_SjE%NqdZ;kaxclxd&WQg}`N`s*<3agk&i~&0 zvJ^J?rxFKig|Q+ zc{!Q4y+ab~DWHCS0P3qTm}A}1kc@5|j6QrnFYs=@&MGwB*`@*v%jz8MWY}AR-ZR{> zhJlG0Ry1%nO+!QTi{cr6w%vD5-H86a@73nv}qHNus0l^;vgKG1koX5M^XtUlO8=NzBgNO`qH<73555faP zL70V58QMr>L!9_A?uqPrhmG|cEE1N`bS&da?EVlntLKL=hG-lfh1ar&Ew{N(Vyk%h z&Cf+%qruUM!BesAahjBl$%$x*8wP6s&SA-z&(7avnsb0j;QI>dh;uGBnNT?H8$~UOCbw1G)53Z}(#C z_=NT$a}|yNEYCI_ny*w-vK@BWq9#u7%CC|kMh}G?X#lb9E%!)PDZEAb`TeYv936Z6 zn*$3%pY$SXlMCnPrO&HeWS^g(yJ&W4b+q1Z-AJ7C_Mmk@0~)5k9KKyWPugO>;7qogk<3p^ks5q zu1&Q)MGju>lxvpc5Alu>iqD@;iorYwpLX|-=2cKytSz_>-er2!{GNSGvhp4;qWbp8 zu7=01hGC6>py6v(@&z{Y6+}fL$ROHz{2@YW$9D75lT#Bj!o{^!Vf=U@aC0?ed#AI! z%-Ev`S4>~X`{!P%!8ZVoqps6)8&Q)Q70!u9RHFgJ58=X%N_`3vb2Kqui(jd5uul&g zZ$1_JyqJyjX+V>Mo5x<1-iMt8?&i^_;WWCu1`v@5RsvZQrVQNMH!9|_dNRo$s($Ls zvI)#7S?w!gi8WATQe!J|l!v$0!p7Z$9NVW*v{dby-zktvBM#c7&Uf~~3}|A!j1i92 zXuX%PztxZvAcU_J-GeXFy>h8-l;rJ?#*+q#TACY~YNqFPEhD9hi;ItyrIuhzoQ5Z- zr~6ZX6K5;p%r*NdQqNVQI7vj>@YtEj!>sNBjIwN&mnI-#kv-B6Nj(9t+P;6dV7Mod zkdj8rK@6L%KVy{^!Ef@vaDu;Xb~){xCZKR!R905@&(F_~wk5e`M<4s`4m#!+hAwPW zXdh-~gE6Lql24FfD-%PRO~EncaayHH{8n!U2U97wb^8dqp*<-=Lr0e=vJ|mG^lK5+ zRun!yoXCNKKmRSEuBjQBZLK6&Lqn^m4|bYmprjRDZ&aabwo3Z+#o_dYZ4SZ5$Jeb_ z1vNfiORcI0TC&~kHBmU9&S-t(Ofuuh#(sX@UzqkOtA+XbE%taK?o2S-mG;XupK#xE z!0Z6Val)x>lU@TL=Sk@zvC*Z~5TnFBvHZ4>YGPXE4Bm5j0QTc-ZHM6sa^?AL`HD<> zXHgGmNq&MC(LjGhOV&_Bt^ku#N+O|=P%sa@J2G?IBjl%Oix>wX6||wQaWQw5S_SwK z(ZG8bz`{;h6$GOAB8LhX0zUQQle=@PG}&!}Pd*mIikLtw=K>`;1>0}=Uc-7cM52U8 z8xlq`US%`BAqOgV4&>FA@z0-m^a(zEWri1ScSR6xT9j~^?Fd&iceMWYVDS?ZkW9j_ z;fN*O91%rx#8h>fa<`{0`=z65MqX;NwexVRc7wqZ0~-(bXu`OgWszq<2+^T|eQC{v zwCd`pC@DR;xNM)zOAg9?`$tThn@&3ZY=KM-earT$2g5w7wh1f&vnf(N? zR}i0-ALgYd_1R^cy4kvbAB(&rE%?ThC4lG>P!QKN;V#*rpL!WdQ^KH$nXtx~oKcN+fsXlccxOb;he8dQ$F)9K6vsHIc&v5=z z87xGmdSss>-y^CFI`}^2a8uG29QlSn&Z~2};RlroP@3XNee7_y$+2BOB?G-rP?=(S zG}4nhu$;iYlZ2*wxVHns9QAFcbN!pn<8eOLF_3DiqX|{hD#8(O=gg~=zbF`Gm9?z$ zq}0*S?Q44GP+!I7#U|WS*y|N5Xq+yGDvnJ$j@K2T6+^PCbznS2i%5YR$Pf7+9jdFV ze+?w}rs4but7>p+AJ5dLA>Ya3$2saml38cr5eHh&+tqGZJAQ}E#2N~4+5uPk)EP8$ zt~RFyg&y*=esB5(d+*FvcBNX*7k;B+mRS#6I*!Ejr>A3`;av|_u}gGa0_fgC5kSeB z4moz83NR_yV-oD;3cmzPuLL0k&aUw_px;OzC^ zuNT~|10EGoq5Ujhtt@?)D*W{{T|4LwW}h!H^g~X18?#GDJNu#@ zN`-l_TmFGqzd3x4XNszlvXk&cP>Q@Q%T}RJWz^LiAIFEkcAk<4^sl8&HxI-1iM6vr z1!wjLN6&&a5tQw4*`M;cg3{dz4q`6Tud-(T@N;KH`e zZ{!Hx7Rl0$2z-K#6gM)ioFHx=x2{t57XIA}7AxQ~f@&FFYy_Io^$QeC*)5MrdtAlh z8fTe}TC}J_l-UB;7uhhQ1Gu-QeXS_L%E00qEItEkg=IboInaw|(H$A~etM73^~uFJ zB^AHuX6S*hT)@@corcyvgA$}0Asfi>h5$j*^IW6W`m1?sa!fs&$0P&7^<2xK=ONQW zR&^=`D}o0x9NQ2StQA`Tk9N8-y7RHECi&QJ8=}YRCLw|bAMn>C?=`Y)*Z7!dxynXL z-1FWN3F4L2&E9UG@CDTSU_tv3q`91tQ4u~A)*m2=iHV5;aD(A>1aohSTqRUpGf|(c zvp`u^PQId|HzrL5BTGMf&hJvyUY zK>SjdG|lJaqPv381dB-I&aVtbU3!xckZG7#rGqolvA^f?`GQ}YA|R1Eu*ZLc1Gg0k zGy-h{fPu$DTO>#Dd*O@*inxSnT!#Pxfs7q?Kz)a?f+IGjcbxbh<(9l(3<DF6WG-|^u8ovQlhB-Ip`g~I`J zd=71of6-KNy$1uXn0I6r>vlXfM*-!8v)H|)mHcmXDL$lSlrrB!JP(hbuXb$wA#hRO zbg3;8)RE+NKVjfgbgZa_o~~-IleDnVkUp8LZS9tW+Fs}rPSx*4vHhWsgXx{=ZZ@0{ zhvOK3A@XHwnB3_T&ha&tXd=E_me;T8HoKL}lIcY+eoyqyZ&4#uEV&G(+0mAWKR&;h#;`Z zsI9e);G!ttBE$|ju9cj6R^C&Fa{IC%Dlahy-C>wlLH{ivL}}- zFC!jXF43QB9BVxJKQ8clKRu`!a#u2S>SI{{F?xK5c2sbYrCLvqww=$8mDs^0(l^n6iMz0Bk8a$%K zO!V*fP6r|WVGb?*C%{}e6Bs%J#*|PeoNz)$R(Q*@BC}6{Yt`5Ns;khC5&mV2KV~%z zwhmL-X*w(YPN&myy)nT>VCE$+_e{oozm!@OZ%$u9TIS?hbhd@q9(DR{y+G56QIgQ! zJ;2Gn0ycZT+J&vrBSMF;kl>`BpJP7A@M3tLU*OwkfiCZ2y1ELkAwQ$5Ts*$Ke_Dpq zXuh@HF9>1KJlULqUB(0{))^)x0RqA`ZJBv|ItYz6*ghUyygv`{!v=1W?_%1nDamug zQkmurpGk%Un%JVYo{Alu8+SN3*MD3$E6zgR#P3SJeq9=IMFGjwGD+UTZ}FFu;9&`S zQ=~Xka}@I?&&2jeATXPX*;xRB^C1iqcUIj5=6ZINp0<$U-ZDR&fnOThgijTqa>##PX-ii zbKpJ!LL-$udq38b((~6nHXUIEnpQ>G3Bqp@$^O-o3}%iLeE5!x7Ub=3h=C}p=?BK> zx2LPw_anBTqY2T(YM{bNlEEJ8fX7JkE#5H1(gcIw%R%R~0^_J^2ny$L0ol!mR8r>) ztJ|L*l;?B3EKPg4&l-jWkVrzHmSpAkuBLQb+n9YFz=mwVLLrdjJ!`e6Fi$x6`QDY~ zCpy8zo3?*3+Uk%H`po?3RSFEi@DUfbF~n$`QCj-cyajqqsvdObR<8;G{mPR!q1^&$ zkJ2m2oq)B}U(}`DA(y!?DD;(*z}l9?7M;wx>1zf~*$*m`rN}_mINTk$|9ia^HzPRg z7QmG=RSSN)A;u{bc*fxSkve?M{WzQ?qNWrZoo743s|L+vn77&A9Kn`GKRazjz{ zQ}d$p5OsdV;Z9ZwNxkzX!{o$>xMS~-NDE6g5DR5r62J~dV3r~ z3!qs(7|mRLPHoVk^2ud$vyfvj2nJuu!39cdvr=NHaP9*g0bDsjuy;Z@YS@SP?9z>GhGXCdd`oKkIpo>hz>I{POl)|XX< zka7ZvK#5eH_W~+caMYBjx01IcZX>x({(?3shrCjxl5mgF%jwI)V|-fgjXQiRU^Za0 zdW43R=ani9eptKI*qXN6Frqp9KGOSrtL#KN;}FU3cv%d6M7jL{GJc`Q#?XAT$4+A0g+!;0^wTy9j{fy=by)J4jMiC`27)8d@WVKH8;;I&>g~gZ4 z%3NoMGK%it9!z_>Yh;efj2Y#HRV=$&AYkdy&{e0W=T6j+8Nu8b(&*?#XwdRL5(&fo z9Th^86$xGTmUSR$=1JLrf0p7%=2ZMT_G>JorHe2fQEpQ^x^d`mw;^szBCah#FVSt9 z`va(ByN{XmtOO?|X0zXp2>IFNm$omyn^t(fDO-oZ(!-?`7sWt9n_F#J@)(M}FdWf2rO`=L-}k0}wdEO`MkyLj8s5BMqZ3?#lm#c!UkJ+xp{f`a zGFA`tN`OEVz1}C8RzPuVRZ?cEF)=GhWHToE4F?QAk@@{hJoSrQgYDK;^54e#tFB&V zosDnRa;SFe($&LQcpz=G-ljp&3StOhYG0+9B0u-)1n26j|A6RSCTsM{Tl_SlBy~UInn#?TI zuw8oi^WIn<>7G0eF;zBqg_j>0Jb7@jxXK(VT<bnk1s%Sl} z^4=?c007rp!IME({g%E^2P||p)em%Nh`U}0cPm)SXe|I9D)%ec4w0vUx}>t#uG$NY zlCj%*E{^>$eVFiWU$B-IU&0ociEw@{IGp@ggHPJul+MXLV=&%fsvb;$#2I*O!NA;^ z6-R^LpLMmjds3Z+ws?jUh$a0eq3_o!e0}^vfnNq)Y9@XJ#@O(L3HQ=+oY#iwmPIP# zG;CJ9@oXKS`h0DW4?{1yu~`0VuOjfWb35@kr@F?RQ%4*Uj3++A$2|kPHhn?4LaOoa zvK#~>M@D3Fv3W=p3#PdiMdZ<-Cib|w-x2jHMUp9O-bhS?jWQHcW&<}TdZ!F5pirJI>%F&{H%hGWIM+LjkMpGg{Us%&0Am2b7 z*InM)5#O09z!}aDYM33vMSw}w%9ria=X8ONuV0_03%fL7b%e;Y>u zBLgywDRqy-MwEEB5_sT;CBeE_So-Z39t_INMRma)m62zqh@7#n+6&k3%$k{fVSFT7 zVuH?81BghO88HOR;}q;0Qp^pot~vTj_YZ)$V^(hV8az0xc(enJ<+CN0)4;800B+8B zzn)qbI3D+j2BHo6sfoN*y(w*Pf?f-tjXfp84IAqm8gISaDOIA3DdOLY?3fFwl3WWtw{0j%)nN% z_4+a6({bvfZ6jRkX{e^fh)qe<(*_t;J*Q;OV6(e9%3e{Y(FHM>J649F!-g+WnYno$ zjm+Vv!sC6JqUmc4+pSQ_J{*fP;fZZ>$9nAv8>9ab!0ym3*}~7xZ?toD8uni^I)X-+ zp@|FuCq2Pbz2X{^ERd_!_aB+&540dFe#GX$jPPLRs$ddl>=wQn_9DF>u9BOQD~4O0 zjL{pMMvmSP9s#w0_S|8X9l%Al*ZGcBoicO=*AEMLHcUk}3(tBW2LpU|uG8N}4ttjO zS5C4pb(>ohd=`~wit!(c!fqT&*F`Gjnh$JyaA<}I;xs8NY+%+?bYpJ=IJ;|l6&161 zS>5Ob8FK&1mtO zTU1;%zkydH_0ij~@&qz#hT&xvqpEc`;6~Bq2bY-dMO;5;-hdp5%hJmw7|=FYjVyG( zAE<}avT6u)VTXxzYoqA?POBK8*ua!C5g;G1k|PKA`i>zEr}q&^Iaj+X@_ZCl6=pVI zrpfr(hi&VC=_fzVM)msx*~yh1xxW*}BvrH2o49Q7pvDMQ+}~i#QO(IYizPjh+$l;w z+cH?h;?;1ZAD)I=th#Rn{}LWbnmx;gGamN)GmS>l0&^hc6k8h%QFUtrIZGMH_;T34 zhwv@z#yBgZBpznzaXQZ`hDg#TE68x6Z;^9j--sVYL6wsZMmV2wfZ>i< z0C??KE_07Pva-l4SSk_)!!7@V5v5Sl6lI26fQ`aXh!U9(!dl8>B$J20xJ211mBr*VRn+aRC)r0)Y9(`f z69YMvXyKkN&Im?CM;?ljI6~nmY*`?AocNZo7FxQpwRUr}T?vhv29Tbj6?Q9pHqb1L zsY+#WSd*X5C5uY{@kOf>(L&3U!RY}@M+;w=A`8JaAI!Ee^RY-eg)jHrIrO9Hf$;c( z4HP7`W=v6nPOOx&aj50#a!!HE2?LA1Gd6lxf*fyk11CnyHByblVC_{Q%&<2(Wh*^2 zaPM6Tpz*ikbgWo_F1H?b3JVe>17IpeI5#qkFgu>$ZCp@nT;5}+nN?;|#Rq;^p*F}p zO%DCCAFQhQ?uT!ewWz&8Dh?Xb5V#@mv5{j?2^e>Zd9diMEL99p8>>8@!{!OGrLyX-$AEAf|0`gI@PUO;PtPl z`DKTuB3)kXw#XU76TM~X-xi+Ynr26{Xou57i_*91BHwUSL_Rs1^x)=ecu23n6Z+oM z2OOj{r;<%~o_f%nT4NEIBF?)+k~tf+=vbwI-Ej2!D`4vS7|}+5VD4)!IWQ==*O_H7 zbyNFHgjn`Dff27D$YWFsM_kx}+ za7VZk@KR^H1NCVR-i)PXR6G%7Y5|~TS$Y{26@JB}<`!$a#0W*)o=wDZIml#D7)dKT zfB?ndRl11cZkop&B2qbv;cgn|B<@)`i{p-%*Bl~VI!oXZZO}4jT{asqwwmj*3(W)2f^swm6 zYN@n$uRQEOxnGEOQubB8S9*RCh&~u7J(Y}IGoouo;-OLO!ZeUG7 z82-vK#vX+$2)qNYv)bt(%m+F^xY;U^8aq=hI@6oS=LYmGIwA%K|BHfKR|eWBKTtIT z*nw|)r=B(vlU6ACR(F^7&h&OU;l;#KKC+(RsNWrtn$vvCl{3eZ&R-D<&sHhwfG~_V zeB}gR^=6G|gTq55mG(A7b8R4*wkzpx?m~tJ`RD~hKdvW4>Dw}MB&?yQW_)&fH)g7YsS$XtI2xa}Y$g{>XySlo z=S;4VnTp`!6!4-+nZ|uW!3=Tuxuzb|R}R-xUave+)s=j!?9+C`{n&2>15i=;E~Ke* zTiN$5O!w4|m`rOs_YT4mL8l9HG~jLk7~*00f}e1-FdWMb9{*G?)94Y_#OGSUy@G_mW;cQnBeBoNB3gbsE{S@-Iblj--i^kNH>^1!r z{5z^ky~+IdzWh`<)coZ;KWwMNGROdgwD?M^W|Gzwweidos1eROz%pvUeBU@SH#S@&i!?FPx;Pjzy$q5obS*d=YlN#7#!aQW z>qh@wT`j%CCP1W3?OC5jDu*4^re|{h@84b;kNn#xl|=n-ErES8k&nWxp^joMoMJL@577n(w#2T#gLb3cfs+K?SApLEM!Mf)eVhW z6R$H(5xMAS7YCeS+;*!D{Tf|S^H(wt@m`$Hlt=WGRZ5qm&KSuRQWgx?flaHR(3b)h zj7Do}11gto%BiRw6T(#2iLKGD;-t!Tu;x0#-pAdX{t9L{!QaKYOS#g;rlP;fD#Y71 zNV}|cu!*m6b(9x=0RI#x8 z;|H)QWM+}F0-@GfU6+u+mYZG5gX2)|wje)f9(RgedDP?PoFNt{IjcjAYJoxj5QhdS zZyXd)n7wR=p!|dgfn}J)zgnfy7T9nUC~O~1ubw%T&;6>fqhYOROWT+Q*j@^hYD!K+ zqn=w@&MPc1+iZS7YVKrsTl*R}c3$NzLaV+VK2TxaTXHXUhLzQPuF&uU1Iu65Xt6lh z^ugNlK1!@)(c!8xSV3+gVC^8iHqgc%s<+s)|9XF#fW}&L((ijfYPbz6gSw*LNUhzW zPb$xY;UTaCPvFXNX8B8g8^ksc> zuI%#N36yk?s@-sgh7pJSelF*tInA9;3+=(Vnq?}(8}#;E4bmxebH-x$orzNd$pq_E z9>IWuFtKc6HCp1URF8EPJDyyQqdeADyH3PJBUI6q^Ac7GQnNXJ4;ik^RXlWJ%kN#5q&XfLR}mLV_-ctu9XPCG=jF9G(1i^q3c`q)u;3-yQi@^%fYXkKp7!xmtN^8{d%*bjFN1LqnnnS;wG)y)R1SS za7NG)FFFXuF{sh-MT-zCqYy4YpVopBxY4fNbe{bIL`0uZ^l#v-Fs{Lil&&V`H0=d2 zM*JQ6+2L6x2p4~-3P9~=`*K8NLI29?<6%mtJ+#NesjErME!LL>2?_Z=4&SFhQ2XBb zZdNwCZagWQAAM(eXmHeXHbxF$SSu;qo;@&G{itD!gEMn9m`h7>8{6}Rd}t}d*D{gC z8{AW**NINjJCCH-Z?fa@KMTUZG8HT8L$KlFqtjcI$4Djb!@=6CQ@T z8L5H15`*GP&&sdvl%w&7BbDA2k}<+jvp`DkWc5fiBvKmeFiIH{^X?^i`B_Yc7`wR~ zNXZ+;`UPqU>hfF6pKWj{=Ul&^#LIboUi*HPkt&)q5vT!No!k+g~$N|twUFs^T_ zG>TJ}e5xs=DJ$3ADu@qV(|V)o=4`jx#zgz5!Y>vqg}<{pS;{L={9@+Rm&q5C?Vh(N zRz;PN5dDsX!QzzmhMZCXjkqGHnDk~4Yfr%K3^$X!#Krb?`poMSg2(;i_TsKjWaP2Z zJ=qu}{a`MD6z@W-}+s2*KPdvOs~2A4T*h2lnPem;8+m_)NYPS=p?V%`wYCq|&s0@n9V z3LfRrMdroWFe! zcHH!`xTz>Q;LDHvoZde{dq-{xCcvES)|4iRi_-@svbR3^fbKrK0a3(j=M6%Y7vBuXjve^mSRvS4BHuE}BR@TAB_+S}VMTv1 zSK9+lfr#&r!FchsZpt`AoF1IQlisnK^(XjpJ`oMLzhp`7QJ`0r=edl&`N6an16JPI zaNP8Goj@*_;ea-+H+K@$L&cC}lRjkeF9WB7G&oM4y173t20m9!MZ ziSq0tWJL}!l#k%(Ej19Eq8(CZs&iVCT4$6gNuq2*cXmCzU2_Vv!OtJN&;X#lF0SX2 z;J#0sh-0lQN)Tw7rd;pNfW~UDjzz7MX4u}>^K#AwHQwIYJDwSATkiiTNkiz@$(`Xm z2Pet)IlCE^p7}l${q!^!G z?QOg?amdbr-5o3P`%eNW-fyQb+cGi|a5&{8#A9z@W~29cvER<|8!C$RT$I2j0wS0t z@9g&6;`|%Q(pF4lnr_TI3T|9f4lY9&s?oXrDHXwD+i2_{^x{Qyq^kBsy+!YEM`riW zaj`(^Rym3v1{&OGozyunJ)?F+;GVhoy55JLct_95Q8A$Aul~^R7rGPZEL15EA6Nvu zdD-+`XxMtT`e@@23hETiSLsQl{?<#yHR8gQ?JLzfL2WJK7Rg{)J?01gK~>x<$~b=- zhFSa1J{TdYa0=h0)9Df0h?r0cbMcjt@wk5f?n*aY#mN#OMp>m6m0?)Iov2|w#il3G0( zv-*M|JC-+)!A(Sj9kdE(Qf0rpR~u}5Sx*UuDEsV`ISs}N$Dryj;XU_}*Mrkq;fSw% zFRjkX^o}c3erw>|m$DpZ3$0B@7gK zEPXA4PRxu&%SV61syJ<>r2BQJ?}ay1VSs_*)Dau0kKeSQ?s)H4qyl2?k`<*qX8h6E zWP_a8cph}vtVAQP5x!G3$Rt471 zy{fD;;q_jxB$ff6@_U<4N6S%GBZzCs_ipqNAVafWH-!Mu{xjd5k9$UWxkDUXj6N zxEi&k(k~)y5GDHDD{Y`_JC^yz`FQt&SN;^yeE(ukwA8`7=qY)377rDE`slZ$*y@ii zMPDSx9Am8>Y=q|BPN<|@Ve5buP2U$`(y&DB)@&e=OgwhAv(QY{_SzYain!^*ONRGw z$j7zXxb!;_UZCq@5P}BCSONad>fC$*_W;L}X=g%ya=CYee`zNp`(XN`PLY?sa-y+z zZ>ksNw50YYGzXMct5w@z{j4+s|F#Molxiv}m=)-`QT3GHU?1ivGm6)jo;mx5_+jh9+v9l+>v1`A z`COx>e3ln*3_-$j|Eam$A+=?{lrvF@KJHoX9;QJ9m+}~RJ?=U8W~@Pp5?Q!-iq8E0 zrcZqt5t*LTs~KDAmuqZMEA9v0*_Fxd_b&Cr*jLE!;hk^eLXU$md#olT$b; z{wgdN-G_urX18!8W~8n{y9MOeW_U>?+jEj-VjT(ig&{5uWGMYW}|g>Fiv6YK?2Yvd zjQ#~LIL&ZD_tPPO?RNDL8n)nRgfI&IDhyy!w1dBD0=t)+k9d8Ef`Xfu1|J#N`lx?5 zV|4S?No+u+p3g+8?ZQsXqQJ7Q^4*o7pJj8(=eHN}=Z`Niz) zjEg?*kQ~Nj3)e~#_-HqL;K2h5jJ9(D@w3&J#)c0=#^T%U6`-Jj3}&=GU0E?e1wC)3$Bfwr$_pac;!f`@iR&tca?pidt{g zdRInfM#iHc4FZY^1Ox;L)B{LTQrFKFME(H;bPEgwg!1>Pwurr*i>aN9zKVx~sk1IU zz}BWQVO(y25mwBXYz&=^iWX7$aw1?Dxeb%Q$BNH(p(6>P=PS?5l9U<;jV!&s{G6>( zmf}^giY}ruwe+Esl$mF%TVkVspSo!5*sn=igJ)r0djt`cNr$m`w!>7{U}HQ9LF`Lo zY6HQF7<_LrZ*m#%L(24|DAX+o|4>y^Z+uBpSQ^X}p{w-D5e`T8;%Z@fiA|mSyz`RF zYo+7~igEca6z)c^9Gw#CaLG-dF@hxkvNSfGlahcTVMkdoeP|VG)P1FuQ5-3G)xz|r zA@}Db%ktbK&}?-QCB=HY_BI z7hf#yZHL}|mWR=z_`Vl=XbS&fp?6Qd4bEQ+BSCpb48AVCrv z8eLr`GV2DksfpP3+3_kL8BqYzjAkI(rg5%GYg)Gv1k+@Yc&i}tKu`obQyat(u)TuF zNQFpZg8Mn!z{5iV*AAV8iD~vW(|3l@yG$K2T4hGG9*zPY853v%UN`MwJ%3Kl=aW_#Uk6TYgvBG;voZdgHoA3Hwy@bSf0aazq;curCGT^M$}KVId=<~y@nqtrK^e3QBxM))?;`gmeOn7P2J5k{?3}-*3(>k z&ktJ}lkRm;O$IiVsxcO#3P4gPb!w_Hdg_rk;9yIRs(0cvBeVFl&_`nATI3D>6b0Ih zRp2-P64T!n&JB)LrkL8bKSpg2(8dz2ZAsm~tf)7fZUKEk$pu)t2nG`vx&cJo4QP3OgzB(_j_;K} zy1c!yy0Dl2*e3(IoFKR>Sk^NX(&vrdP$l6>vl#Xmo9@8Wh}^_`NM8}C!Ke(7k`v_- z{jRhaJBcc)kA=0KZL z!3}N5L<~h-DP=3$1P3kz6uil`+a#Dkop&KCE}Or~;1(Ap{l=dlPvrQER{Iv@KMSK! zW5+xAw;(_N76t*)4kw8Dw2B9j`Vfe`mV1`e4K*~S zl~1!K&A*7Q`}q6vmWRT%pE}-h;oYzVE#VmPl>01p{B&26#!~PHGD?i6(CpcXO_x{I zi(0FJEs~fk!f0v9(pWvy9>oNqNC^98>l8C!s@N}QH{z&fonRtmdCIdmWPx5<2%@O) zB=1q>NZjx+KCN4+&|?IbQM0~yDMC3~f{3mXm+{K@~f{KWu&hfQZod%J)2QQeq!r#?nRF<-F)Jp^My zH*t2=QYy$#t4;%il!N$z^K0W=;zz#c38sv8oYiC_>;QWJ5u5n|lS`lg+lPBM8k#v( z;I#p4HuhKUz-lquRS3}>eMqFhCADP#I+hORf;T zP2_cULkulUU>oCl~E;AdlFMPZ7V=7Og#)+1N?xarAS^5RcQTZ+;FE!rP~KuT1s zXAFhGjpqgIXTehezK56pNPm}mxOyG}2uKqO2nhY((|56OwKcLcw6vjjurps&v*tvx zA=sXv;(AQNubnwP6%)S3b(S%S;38dM4xd9$7qJp4DopBU;TlvhJ7mOn8i`Y}nMK8a zI*3c{rvvv`}}Krem^ZDAq5!}SW&1A z{}G(V*_d)H@b{6vE@+CZx(-M_5#pjd4oJFf=^wZ!c9K}`><%?fjwk$PRk6fd6lKF= z|MJJmk}M&6O9iKX58GZIPY<_|-8`q5vnHLT$j1!*UyDj?3gGRw*LX`NCGe}+olzpB z#z>~o8zKSWQ=>i^t-w-UCT2ye)C8KMQX7u9x1rV!;CfAc^}H)e`sk6$|$Y# zt1t^ozSe}UkLVyEFVIE5z8?j-QXp#ywfACSc}1Z1)}Uc_#naSp1yB?8$g0#--WD-e z*4L*s&P{Op;Z%vX*DqE^b#~xcb+T~eOw`p>vrYs}lysi?$>Mzs;}NtcG*XAx3F_2J z7Isz^4;vfxxUr`9?zmE7>27=N>OFt&LpnQet)sGm81HlL6@-fTem-_YQ|gr`2c&_6 zgWKioI*d8ur=A~PWa8cyHS+T5F2ef9L6fx(rD2Z7I-8WARMp9XV4_T-7!Ew$rq`yl zj13Jb7Y!bKXsWBb~adixRjgP zA}Cv3U1hW`ZYPM^&{IE#ySb?g%J8j#FN@5+>>QBT4v)npz-KQtRQLP}bQ-LKR<&8S zB$+?u+akk5VylVJyDQz~2F_4!C&pmyOW6~A`acuu55{gX9KUBj4y<3Q+IWGNu6p=s0P zTINwIMMdSo*IaZE(s=`xz0b_asiCK|Cs;7s*52MZo#l1caLtuD>#i+yNs6ue_;7l) zl9qOScsNqoopmFc$6R^^wy?CMHtb!Xlt!{Uti&rsU51B`g%tkc35 z#kY(=oiN{!Jf^&NS**Yxsa0QQuSVL1YVdb79I5J#tmgn=(CJa%j`0Cf9vq)u%yo>F zQSAKAvJH;&kF7Y_*~Pn=;|q578#>nxmKZy#K&rJ)`D`2nJn^86)OcKxPot$&B z%nO~1r0-uGqGIK#WQ1JpPu#GE4QcxNoO5Rs@hI%+Rw=h~-UQ`DoQp$41_N$GG``Bn zL(Xrl!0cJI&O+m5Dz;f|0Ly3&JN9g^qdE~`n4UR}zBSeM`Q;GcAIP`tU zq)Vr(t!8li#6COk2eRPBuO=Y%3rp4cv5DSv(|FCwca$K=$%O^G;*1e43jX%?D+*Ru zUw#_7jFn$@y{~_H*I+6XRy$ z`yfE{i~w+kilzhDOX_>)Ujin?q-}_(LbzOwxqxT_PmcsY`*Ql&xUuqk5pCE zJl(&#qNkjZbw--3X@Cd~sp%_w78b@ayw&#EjKs(XtC|KECb*MH#c0uV;m&H{)V;`A zKyo?QTf!-jr1jp-3X3UMo}lBgt@5Y}1H&4~jDuL_;Gkzf0bjiUzKo5XjG>^QuvdCr zmU*Gf+;x4T#R3(|y!H&^0`Tul{EpW&td)X%Roy4Uwex#@$FD!{ql!TF*{+N%t)jh- z6z?C5^n<5Ghk5IRXo)2(Ke?OvxoN?+YjSLc-PS7LlQ3P?`-AbSefnz3ASvaYS6k<< zI$bZ8EL11E*Cl%SwvsI;up)UNVEBzn`xxdT@rxIttr)pA}&w~Ul6TB zK1a`t5uCH!Z8;WC+7gx^Pp(7ux4^NCr5|-1zXb&HT9<;2FWYFni>d-U+BqE^-E2Xz za)z%`oF*yHqfD&9YgJzcDUdpP>3ZFqVp_fiZ6(*&F?FJ zQK8X%vqVcmZ}^X7LJ9NOg2kT!314vkxe7`!31s~y00N@S0|LVOy9{u)v~{(yw6kU~ zvavU|HnB7`cQUm7*TQF#&)RvPJz@KvMy1Wh%AR3A{sVIzXIQ(=)YQdvm0FUeN}BpG zKvJSFofs|dVgJnM!xe;Z5JJ!|U3T+QjkjrG(}MR6D%bHgwpn(@Bahci@x{5>cjny9 z;zW!eMqR*t-sE++;yrf!nw~sfKG3lx>Lr8}XzevU?*V?QJ$2ia8yVC$aNBp+q}>ev zM^exo*=^|ua96t9)kw(KMyE7KJ*CvpF8iM3M(2gDcV@cZ)qEk8l@#X(3>@4G_2vv? zZ|n~Z2roR_CQIR8y_IL8&vm{t0;~-TK;|xzTv0H#1};MTCfH0TH3H`4aiC-?k+H>0 zqm~n+Fpt&CGgt^fb_=vF4BfQi1XFH7P7VxfN(NE$fKCG(=t{hY7&*^QJ-jD;vow~R zCCB_4cs>&*59kv@NKoDf5-oZ*15%>MXS&HSkP@cO=-g1}bkZJAn_(=Q<{t&O#AkZK z&WsUBEC@eXTTsyDMsSSi15X~=wJHy8G;ruh;9Hu_7*LNJy}}8)oKj}Emn_=Siip0p zI)n7Nf592vv14t;Xb9SCO4wVrNG;W*!NagPd_Tyak!4ZEHX$F6`5NRAbPiF0UH-O_+Ej}LaWn1 zRMxz}Kyv;lMRx)fkTL{wW$+DTdo}*~agswY=IdpCbA4?p+;|09+rBqLGFJYJd+xa` zq^>~U;E^U0tIq6H%W+bC>z*}2vn&vQ#Pazrg&qZE4dmXC&EzH*BL==bm4XlD#e7Wd zd;kw$4bLK+`oqIRLtsqE8v7)p*%r}ptXjf0Gxl*`XYeJ(&~0zVTynsg=6#?i%a5Jq zsI5u*S6=lD{hy+-5A#a;{7brDdH&&jwxZrD*ZYRSevY2@TgZ$ij=H&T=1NghF;nGA zDuUl^5$q&2BuLeYCwQ5^Ypznq)5ss$1#q+Q@H4Zs@N>q$v$C?jS6&@Fog6&9-Mj&A zo^CEn0{pz(U!9J2Q}+&_OAmMuoc!tJ{fiS=-XIq%Tqj_kRD2Iw$dYxM|WiigkK zn4M@p4M6!qNWZ|s53mz53Uxh@dgJk){4`|8S%;&GceI@5rR91 z$=iShrQWnuV1ct8%8?mJi1w=}Usy9Z7p^*Onv>RiCjs=yZqhKo;3z1U4fsg!a3%3` z`HoNmy2M$0pHd;wutu|dC@7Nc~w8(sZJiTOnkCcqSOwBIuZhaQ3NF=>5q& znPlJr)2am)T1yy+;Z8`9&$APVFYn!fYCAor7A!)D2mf8)Zt2o@dU*hiEEu_mii(4# z+#~(C-58?&TWv(dsK_?qgz@0-FtpyEuXG{#G;#)Lr&&)Dj$xOV5=CQ)7r&^C;W#O- zgR278S;LOCa^TVnj-|LBL+TOzvfv>t`r9NB-?iG(0deULcJ)+25-9NF;`AUvBg zxrg{PJ1Dz4%P@QWa3g3*^jF^eovSzSX`M+@gCaf(;z!k+dtb1t{lIMsdoISXLx%*i z5ft#}ff>HnsoM$+o$xkwg9!<#<#+S+C^#+hksYlcrMZg5HP)lDp?EJA8ZrC z$-6LyVwG7Ja($W7J`;pl06J&88xSMG#Ab=eea)5M0V69L3DH$Qm87xN8Mj>Wn8ap~ zXvK&Zi5M&~Cnp$3;yP?g+V!i3zCQ*QG`kWBl>XkaI_d4wTpOlm0np#6jF8yst5H{} z2WCo*cmOyHx7agL6{u0O8Tdp6)_WV+6(tM}E``*)*$-ZsejYFN)2OWy3%~&js^bMS zcT9N`45U$}ylbJ|%Y5A6HA=*cppFhenq*I;JDg2l_Kz^V5`pGV&;&g)5CjCx2Cj;YzqbhS6RtoTzR&98Z^T!7p!%6R{L&@TQ2ORcVWpo~2!n))qIeAvW-)0<_WK2MutTLMDkOWq zxFdcM{dON@cbU5%M-~NY*OT^@Ds&-wU>;XgRPTJ=^r{iAO@f5++@f z`nk3J7HaNWu)fl#{9AM~eL+F6k84$YE<**6+fKMennONmdv=)=dN>!auCycltb+N7 z^AD{H{+F=2bAvejU2h>l3^qny9H?b*k}<@t%WB^ugc2utPhzUmjO6HK#!h9`xl4VO z1Nvj><_Nyu>qCpYZPdEG>`I^knm?6ZphtJ4@LnPveUvyHa>WS>#YBIS4tps~_RwflmBy$W zwzxGop4@?gE5lFHD!Yv&g#({y7+4e$@$E*c8t%ltX@5sqGlMdW%pmpN`d{IwceRGF z$Wt>gpU9r8`b8g>aJpmY4eAX>^*PMeE<3{blFx3TM=*cE2>I97qAQ?fQ$&c?0%_)Wq4n26A_i=ckrU*x^h#nATlQpl@?e>X1%8D5EHP#paGQs=yZK zt!sDk8zeCH%;X)*g?_;)jo+$EwvTX<`OHMFiDU_NR7oA1 zi{wUJ0AF?d+)+ezK4mUx6vQo>bV?MskIv*cyMg@fGnT9|TMI}Ru<&+B&QEx{kz9eK zpO-Eux6eYvebp}O2@#yiPqHct-gC5SKUo~%x|<0dF@!#vWJ4|v&+vW+l*ySam4PAM zP~3NP^0q7jznZn0;ENpdqTzR~q_ughFS#M22A}ZrZ|zW7fUZ{O+8iF?(C4g z$=I<%Tr)f6^?hh3KXHQ2CI=cd6EGsx{4rp@wqQocuiSk25hCFvHUnZ6?D(Ryg)eeZULfpLoW`W5JM4cu2&_BKcE_G3LA!$jRSt9C=5op&*cW-NFzYkv{5dE&{ntg~=g=qh-j43s5 zaTUzGw!a>0{4x(iJD`|kh5&snKoHaF+5^7Qg<*I%-6_Q-g?G&b*-^YtRyPkFq? z&{@9;moXAWf%|&d`iu|8RfkJVnSkwqYhYpEN@8v-y8jRlYTan^CO3m;E(-ZOf}VuF zd3<0rmtB*;b+ife^0xA2MAG^}saV0F8yQ~8EiEUDkExMQE3++?~LvW>uuY~g_j8QWjs6;&*#3pHN&$skSTs?+? zw#Fo8rwWE+pU*)nz=i)r8@P{BM!r@%1-CP1f)-AwUeTR;@jl7RwBztqELrqxY+)(- zyyH(6asy};H7XC z=~t5dzy-If%&OcO!=5AOarS-9{@dVX)~@#+Cps4RkXg&!(REcpnxPpFcnqMuvWhw1 zR%W0I%_@WI<2Rj*x$K}CX2oU@)606hZKRr&l`!(uR@^5mp~wDHnal}v2a0Rxc;g^b zwyBP2A4^b~hvVS5DpU6HO;46fMiOr2KnYK}!U$Kkt&9`vOdP&Q^(lKqmaTXUoe~+X z-*1aR`VH4w{l%D5JUz>(joGuT#$9Oeds2}UQ%6y_GKgK`mzl6sAib_An-+a4z3%vN zyuUj^=S$iQ9ppc*PNRCeAIg{wfz`!V0tqn(INZpH?)DDC!BNod2Wt76?Y!7gq2VKOf;Jy#gCaf#9 zNf~3ECKdOWWdzgAu)O^onvc@5E6J13B^1wC13j!@E2cvP1uOV4I8d2B+L++}58Tnm?2VJVm+T%_t z$pzJ8o^OZ~BlU4nI<-6yt9NhcL`ueh*~fVw_HvTclwFPo0fMwXPAlJ)b0AYz!C!T3 z_cqT32RKLr|Bz`x*q9R_gMow(@EuH`I$Sogu5}DFfZ)|19|zCb=otnqoS)3Vy|DK^ z=;By}8?LI4GKr=gZ1pHa7tnZU;s&=9ZW!hB$bm~X7-mO&wxx0+RWhoE4hXP|wk~t^ zH_)n)^mYQ_jKPh-K~Zh(bec*cE4=!{T@vke$y}}f^z1SlHwj55^6UPA|GR$2ls6)GMnM zf!W&8!djOzVBw%Td{yB=I$+bys~TyGUMgBPLIM6Ly*{fEu8BC$V0h8hq&Kb7knq-Q8tKd0juBeO1!)lNK8X`gCFB8MuPdtMbH+?E zNg@}aM=V~=2p@dC^}=&77fqh}F~CY9a@~5+tBlzO?h2!$^?LrD5#ZiZv#28_@l%Js zqQfqJ65~F**MDMWKnXXfi?d3UH&i6|QCN~uC4^b>aj8H9*A7)A-}7{i=T~JSHYX;A zD_~|f2HB9PCzjQ;^vq`5NtJQ;X}7f#Jrqh8NOln5QsXo}X$rd5j)e@>i62?7zV3Jh(138}+^s|K58)sS=jhg#H&Hh(z~zPZ!d0yCUZQ#w?wt5$LDR1tDhIga)# z=56S=pjhPEbX~OlJxhJeYl?P2?lOGj8FjP(OPbhK<@WIJxCoC?f$TV~N4|Zf5xgch z%ou5v^UTZO#tjGQV0q}yJfgJlV zUzG@AZMf)IX?~j64S3$aE7ovyGubzAc-kXapK1?w5)}J?O@RXgOc8^uNTx9wMAh?2 zV%8=So1)cge4+9wkiE&aCCm_h{87N%h3mE_-NgdY+!=W(Bi;W@4bKd z!YMJS|2(qHLX5>xR<$GTBArOPbS!YF_jrFg1QUJA(txyoA@(f(`Mc-)I+p;gC;ZWV(ItG}jLJKvWb>63!GXqxN;GKKXO zu!@C2FY+2qydL&qrHg_x65zXH(d&!3STr}}*C{DGL&lz`B&&#^ISx^ACD^Q?#_yh2 z;&^$j6m7vQ4L>A*S^3X8f(J1FL9~?+Da|I=?N3kJ?7Y{RJI_HP&M`I%+W)iYIm% zTK8)yP)?=~QsLH~L`BH~@9fgj#*z^dn?U`Mtoc)T=(($jJiMU$jX6h_W0p5naxa%m zoM#_l6?5hll?ze40OZvCvWm%O{reX;LF}U2FV>y_O0kgVp0%m#D7$=&KZRJw00wVG z6&HkyiDtB_GViGwl6>0NtJ;F5Jmm(DvGdtcXS>P%yOLo+tH31~`&E>b#n@HjI#~Qc z^jYIN555(PSW{~YpdXrtx*2#&gfgwAB2a+W`5?6kDRdpaLl6)zpRLz(fY0}}nssUz zPf!bPQe31P#G7XanaV;E0R4(>k3SvNVbUNg()KFmHP<>^G$yC@HMG-sGyVcusW?j9 z64AR@6uq^vjYo@ao4SFn+^iY1*qfVmzKi}NU>v#$&0#P^h(-mpYnyzpPF;FH%kfn0 z#WSZT-ZYEY!k@a+RdY`nH_+83=Nvz*fPtnhLT@D#N4^gD&4ymMv~=9GP4vbvvjWu&S%@+KpP%Cmm)5 z<_CJ(YBnFfRLuem?Xz-W=8{3V@gwn=ZUfUoIQ?`EB;>uHG3xE81AVGhm#h|G!tnPF zAG>86BPo>-Hpln`ihu1;6$ z95{L--aWW&h9Jv|18M(U0xx2$YTsjUUn8B&l#@ZLAgul%a*_>I_pyyTI3r;mftN|SZy#DOi)HfCA>#x)XB?r_3W3-?6(ImAcZ>OGL^|$pDbfk(OiC@E5gy?y(+n!Ucp3dC zA@kghcd-GFAY~(iQH*FX)@fr}mv1g0XJu&xh=Vo5&jG2l((E2(p`H>2pNcelBL_nZ<%3Z8q|*BQS{vhyz!Vr; zO1-4!=6;*2@d;Kq*YCo#Vh|yS@8ATIhN*d~LGSFSY8rOg^cGF%*_WCKKs%91X&r-A z>AaNgE$9o;9dQM%M92zGhNtW?c8B#DYV=r(QUPKN0@Tk#B)5_sK-j4NfK1dti&-X3 zz>Q6gZjwNgu^MOOT$IV@C1-bXVpI=y8hnn? zbr@S{$OXI4$!-cg?pQ}QuY}I8^}mL0!tuhnpYfc&4G;%0ImGz~P|Jb9b`=g;&v_}= zGQ>s9%acW0AFrWI!=Cmm5|HDN_t&H|ZP;Rdhs>VDW_`U9eO+Xoip=R3Sg;UV6t<WEN=y%s&*K?C1zXFqe~LBu-<5#EF(Rgi+tYeq+&Y7%pEI?I)PR&iX_K;uXHb;0p?J7YPE zhaX8VtnN7ZC?89=$yyOmH$;XXw0BE#68qB-_|>>W1t?YEc8IX6+B~S zubK}I4<)Onot@InYc4buP`r~6H7qxJS zr6X`-Y+W>xe-_PXgve*N0^1s65yfTklFqnX9A&h$rQ^6cW0N!9@~syq8iN+fN`B_3 z29!{+{_`-D@U=|A{UTIVsg{Ufe6qEHzW5nKuF3ic3k!3130swGc-s@=Gg00=2VTh2 z>)5=r;6A3GV(~tN%l5)Yt`+3lK1eig?N-rh_oin5COn?o>;Tj>Z;<;_?waDIdj1>g zD=_E9Oa3E9z)MwHiHKExEBN|j*Z1z5!$0y8vaxk;_QTx9TDT@(e+k=c=WF<(z@y0G z)StHsdQ4{}G zLVosJ>f=)6dmag(ClDT&yUe1IiO2H!nRd#<mQQ8Q diff --git a/examples/acados_matlab_octave/linear_mass_spring_model/example_closed_loop.m b/examples/acados_matlab_octave/linear_mass_spring_model/example_closed_loop.m index 2a8cb846b8..3301fd5ed5 100644 --- a/examples/acados_matlab_octave/linear_mass_spring_model/example_closed_loop.m +++ b/examples/acados_matlab_octave/linear_mass_spring_model/example_closed_loop.m @@ -30,17 +30,8 @@ % %% example of closed loop simulation -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 - - +clear all; clc; +check_acados_requirements() %% handy arguments compile_interface = 'auto'; @@ -66,10 +57,10 @@ %ocp_cost_type = 'ext_cost'; - %% setup problem % linear mass-spring system model = linear_mass_spring_model(); + % dims T = 10.0; % horizon length time nx = model.nx; % number of states @@ -78,6 +69,7 @@ ny_e = nx; % number of outputs in mayer term nbx = nx/2; % number of state bounds nbu = nu; % number of input bounds + % 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 @@ -86,6 +78,7 @@ W_e = eye(ny_e); % 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 = zeros(nx, 1); x0(1)=2.5; x0(2)=2.5; Jbx = zeros(nbx, nx); for ii=1:nbx Jbx(ii,ii)=1.0; end @@ -95,11 +88,10 @@ lbu = -0.5*ones(nu, 1); ubu = 0.5*ones(nu, 1); - - %% acados ocp model ocp_model = acados_ocp_model(); ocp_model.set('T', T); + % symbolics ocp_model.set('sym_x', model.sym_x); if isfield(model, 'sym_u') @@ -108,36 +100,46 @@ if isfield(model, 'sym_xdot') ocp_model.set('sym_xdot', model.sym_xdot); end + % cost +ocp_model.set('cost_type_0', ocp_cost_type); ocp_model.set('cost_type', ocp_cost_type); ocp_model.set('cost_type_e', ocp_cost_type); if (strcmp(ocp_cost_type, 'linear_ls')) + ocp_model.set('cost_Vu_0', Vu); ocp_model.set('cost_Vu', Vu); + ocp_model.set('cost_Vx_0', Vx); ocp_model.set('cost_Vx', Vx); ocp_model.set('cost_Vx_e', Vx_e); + ocp_model.set('cost_W_0', W); ocp_model.set('cost_W', W); ocp_model.set('cost_W_e', W_e); + ocp_model.set('cost_y_ref_0', yr); ocp_model.set('cost_y_ref', yr); ocp_model.set('cost_y_ref_e', yr_e); elseif (strcmp(ocp_cost_type, 'nonlinear_ls')) - ocp_model.set('cost_expr_y', model.expr_y); - ocp_model.set('cost_expr_y_e', model.expr_y_e); + ocp_model.set('cost_expr_y_0', model.cost_expr_y); + ocp_model.set('cost_expr_y', model.cost_expr_y); + ocp_model.set('cost_expr_y_e', model.cost_expr_y_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(ocp_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); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end + % dynamics if (strcmp(ocp_sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end + % constraints ocp_model.set('constr_x0', x0); ocp_model.set('constr_Jbx', Jbx); @@ -149,8 +151,6 @@ ocp_model.model_struct - - %% acados ocp opts ocp_opts = acados_ocp_opts(); ocp_opts.set('compile_interface', compile_interface); @@ -168,18 +168,13 @@ ocp_opts.opts_struct - - %% acados ocp % create ocp ocp = acados_ocp(ocp_model, ocp_opts); -ocp -% ocp.C_ocp - - %% acados sim model sim_model = acados_sim_model(); + % symbolics sim_model.set('sym_x', model.sym_x); if isfield(model, 'sym_u') @@ -188,20 +183,19 @@ if isfield(model, 'sym_xdot') sim_model.set('sym_xdot', model.sym_xdot); end + % model sim_model.set('T', T/ocp_N); if (strcmp(sim_method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); end sim_model.model_struct - - %% acados sim opts sim_opts = acados_sim_opts(); sim_opts.set('compile_interface', compile_interface); diff --git a/examples/acados_matlab_octave/linear_mass_spring_model/example_ocp.m b/examples/acados_matlab_octave/linear_mass_spring_model/example_ocp.m index 2a048a3f45..6fd0cf4a21 100644 --- a/examples/acados_matlab_octave/linear_mass_spring_model/example_ocp.m +++ b/examples/acados_matlab_octave/linear_mass_spring_model/example_ocp.m @@ -30,15 +30,8 @@ % %% test of native matlab interface -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 - +clear all; clc; +check_acados_requirements() %% arguments compile_interface = 'auto'; @@ -72,13 +65,9 @@ %cost_type = 'nonlinear_ls'; cost_type = 'auto'; - - %% create model entries model = linear_mass_spring_model(); - - % dims T = 10.0; % horizon length time nx = model.nx; @@ -108,6 +97,7 @@ nh = nu+nx; nh_e = nx; 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 @@ -116,6 +106,7 @@ W_e = eye(ny_e); % 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 = zeros(nx, 1); x0(1)=2.5; x0(2)=2.5; if (ng>0) @@ -140,9 +131,6 @@ ubu = 0.5*ones(nbu, 1); end - - - %% acados ocp model ocp_model = acados_ocp_model(); ocp_model.set('name', model_name); @@ -156,39 +144,49 @@ if isfield(model, 'sym_xdot') ocp_model.set('sym_xdot', model.sym_xdot); end + % cost +ocp_model.set('cost_type_0', cost_type); 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_0', Vu); ocp_model.set('cost_Vu', Vu); + ocp_model.set('cost_Vx_0', Vx); ocp_model.set('cost_Vx', Vx); ocp_model.set('cost_Vx_e', Vx_e); + ocp_model.set('cost_W_0', W); ocp_model.set('cost_W', W); ocp_model.set('cost_W_e', W_e); + ocp_model.set('cost_y_ref_0', yr); ocp_model.set('cost_y_ref', yr); ocp_model.set('cost_y_ref_e', yr_e); elseif (strcmp(cost_type, 'nonlinear_ls')) - ocp_model.set('cost_expr_y', model.expr_y); - ocp_model.set('cost_expr_y_e', model.expr_y_e); + ocp_model.set('cost_expr_y_0', model.cost_expr_y); + ocp_model.set('cost_expr_y', model.cost_expr_y); + ocp_model.set('cost_expr_y_e', model.cost_expr_y_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); + ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end + % dynamics if (strcmp(dyn_type, 'explicit')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); elseif (strcmp(dyn_type, 'implicit')) ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); else ocp_model.set('dyn_type', 'discrete'); - ocp_model.set('dyn_expr_phi', model.expr_phi); + ocp_model.set('dyn_expr_phi', model.dyn_expr_phi); end + % constraints ocp_model.set('constr_x0', x0); if (ng>0) @@ -200,10 +198,13 @@ 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', model.expr_h); + ocp_model.set('constr_expr_h_0', model.constr_expr_h_0); + ocp_model.set('constr_lh_0', lh); + ocp_model.set('constr_uh_0', uh); + ocp_model.set('constr_expr_h', model.constr_expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); - ocp_model.set('constr_expr_h_e', model.expr_h_e); + ocp_model.set('constr_expr_h_e', model.constr_expr_h_e); ocp_model.set('constr_lh_e', lh_e); ocp_model.set('constr_uh_e', uh_e); else @@ -215,9 +216,7 @@ ocp_model.set('constr_ubu', ubu); end -ocp_model.model_struct - - +% ocp_model.model_struct %% acados ocp opts ocp_opts = acados_ocp_opts(); @@ -247,10 +246,6 @@ %% acados ocp % create ocp ocp = acados_ocp(ocp_model, ocp_opts); -ocp -ocp.C_ocp - - % set trajectory initialization x_traj_init = zeros(nx, N+1); @@ -258,24 +253,20 @@ ocp.set('init_x', x_traj_init); ocp.set('init_u', u_traj_init); - % solve tic; ocp.solve(); time_ext = toc - %x0(1) = 1.5; %ocp.set('constr_x0', x0); % if not set, the trajectory is initialized with the previous solution - % get solution u = ocp.get('u'); x = ocp.get('x'); - % get info status = ocp.get('status'); sqp_iter = ocp.get('sqp_iter'); diff --git a/examples/acados_matlab_octave/linear_mass_spring_model/example_sim.m b/examples/acados_matlab_octave/linear_mass_spring_model/example_sim.m index fb88aed115..9e56c0a16c 100644 --- a/examples/acados_matlab_octave/linear_mass_spring_model/example_sim.m +++ b/examples/acados_matlab_octave/linear_mass_spring_model/example_sim.m @@ -30,17 +30,8 @@ % %% test of native matlab interface -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 - - +clear all; clc; +check_acados_requirements(); %% arguments compile_interface = 'auto'; @@ -53,29 +44,25 @@ num_stages = 4; num_steps = 4; - - %% model model = linear_mass_spring_model(); nx = model.nx; nu = model.nu; - - %% acados sim model sim_model = acados_sim_model(); sim_model.set('T', 0.5); if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); sim_model.set('sym_x', model.sym_x); if isfield(model, 'sym_u') sim_model.set('sym_u', model.sym_u); end else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_x', model.sym_x); sim_model.set('sym_xdot', model.sym_xdot); if isfield(model, 'sym_u') @@ -88,9 +75,6 @@ sim_model.model_struct - - - %% acados sim opts sim_opts = acados_sim_opts(); sim_opts.set('compile_interface', compile_interface); @@ -105,8 +89,6 @@ sim_opts.opts_struct - - %% acados sim % create sim sim = acados_sim(sim_model, sim_opts); diff --git a/examples/acados_matlab_octave/linear_mass_spring_model/linear_mass_spring_model.m b/examples/acados_matlab_octave/linear_mass_spring_model/linear_mass_spring_model.m index b2a5d3acf9..7fb7b43bf5 100644 --- a/examples/acados_matlab_octave/linear_mass_spring_model/linear_mass_spring_model.m +++ b/examples/acados_matlab_octave/linear_mass_spring_model/linear_mass_spring_model.m @@ -79,17 +79,19 @@ A = M(1:nx,1:nx); B = M(1:nx,nx+1:end); -expr_f_expl = Ac*sym_x + Bc*sym_u + c_const; -expr_f_impl = expr_f_expl - sym_xdot; -expr_phi = A*sym_x + B*sym_u; +dyn_expr_f_expl = Ac*sym_x + Bc*sym_u + c_const; +dyn_expr_f_impl = dyn_expr_f_expl - sym_xdot; +dyn_expr_phi = A*sym_x + B*sym_u; %% constraints -expr_h = [sym_u; sym_x]; -expr_h_e = [sym_x]; +constr_expr_h_0 = sym_u; +constr_expr_h = [sym_u; sym_x]; +constr_expr_h_e = sym_x; -%% nonlnear least squares -expr_y = [sym_u; sym_x]; -expr_y_e = [sym_x]; +%% nonlinear least squares +cost_expr_y_0 = sym_u; +cost_expr_y = [sym_u; sym_x]; +cost_expr_y_e = sym_x; %% external cost yr_u = zeros(nu, 1); @@ -97,11 +99,13 @@ dWu = 2*ones(nu, 1); dWx = ones(nx, 1); +ymyr_0 = sym_u - yr_u; ymyr = [sym_u; sym_x] - [yr_u; yr_x]; ymyr_e = sym_x - yr_x; -expr_ext_cost = 0.5 * ymyr' * ([dWu; dWx] .* ymyr); -expr_ext_cost_e = 0.5 * ymyr_e' * (dWx .* ymyr_e); +cost_expr_ext_cost_0 = 0.5 * ymyr_0' * (dWu .* ymyr_0); +cost_expr_ext_cost = 0.5 * ymyr' * ([dWu; dWx] .* ymyr); +cost_expr_ext_cost_e = 0.5 * ymyr_e' * (dWx .* ymyr_e); %% populate structure model.nx = nx; @@ -109,13 +113,16 @@ model.sym_x = sym_x; model.sym_xdot = sym_xdot; model.sym_u = sym_u; -model.expr_f_expl = expr_f_expl; -model.expr_f_impl = expr_f_impl; -model.expr_phi = expr_phi; -model.expr_h = expr_h; -model.expr_h_e = expr_h_e; -model.expr_y = expr_y; -model.expr_y_e = expr_y_e; -model.expr_ext_cost = expr_ext_cost; -model.expr_ext_cost_e = expr_ext_cost_e; - +model.dyn_expr_f_expl = dyn_expr_f_expl; +model.dyn_expr_f_impl = dyn_expr_f_impl; +model.dyn_expr_phi = dyn_expr_phi; +model.constr_expr_h_0 = constr_expr_h_0; +model.constr_expr_h = constr_expr_h; +model.constr_expr_h_e = constr_expr_h_e; +model.cost_expr_y_0 = cost_expr_y_0; +model.cost_expr_y = cost_expr_y; +model.cost_expr_y_e = cost_expr_y_e; +model.cost_expr_ext_cost_0 = cost_expr_ext_cost_0; +model.cost_expr_ext_cost = cost_expr_ext_cost; +model.cost_expr_ext_cost_e = cost_expr_ext_cost_e; +end diff --git a/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m b/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m index 6af257e536..4ee4522946 100644 --- a/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m +++ b/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; % check that env.sh has been run env_run = getenv('ENV_RUN'); @@ -130,9 +130,12 @@ constraint_h = 1; if constraint_h nh = length(model.expr_h); + ocp_model.set('constr_expr_h_0', model.expr_h); ocp_model.set('constr_expr_h', model.expr_h); lh = 0; uh = 200; + ocp_model.set('constr_lh_0', lh); + ocp_model.set('constr_uh_0', uh); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); else @@ -162,6 +165,7 @@ ocp_model.set('cost_W_e', W_e); ocp_model.set('cost_y_ref', yr); ocp_model.set('cost_y_ref_e', yr_e); + ocp_model.set('cost_Vz', zeros(ny,nz)); elseif (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); diff --git a/examples/acados_matlab_octave/pendulum_dae/example_sim.m b/examples/acados_matlab_octave/pendulum_dae/example_sim.m index 3ea66db94d..35d411a59b 100644 --- a/examples/acados_matlab_octave/pendulum_dae/example_sim.m +++ b/examples/acados_matlab_octave/pendulum_dae/example_sim.m @@ -29,7 +29,7 @@ % -clear all +clear all; clc; % check that env.sh has been run env_run = getenv('ENV_RUN'); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_closed_loop.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_closed_loop.m index 6b8a34254e..c37b440bee 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_closed_loop.m @@ -157,17 +157,17 @@ ocp_model.set('cost_y_ref', yr); ocp_model.set('cost_y_ref_e', yr_e); elseif (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); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % dynamics if (strcmp(ocp_sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); @@ -180,6 +180,9 @@ 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.expr_h); + ocp_model.set('constr_lh_0', lbu); + ocp_model.set('constr_uh_0', ubu); ocp_model.set('constr_expr_h', model.expr_h); ocp_model.set('constr_lh', lbu); ocp_model.set('constr_uh', ubu); @@ -248,10 +251,10 @@ sim_model.set('T', T/ocp_N); if (strcmp(sim_method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); end %sim_model.model_struct 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 c603542d1a..29211d7784 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 @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear VARIABLES +clear all; clc; % check that env.sh has been run env_run = getenv('ENV_RUN'); @@ -158,10 +158,10 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); @@ -174,7 +174,10 @@ 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', model.expr_h); + 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); @@ -232,9 +235,6 @@ %% acados ocp % create ocp ocp = acados_ocp(ocp_model, ocp_opts); -ocp -disp('ocp.C_ocp'); -disp(ocp.C_ocp); %ocp.model_struct 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 879666b559..493fed101f 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 @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; import casadi.* % model_path = fullfile(pwd,'..','pendulum_on_cart_model'); @@ -101,18 +101,21 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 35; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); ocp_model.set('constr_x0', x0); % ... see ocp_model.model_struct to see what other fields can be set diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_param_sens.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_param_sens.m index 4f2cb055eb..ccbec4b718 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_param_sens.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_param_sens.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; % check that env.sh has been run env_run = getenv('ENV_RUN'); @@ -165,15 +165,18 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); if (nh>0) - ocp_model.set('constr_expr_h', model.expr_h); + 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); 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 8389597c32..f0e4782580 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 @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear VARIABLES +clear all % check that env.sh has been run env_run = getenv('ENV_RUN'); @@ -164,10 +164,10 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); @@ -180,7 +180,10 @@ 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', model.expr_h); + 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); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_sim.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_sim.m index f456774323..2664c9e466 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_sim.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_sim.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear VARIABLES +clear all % check that env.sh has been run env_run = getenv('ENV_RUN'); @@ -77,10 +77,10 @@ if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); % if isfield(model, 'sym_z') % sim_model.set('sym_z', model.sym_z); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_solution_sens_closed_loop.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_solution_sens_closed_loop.m index 71867fabcd..f57a04cee9 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/example_solution_sens_closed_loop.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/example_solution_sens_closed_loop.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; model_path = fullfile(pwd,'..','pendulum_on_cart_model'); addpath(model_path) @@ -95,20 +95,23 @@ % % external cost -> with detection linear least squares % 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); +ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); +ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); % dynamics ocp_model.set('dyn_type', 'explicit'); -ocp_model.set('dyn_expr_f', model.expr_f_expl); +ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); ocp_model.set('constr_x0', xcurrent); %% acados ocp set opts @@ -142,7 +145,7 @@ sim_model.set('sym_u', model.sym_u); sim_model.set('sym_xdot', model.sym_xdot); sim_model.set('dyn_type', 'implicit'); -sim_model.set('dyn_expr_f', model.expr_f_impl); +sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); % acados sim opts sim_opts = acados_sim_opts(); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/experiment_dae_formulation.m b/examples/acados_matlab_octave/pendulum_on_cart_model/experiment_dae_formulation.m index 0af7d47686..b5b7d8dfea 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/experiment_dae_formulation.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/experiment_dae_formulation.m @@ -142,23 +142,23 @@ ocp_model.set('cost_type', cost_type); ocp_model.set('cost_type_e', cost_type); - ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); - ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); nh = length(model.expr_h); - ocp_model.set('constr_expr_h', model.expr_h); + ocp_model.set('constr_expr_h', model.constr_expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m index b8fd563ed4..f4c51e118f 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model.m @@ -63,26 +63,46 @@ sin_theta = sin(theta); cos_theta = cos(theta); denominator = M + m - m*cos_theta.^2; -expr_f_expl = vertcat(v, ... - dtheta, ... - (- l*m*sin_theta*dtheta.^2 + F + g*m*cos_theta*sin_theta)/denominator, ... - (- l*m*cos_theta*sin_theta*dtheta.^2 + F*cos_theta + g*m*sin_theta + M*g*sin_theta)/(l*denominator)); -expr_f_impl = expr_f_expl - sym_xdot; +dyn_expr_f_expl = vertcat(v, ... + dtheta, ... + (- l*m*sin_theta*dtheta.^2 + F + g*m*cos_theta*sin_theta)/denominator, ... + (- l*m*cos_theta*sin_theta*dtheta.^2 + F*cos_theta + g*m*sin_theta + M*g*sin_theta)/(l*denominator)); +dyn_expr_f_impl = dyn_expr_f_expl - sym_xdot; %% constraints -expr_h = sym_u; +constr_expr_h_0 = sym_u; +constr_expr_h = sym_u; %% cost +% generic cost W_x = diag([1e3, 1e3, 1e-2, 1e-2]); W_u = 1e-2; -expr_ext_cost_e = 0.5 * sym_x'* W_x * sym_x; -expr_ext_cost = expr_ext_cost_e + 0.5 * sym_u' * W_u * sym_u; +cost_expr_ext_cost_e = 0.5 * sym_x'* W_x * sym_x; +cost_expr_ext_cost = cost_expr_ext_cost_e + 0.5 * sym_u' * W_u * sym_u; +cost_expr_ext_cost_0 = 0.5 * sym_u' * W_u * sym_u; + % nonlinear least sqares +cost_expr_y_0 = sym_u; +cost_W_0 = W_u; cost_expr_y = vertcat(sym_x, sym_u); -W = blkdiag(W_x, W_u); -model.cost_expr_y_e = sym_x; -model.W_e = W_x; +cost_W = blkdiag(W_x, W_u); +cost_expr_y_e = sym_x; +cost_W_e = W_x; + +% linear least squares +ny_0 = nu; % number of outputs in initial cost term +cost_Vx_0 = zeros(ny_0,nx); +cost_Vu_0 = eye(nu); +cost_y_ref_0 = zeros(ny_0, 1); +ny = nx+nu; % number of outputs in lagrange term +cost_Vx = [eye(nx); zeros(nu,nx)]; % state-to-output matrix in lagrange term +cost_Vu = [zeros(nx, nu); eye(nu)]; % input-to-output matrix in lagrange term +cost_y_ref = zeros(ny, 1); % output reference in lagrange term + +ny_e = nx; % number of outputs in terminal cost term +cost_Vx_e = eye(ny_e, nx); +cost_y_ref_e = zeros(ny_e, 1); %% populate structure @@ -91,13 +111,31 @@ model.sym_x = sym_x; model.sym_xdot = sym_xdot; model.sym_u = sym_u; -model.expr_f_expl = expr_f_expl; -model.expr_f_impl = expr_f_impl; -model.expr_h = expr_h; -model.expr_ext_cost = expr_ext_cost; -model.expr_ext_cost_e = expr_ext_cost_e; +model.dyn_expr_f_expl = dyn_expr_f_expl; +model.dyn_expr_f_impl = dyn_expr_f_impl; + +model.constr_expr_h_0 = constr_expr_h_0; +model.constr_expr_h = constr_expr_h; + +model.cost_expr_ext_cost_0 = cost_expr_ext_cost_0; +model.cost_expr_ext_cost = cost_expr_ext_cost; +model.cost_expr_ext_cost_e = cost_expr_ext_cost_e; + +model.cost_expr_y_0 = cost_expr_y_0; +model.cost_W_0 = cost_W_0; model.cost_expr_y = cost_expr_y; -model.W = W; +model.cost_W = cost_W; +model.cost_expr_y_e = cost_expr_y_e; +model.cost_W_e = cost_W_e; + +model.cost_Vx_0 = cost_Vx_0; +model.cost_Vu_0 = cost_Vu_0; +model.cost_y_ref_0 = cost_y_ref_0; +model.cost_Vx = cost_Vx; +model.cost_Vu = cost_Vu; +model.cost_y_ref = cost_y_ref; +model.cost_Vx_e = cost_Vx_e; +model.cost_y_ref_e = cost_y_ref_e; end diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model_dae.m b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model_dae.m index 971545df2f..28ea9ad283 100644 --- a/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model_dae.m +++ b/examples/acados_matlab_octave/pendulum_on_cart_model/pendulum_on_cart_model_dae.m @@ -81,9 +81,9 @@ model.sym_xdot = sym_xdot; model.sym_z = sym_z; model.sym_u = sym_u; -model.expr_f_impl = expr_f_impl; -model.expr_h = expr_h; -model.expr_ext_cost = expr_ext_cost; -model.expr_ext_cost_e = expr_ext_cost_e; +model.dyn_expr_f_impl = expr_f_impl; +model.constr_expr_h = expr_h; +model.cost_expr_ext_cost = expr_ext_cost; +model.cost_expr_ext_cost_e = expr_ext_cost_e; end \ No newline at end of file diff --git a/examples/acados_matlab_octave/simple_dae_model/example_ocp.m b/examples/acados_matlab_octave/simple_dae_model/example_ocp.m index c727f0ad06..cb3edf4cb4 100644 --- a/examples/acados_matlab_octave/simple_dae_model/example_ocp.m +++ b/examples/acados_matlab_octave/simple_dae_model/example_ocp.m @@ -30,7 +30,7 @@ % %% test of native matlab interface -clear all +clear all; clc; model_name = 'simple_dae'; diff --git a/examples/acados_matlab_octave/swarming/example_closed_loop.m b/examples/acados_matlab_octave/swarming/example_closed_loop.m index 61d23ca96e..9b88119d9a 100644 --- a/examples/acados_matlab_octave/swarming/example_closed_loop.m +++ b/examples/acados_matlab_octave/swarming/example_closed_loop.m @@ -150,6 +150,9 @@ % Constraints ocp_model.set('constr_x0', x0); +ocp_model.set('constr_expr_h_0', model.expr_h); +ocp_model.set('constr_lh_0', lh); +ocp_model.set('constr_uh_0', uh); ocp_model.set('constr_expr_h', model.expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); @@ -199,8 +202,6 @@ % Create ocp ocp = acados_ocp(ocp_model, ocp_opts); -ocp -ocp.C_ocp %% Acados simulation model @@ -371,11 +372,12 @@ ylim([0 Inf]); -if status == 0 +if any(status)== 0 fprintf('\nsuccess!\n\n'); else - fprintf('\nsolution failed!\n\n'); + fprintf('\nsolution failed in some steps!\n\n'); end -waitforbuttonpress; -return; +if is_octave + waitforbuttonpress; +end \ No newline at end of file diff --git a/examples/acados_matlab_octave/swarming/example_ocp.m b/examples/acados_matlab_octave/swarming/example_ocp.m index d971c41f4a..9cca85dc5d 100644 --- a/examples/acados_matlab_octave/swarming/example_ocp.m +++ b/examples/acados_matlab_octave/swarming/example_ocp.m @@ -157,6 +157,9 @@ % Constraints ocp_model.set('constr_x0', x0); +ocp_model.set('constr_expr_h_0', model.expr_h); +ocp_model.set('constr_lh_0', lh); +ocp_model.set('constr_uh_0', uh); ocp_model.set('constr_expr_h', model.expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); @@ -208,8 +211,6 @@ % Create ocp ocp = acados_ocp(ocp_model, ocp_opts); -ocp -ocp.C_ocp % Set trajectory initialization step_mat = repmat((0:1:nb_steps),3*N,1); diff --git a/examples/acados_matlab_octave/test/simulink_test.m b/examples/acados_matlab_octave/test/simulink_test.m index ad90a2fed2..c6b5b485a8 100644 --- a/examples/acados_matlab_octave/test/simulink_test.m +++ b/examples/acados_matlab_octave/test/simulink_test.m @@ -19,6 +19,7 @@ % outputs simulink_opts.outputs.utraj = 1; simulink_opts.outputs.xtraj = 1; +simulink_opts.outputs.cost_value = 1; simulink_opts.samplingtime = '-1'; % 't0' (default) - use time step between shooting node 0 and 1 diff --git a/examples/acados_matlab_octave/test/simulink_u_traj_ref.mat b/examples/acados_matlab_octave/test/simulink_u_traj_ref.mat index f2b275085ebe8ffef16feff1e71b330c8563c5f6..70a9293d963b5831773da481ca5b390d77350dde 100644 GIT binary patch delta 223 zcmV<503iR=0@nhNA1zQrS4mDbG%O%Pa%Ew3Wn>_4ZaN@TXmub>VR9fbG$1iHIx{#r zGB_YIFfufeQ6rIH1(9?R0nd?zH-BD#{{um=!#*f2+q2d2ovwyMaMN!P|G?M(VDSSG zTSDNp=c(Btk?V~P^rh4mgY?A-F?_}Eo!;>6vAsmXzZ|Q}dT-nF Z&zSD?=HeCm>DsFM*s3qu0|0yCUJNM_4ZaN@TXmub>VR;}jHy|-MIxsXk zFft%AFfuWbQ6rIH1(9?R0nCwwH-CNq{sTd<11FSzTe{Wpovwz%=O!@!09Y7ELu?6w z)AydTghZ}4I^Y>wehj2fqdZGW{-U1)yYU0-pPwxpv|EoVhOB3DIL#IMV`C%#fyoz} zrzN_x9JpMdVW_zI;(qt0wY#jfUf7H5vt(8M{>q;5ZO#4--yYjP`(S)^SsCwb`#{D? WCT}iYu}==3bbzh;qCEf_$zP*C!ehYz diff --git a/examples/acados_matlab_octave/test/test_all_examples.m b/examples/acados_matlab_octave/test/test_all_examples.m new file mode 100644 index 0000000000..823380faa3 --- /dev/null +++ b/examples/acados_matlab_octave/test/test_all_examples.m @@ -0,0 +1,132 @@ +% +% 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; clc; close all; +p = path; % save current path for loading later +start_dir = pwd; + +% list the examples you would like to test +targets = { +% '../generic_dyn_disc/disc_dyn_example_ocp.m'; +% '../generic_external_cost/external_cost_example_ocp.m'; +% '../getting_started/extensive_example_ocp.m'; +% '../getting_started/minimal_example_closed_loop.m'; +% '../getting_started/minimal_example_sim.m'; +% '../getting_started/simulink_example.m'; +% '../getting_started/simulink_example_advanced.m'; +% '../linear_mass_spring_model/example_closed_loop.m'; +% '../linear_mass_spring_model/example_ocp.m'; +% '../linear_mass_spring_model/example_sim.m'; +% '../linear_mpc/example_closed_loop.m'; +% '../lorentz/example_mhe.m'; +% '../masses_chain_model/example_closed_loop.m'; +% '../masses_chain_model/example_ocp.m'; +% '../pendulum_dae/example_closed_loop.m'; % error while reshaping cost.Vz_0 +% '../pendulum_dae/example_sim.m'; +% '../pendulum_on_cart_model/example_closed_loop.m'; +% '../pendulum_on_cart_model/example_ocp.m'; +% '../pendulum_on_cart_model/example_ocp_custom_hess.m'; +% '../pendulum_on_cart_model/example_ocp_param_sens.m'; % Unable to resolve the name ocp.t_ocp.eval_param_sens. +% '../pendulum_on_cart_model/example_ocp_reg.m'; +% '../pendulum_on_cart_model/example_sim.m'; +% '../pendulum_on_cart_model/example_solution_sens_closed_loop.m'; % Unable to resolve the name ocp.t_ocp.eval_param_sens. +% '../pendulum_on_cart_model/experiment_dae_formulation.m'; +% '../race_cars/main.m'; % acados returns status 4 +% '../simple_dae_model/example_ocp.m'; +% '../swarming/example_closed_loop.m'; +% '../swarming/example_ocp.m'; +% '../swarming/example_sim.m'; +% '../wind_turbine_nx6/example_closed_loop.m'; +% '../wind_turbine_nx6/example_ocp.m'; +% '../wind_turbine_nx6/example_sim.m'; + +% './test_checks.m'; % SHOULD FAIL, DOES +% './test_mhe_lorentz.m'; % OK +% './test_ocp_OSQP.m'; % CRASH - solver not installed +% './test_ocp_linear_mass_spring.m'; % OK +% './test_ocp_pendulum_dae.m'; % OK +% './test_ocp_pendulum_on_cart.m'; % CRASH - solver not installed +% './test_ocp_qpdunes.m'; % CRASH - solver not installed +% './test_ocp_simple_dae.m'; % OK +% './test_ocp_wtnx6.m'; % FAIL - when not in installation folder? +% './test_sens_adj.m'; % OK +% './test_sens_forw.m'; % OK +% './test_sens_hess.m'; % OK +% './test_sim_dae.m'; % OK +% './test_target_selector.m' % OK +}; + +test_ok = zeros(1,length(targets)); % keep track of test results +test_messages = cell(1,length(targets)); % and error messages + +for test_number = 1:length(targets) + [target_dir,target_file,target_extension] = fileparts(targets{test_number}); + cd(target_dir) + save test_workspace.mat % due to "clear all" in the examples + try + run([target_file,target_extension]) + clear; close all; clc; % clean up after a succesful test + load test_workspace.mat + if contains(target_file,'simulink'); bdclose('all'); end + test_ok(test_number) = 1; % save the result + disp(['testing ',targets{test_number},' succesful']) + pause(3) % wait a bit before moving on + catch exception + clearvars -except exception; % keep the caught error + close all; clc; + load test_workspace.mat + if contains(target_file,'simulink'); bdclose('all'); end + warning(['testing ',targets{test_number},' failed']) + test_messages{test_number} = exception.message; % keep the message + clear exception; % forget error before the next test + pause(3) % wait a bit before moving on + end + delete test_workspace.mat % clean up the example folder + cd(start_dir) % go back to the starting folder + path(p); % restore the saved path +end + +% print out test results +clc; +disp('Succesful tests: ') +for test_number = 1:length(targets) + if test_ok(test_number) + disp(targets{test_number}) + end +end +disp(' ') +disp('Failed tests: ') +for test_number = 1:length(targets) + if ~test_ok(test_number) + disp(targets{test_number}) + disp([' message: ',test_messages{test_number}]) + end +end \ No newline at end of file diff --git a/examples/acados_matlab_octave/test/test_checks.m b/examples/acados_matlab_octave/test/test_checks.m index cd34ca4ab4..a5653e8e29 100644 --- a/examples/acados_matlab_octave/test/test_checks.m +++ b/examples/acados_matlab_octave/test/test_checks.m @@ -67,10 +67,10 @@ if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); end diff --git a/examples/acados_matlab_octave/test/test_ocp_OSQP.m b/examples/acados_matlab_octave/test/test_ocp_OSQP.m index c5a96730f6..c6f174c276 100644 --- a/examples/acados_matlab_octave/test/test_ocp_OSQP.m +++ b/examples/acados_matlab_octave/test/test_ocp_OSQP.m @@ -66,21 +66,21 @@ ocp_model.set('sym_xdot', model.sym_xdot); % 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); +ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); +ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; ocp_model.set('constr_lh', -U_max); % lower bound on h ocp_model.set('constr_uh', U_max); % upper bound on h diff --git a/examples/acados_matlab_octave/test/test_ocp_linear_mass_spring.m b/examples/acados_matlab_octave/test/test_ocp_linear_mass_spring.m index 5ee9c2ee81..a7485cbebd 100644 --- a/examples/acados_matlab_octave/test/test_ocp_linear_mass_spring.m +++ b/examples/acados_matlab_octave/test/test_ocp_linear_mass_spring.m @@ -149,26 +149,26 @@ ocp_model.set('cost_y_ref', yr); ocp_model.set('cost_y_ref_e', yr_e); elseif (strcmp(cost_type, 'nonlinear_ls')) - ocp_model.set('cost_expr_y', model.expr_y); - ocp_model.set('cost_expr_y_e', model.expr_y_e); + ocp_model.set('cost_expr_y', model.cost_expr_y); + ocp_model.set('cost_expr_y_e', model.cost_expr_y_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); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % dynamics if (strcmp(dyn_type, 'explicit')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl); elseif (strcmp(dyn_type, 'implicit')) ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); else ocp_model.set('dyn_type', 'discrete'); - ocp_model.set('dyn_expr_phi', model.expr_phi); + ocp_model.set('dyn_expr_phi', model.dyn_expr_phi); end % constraints ocp_model.set('constr_x0', x0); @@ -181,10 +181,10 @@ 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', model.expr_h); + ocp_model.set('constr_expr_h', model.constr_expr_h); ocp_model.set('constr_lh', lh); ocp_model.set('constr_uh', uh); - ocp_model.set('constr_expr_h_e', model.expr_h_e); + ocp_model.set('constr_expr_h_e', model.constr_expr_h_e); ocp_model.set('constr_lh_e', lh_e); ocp_model.set('constr_uh_e', uh_e); else diff --git a/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m b/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m index 10e5ea5d59..424d635d19 100644 --- a/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m +++ b/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m @@ -144,26 +144,26 @@ 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); + ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end %% constraints ocp_model.set('constr_x0', x0); if itest == 1 ng = 0; - ocp_model.set('constr_expr_h', model.expr_h); + 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_0', model.expr_h); + 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); elseif itest == 2 @@ -180,7 +180,10 @@ elseif itest == 3 ng = 0; ocp_model.set('constr_type', 'auto'); - ocp_model.set('constr_expr_h', model.expr_h); + 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_type_e', 'auto'); diff --git a/examples/acados_matlab_octave/test/test_ocp_qpdunes.m b/examples/acados_matlab_octave/test/test_ocp_qpdunes.m index 7bbc1deb3c..a771cd7861 100644 --- a/examples/acados_matlab_octave/test/test_ocp_qpdunes.m +++ b/examples/acados_matlab_octave/test/test_ocp_qpdunes.m @@ -93,18 +93,21 @@ % dynamics if (strcmp(sim_method, 'erk')) ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + 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.expr_f_impl); + ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl); end % constraints ocp_model.set('constr_type', 'auto'); -ocp_model.set('constr_expr_h', model.expr_h); +ocp_model.set('constr_expr_h_0', model.constr_expr_h); +ocp_model.set('constr_expr_h', model.constr_expr_h); U_max = 80; -ocp_model.set('constr_lh', -U_max); % lower bound on h -ocp_model.set('constr_uh', U_max); % upper bound on h +ocp_model.set('constr_lh_0', -U_max); % lower bound on h +ocp_model.set('constr_uh_0', U_max); % upper bound on h +ocp_model.set('constr_lh', -U_max); +ocp_model.set('constr_uh', U_max); ocp_model.set('constr_x0', x0); % ... see ocp_model.model_struct to see what other fields can be set diff --git a/examples/acados_matlab_octave/test/test_sens_adj.m b/examples/acados_matlab_octave/test/test_sens_adj.m index 174337d147..df02933c70 100644 --- a/examples/acados_matlab_octave/test/test_sens_adj.m +++ b/examples/acados_matlab_octave/test/test_sens_adj.m @@ -71,10 +71,10 @@ if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); end diff --git a/examples/acados_matlab_octave/test/test_sens_forw.m b/examples/acados_matlab_octave/test/test_sens_forw.m index efca759e17..b7456c95f1 100644 --- a/examples/acados_matlab_octave/test/test_sens_forw.m +++ b/examples/acados_matlab_octave/test/test_sens_forw.m @@ -78,10 +78,10 @@ if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); % if isfield(model, 'sym_z') % sim_model.set('sym_z', model.sym_z); diff --git a/examples/acados_matlab_octave/test/test_sens_hess.m b/examples/acados_matlab_octave/test/test_sens_hess.m index 8bb8f46eaf..79beabf0d4 100644 --- a/examples/acados_matlab_octave/test/test_sens_hess.m +++ b/examples/acados_matlab_octave/test/test_sens_hess.m @@ -72,10 +72,10 @@ if (strcmp(method, 'erk')) sim_model.set('dyn_type', 'explicit'); - sim_model.set('dyn_expr_f', model.expr_f_expl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_expl); else % irk irk_gnsf sim_model.set('dyn_type', 'implicit'); - sim_model.set('dyn_expr_f', model.expr_f_impl); + sim_model.set('dyn_expr_f', model.dyn_expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); % if isfield(model, 'sym_z') % sim_model.set('sym_z', model.sym_z); 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 ec17396788..a7eb059e94 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 @@ -95,7 +95,7 @@ function generate_c_code_nonlinear_least_squares( model, opts, target_dir ) % generate hessian y_0_adj = jtimes(fun, x, y_0, true); y_0_hess = jacobian(y_0_adj, [u; x]); - dy_dz = jacobian(fun, z) + dy_dz = jacobian(fun, z); % Set up functions y_0_fun = Function([model_name,'_cost_y_0_fun'], {x, u, z, p}, {fun}); y_0_fun_jac_ut_xt = Function([model_name,'_cost_y_0_fun_jac_ut_xt'], {x, u, z, p}, {fun, [jac_u'; jac_x'], dy_dz}); @@ -150,10 +150,10 @@ function generate_c_code_nonlinear_least_squares( model, opts, target_dir ) ny_e = length(fun); if isSX y_e = SX.sym('y', ny_e, 1); - u = SX.sym('u', 0, 0) + u = SX.sym('u', 0, 0); else y_e = MX.sym('y', ny_e, 1); - u = MX.sym('u', 0, 0) + u = MX.sym('u', 0, 0); end % generate hessian y_e_adj = jtimes(fun, x, y_e, true); From 303832b5fe4083896063409ff7c50a08b282a5a4 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 14 Mar 2024 17:23:05 +0100 Subject: [PATCH 14/16] Fix solution sensitivities in Matlab (#1050) Closes https://github.com/acados/acados/issues/1049 --- interfaces/acados_matlab_octave/acados_ocp.m | 2 +- .../c_templates_tera/matlab_templates/acados_mex_create.in.c | 4 ++-- .../c_templates_tera/matlab_templates/mex_solver.in.m | 4 ++++ 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index cdb2cfb2ea..75660e3ad6 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -232,7 +232,7 @@ function generate_c_code(obj, simulink_opts) function eval_param_sens(obj, field, stage, index) - ocp.t_ocp.eval_param_sens(field, stage, index); + obj.t_ocp.eval_param_sens(field, stage, index); end function value = get_cost(obj) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c index 8d8c15f879..f4ed415d11 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c @@ -65,6 +65,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule); ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule); ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_out *sens_out = {{ model.name }}_acados_get_sens_out(acados_ocp_capsule); ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule); void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule); @@ -137,11 +138,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) l_ptr[0] = (long long) nlp_solver; mxSetField(plhs[0], 0, "solver", solver_mat); - // TODO: sens_out not actually implemented in templates.. // sens_out mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); l_ptr = mxGetData(sens_out_mat); - l_ptr[0] = (long long) 1; + l_ptr[0] = (long long) sens_out; mxSetField(plhs[0], 0, "sens_out", sens_out_mat); // capsule 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 bf2bc501b4..0d1563931e 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 @@ -96,6 +96,10 @@ function set(varargin) value = ocp_get_cost(obj.C_ocp); end + 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: From faeced417f614ae389883f3fee17be8fd3b1ba0e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 18 Mar 2024 12:06:38 +0100 Subject: [PATCH 15/16] Add DS1104 support (#1052) Co-authored-by: Hannes Homburger --- acados/utils/math.c | 12 ++++++ acados/utils/math.h | 4 ++ acados/utils/timing.c | 9 +++++ acados/utils/timing.h | 9 +++++ cmake/Platform/dSpaceDS1104.cmake | 53 ++++++++++++++++++++++++++ cmake/Toolchain-dSpaceDS1104.cmake | 61 ++++++++++++++++++++++++++++++ utils/license/license.m | 27 ------------- 7 files changed, 148 insertions(+), 27 deletions(-) create mode 100644 cmake/Platform/dSpaceDS1104.cmake create mode 100644 cmake/Toolchain-dSpaceDS1104.cmake delete mode 100644 utils/license/license.m diff --git a/acados/utils/math.c b/acados/utils/math.c index 07ab5f5473..0d198cf270 100644 --- a/acados/utils/math.c +++ b/acados/utils/math.c @@ -49,6 +49,18 @@ int isnan(double x) } #endif +#if defined(_DS1104) +double fmax(double a, double b) +{ + return a > b ? a : b; +} + +int isnan(double x) +{ + return x != x; +} +#endif + void dgemm_nn_3l(int m, int n, int k, double *A, int lda, double *B, int ldb, double *C, int ldc) { int ii, jj, kk; diff --git a/acados/utils/math.h b/acados/utils/math.h index 7156a82084..4bbef9cbf5 100644 --- a/acados/utils/math.h +++ b/acados/utils/math.h @@ -41,6 +41,10 @@ extern "C" { double fmax(double a, double b); int isnan(double x); #endif +#if defined(_DS1104) +double fmax(double a, double b); +int isnan(double x); +#endif #define MIN(a,b) (((a)<(b))?(a):(b)) #define MAX(a,b) (((a)>(b))?(a):(b)) diff --git a/acados/utils/timing.c b/acados/utils/timing.c index 62862a94ed..36d5fd4b48 100644 --- a/acados/utils/timing.c +++ b/acados/utils/timing.c @@ -67,6 +67,15 @@ real_t acados_toc(acados_timer* t) return (real_t) duration / 1e9; } +#elif defined(_DS1104) + +void acados_tic(acados_timer* t) +{ + ds1104_tic_start(); + t->time = ds1104_tic_read(); +} + +real_t acados_toc(acados_timer* t) { return ds1104_tic_read() - t->time; } #elif defined(__MABX2__) diff --git a/acados/utils/timing.h b/acados/utils/timing.h index 08e93cb5c3..6edbd6cc08 100644 --- a/acados/utils/timing.h +++ b/acados/utils/timing.h @@ -72,6 +72,15 @@ typedef struct acados_timer_ double time; } acados_timer; +#elif defined(_DS1104) + +#include + +typedef struct acados_timer_ +{ + double time; +} acados_timer; + #else /* Use POSIX clock_gettime() for timing on non-Windows machines. */ diff --git a/cmake/Platform/dSpaceDS1104.cmake b/cmake/Platform/dSpaceDS1104.cmake new file mode 100644 index 0000000000..90e958c7c1 --- /dev/null +++ b/cmake/Platform/dSpaceDS1104.cmake @@ -0,0 +1,53 @@ +# +# 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.; +# + + + +# The dSpace compiler only works with the prefixes/suffixes below! + +set(CMAKE_IMPORT_LIBRARY_PREFIX "") +set(CMAKE_SHARED_LIBRARY_PREFIX "") +set(CMAKE_SHARED_MODULE_PREFIX "") +set(CMAKE_STATIC_LIBRARY_PREFIX "") + +set(CMAKE_EXECUTABLE_SUFFIX ".exe") +set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib") +set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib") +set(CMAKE_SHARED_MODULE_SUFFIX ".lib") +set(CMAKE_STATIC_LIBRARY_SUFFIX ".lib") + +set(CMAKE_C_FLAGS "\"-J${DSPACE_RTLIB}\"") + +set(CMAKE_INCLUDE_FLAG_C "-J") +set(CMAKE_INCLUDE_FLAG_CXX "-J") + +add_definitions(-D_DS1104) +remove_definitions(-DLINUX) +remove_definitions(-D__LINUX__) diff --git a/cmake/Toolchain-dSpaceDS1104.cmake b/cmake/Toolchain-dSpaceDS1104.cmake new file mode 100644 index 0000000000..185048d751 --- /dev/null +++ b/cmake/Toolchain-dSpaceDS1104.cmake @@ -0,0 +1,61 @@ +SET(CMAKE_SYSTEM_NAME "dSpaceDS1104") +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}) +SET(CMAKE_SYSTEM_PROCESSOR "ppc") + +file(TO_CMAKE_PATH "C:/ProgramData/dSPACE/AD34425A-2298-40D1-8728-E06238C82A90" DSPACE_TOOLS) +set(DSPACE_RTLIB "C:/dSPACE_RCPHIL_2020_A/DS1104/RTLib") +set(DSPACE_PPCTOOLS "${DSPACE_TOOLS}/Compiler/PPCTools") + +# C Compiler +find_program(CMAKE_C_COMPILER NAMES ${DSPACE_PPCTOOLS}/bin/mccppc.exe) +# C++ Compiler -- not used +find_program(CMAKE_CXX_COMPILER NAMES ${DSPACE_PPCTOOLS}/bin/mccppc.exe) +# Assembler +find_program(CMAKE_ASM_COMPILER NAMES ${DSPACE_PPCTOOLS}/bin/asmppc.exe) +set(CMAKE_RANLIB ":") + +find_program(CMAKE_MAKE_PROGRAM NAMES "C:/dSPACE_RCPHIL_2020_A/Exe/DSMAKE.exe") + +SET(CMAKE_FIND_ROOT_PATH ${DSPACE_PPCTOOLS}) +SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# add_definitions(-D__MABX2__ -D__DSPACE__) +add_definitions(-D_DS1104 -D__DSPACE__) +# Compiler flags +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -H -J{DSPACE_RTLIB}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -H -J{DSPACE_RTLIB}") +set(CMAKE_C_FLAGS "-Ot -Oi -Or -Ox -D_INLINE" CACHE STRING "" FORCE) +# set(CMAKE_C_FLAGS "-D_INLINE" CACHE STRING "" FORCE) + +# acados flags +set(BLASFEO_TARGET "GENERIC" CACHE STRING "BLASFEO Target architecture") +set(HPIPM_TARGET "GENERIC" CACHE STRING "HPIPM Target architecture") +set(BUILD_SHARED_LIBS OFF CACHE STRING "Build shared libraries") +set(BLASFEO_EXAMPLES OFF CACHE BOOL "Examples disabled") +set(EXT_DEP OFF CACHE BOOL "Compile external dependencies in BLASFEO") +set(ACADOS_INSTALL_DIR "install" CACHE PATH "Installation path to ACADOS_INSTALL_DIR") + + +# import from platform folder +set(CMAKE_IMPORT_LIBRARY_PREFIX "") +set(CMAKE_SHARED_LIBRARY_PREFIX "") +set(CMAKE_SHARED_MODULE_PREFIX "") +set(CMAKE_STATIC_LIBRARY_PREFIX "") + +set(CMAKE_EXECUTABLE_SUFFIX ".exe") +set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib") +set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib") +set(CMAKE_SHARED_MODULE_SUFFIX ".lib") +set(CMAKE_STATIC_LIBRARY_SUFFIX ".lib") + +set(CMAKE_C_FLAGS "\"-J${DSPACE_RTLIB}\"") + +set(CMAKE_INCLUDE_FLAG_C "-J") +set(CMAKE_INCLUDE_FLAG_CXX "-J") + +# add_definitions(-D__MABX2__) +remove_definitions(-DLINUX) +remove_definitions(-DOS_WINDOWS) +remove_definitions(-D__LINUX__) diff --git a/utils/license/license.m b/utils/license/license.m deleted file mode 100644 index 34af0fb5e8..0000000000 --- a/utils/license/license.m +++ /dev/null @@ -1,27 +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.; \ No newline at end of file From 097e034b7ae86ebc3c1305a6d35bcd78418a299d Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Mon, 18 Mar 2024 12:30:45 +0100 Subject: [PATCH 16/16] Furuta example (#1053) * cleanup python examples * add Furuta pendulum --------- Co-authored-by: Jonathan Frey --- examples/acados_python/Unicycle/main.py | 12 +- .../export_chain_mass_integrator.py | 4 +- .../chain_mass/minimal_example_sim.py | 6 +- .../chain_mass/run_nominal_control.py | 8 +- .../crane/time_optimal_example.py | 4 +- examples/acados_python/cstr/main.py | 4 +- .../minimal_example_external_ode.py | 8 +- .../furuta_pendulum/furuta_model.py | 126 +++++++++++ .../furuta_pendulum/integrator_experiment.py | 201 ++++++++++++++++++ .../furuta_pendulum/main_closed_loop.py | 184 ++++++++++++++++ .../acados_python/furuta_pendulum/utils.py | 28 +++ .../minimal_example_ocp_generic_impl_dyn.py | 8 +- .../minimal_example_closed_loop.py | 8 +- .../getting_started/minimal_example_ocp.py | 8 +- .../getting_started/minimal_example_sim.py | 4 +- .../acados_python/getting_started/utils.py | 10 +- .../linear_mass_test_problem.py | 4 +- .../as_rti/as_rti_closed_loop_example.py | 8 +- .../pendulum_on_cart/common/utils.py | 4 - .../custom_update/example_custom_rti_loop.py | 8 +- .../cython_example_closed_loop.py | 8 +- .../example_solution_sens_closed_loop.py | 12 +- .../mhe/closed_loop_mhe_ocp.py | 6 +- .../pendulum_on_cart/mhe/export_mhe_solver.py | 6 +- .../mhe/export_mhe_solver_with_param.py | 6 +- .../pendulum_on_cart/mhe/export_ocp_solver.py | 4 +- .../mhe/minimal_example_mhe.py | 14 +- .../minimal_example_mhe_with_noisy_param.py | 14 +- .../mhe/minimal_example_mhe_with_param.py | 14 +- .../ocp/example_custom_hessian.py | 8 +- .../ocp/example_ocp_dynamics_formulations.py | 8 +- .../ocp/example_sqp_qpDUNES.py | 8 +- .../ocp/example_sqp_rti_loop.py | 8 +- .../ocp/minimal_example_ocp_cmake.py | 8 +- .../ocp/minimal_example_ocp_reuse_code.py | 16 +- .../ocp/nonuniform_discretization_example.py | 8 +- .../ocp/ocp_example_cost_formulations.py | 6 +- .../ocp/ocp_example_h_init_contraints.py | 8 +- .../pendulum_on_cart/ocp/simulink_example.py | 8 +- .../sim/extensive_example_sim.py | 6 +- .../sim/minimal_example_sim_cmake.py | 6 +- .../policy_gradient_example.py | 8 +- .../test_solution_sens_and_exact_hess.py | 4 +- examples/acados_python/pmsm_example/main.py | 12 +- .../race_cars/acados_settings.py | 4 +- .../race_cars/acados_settings_dev.py | 4 +- examples/acados_python/race_cars/main.py | 8 +- examples/acados_python/rsm_example/main.py | 12 +- examples/acados_python/tests/reset_test.py | 8 +- .../tests/soft_constraint_test.py | 8 +- .../tests/test_cost_integration_euler.py | 8 +- .../tests/test_cost_integration_value.py | 8 +- .../acados_python/tests/test_cython_ctypes.py | 16 +- .../tests/test_ggn_cost_integration.py | 8 +- .../acados_python/tests/test_ocp_setting.py | 8 +- examples/acados_python/tests/test_osqp.py | 8 +- .../test_parametric_nonlinear_constraint_h.py | 8 +- examples/acados_python/tests/test_sim_dae.py | 6 +- .../piecewiese_polynomial_control_example.py | 6 +- .../test_polynomial_controls_and_penalties.py | 4 +- .../time_varying/test_time_varying_irk.py | 2 +- .../time_accelerating_pendulum_simulation.py | 4 +- .../timing_example/reset_timing.py | 8 +- .../acados_python/zoRO_example/cstr/main.py | 4 +- .../pendulum_on_cart/minimal_example_zoro.py | 8 +- 65 files changed, 768 insertions(+), 235 deletions(-) create mode 100644 examples/acados_python/furuta_pendulum/furuta_model.py create mode 100644 examples/acados_python/furuta_pendulum/integrator_experiment.py create mode 100644 examples/acados_python/furuta_pendulum/main_closed_loop.py create mode 100644 examples/acados_python/furuta_pendulum/utils.py diff --git a/examples/acados_python/Unicycle/main.py b/examples/acados_python/Unicycle/main.py index cccc5e9694..f62ce183bf 100644 --- a/examples/acados_python/Unicycle/main.py +++ b/examples/acados_python/Unicycle/main.py @@ -20,8 +20,8 @@ def create_ocp_solver_description() -> AcadosOcp: model = export_robot_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu # set dimensions @@ -86,11 +86,11 @@ def closed_loop_simulation(): # prepare simulation Nsim = 100 - nx = ocp.model.x.size()[0] - nu = ocp.model.u.size()[0] + nx = ocp.model.x.rows() + nu = ocp.model.u.rows() - simX = np.ndarray((Nsim + 1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim + 1, nx)) + simU = np.zeros((Nsim, nu)) xcurrent = X0 simX[0, :] = xcurrent diff --git a/examples/acados_python/chain_mass/export_chain_mass_integrator.py b/examples/acados_python/chain_mass/export_chain_mass_integrator.py index 8e950d03c7..0b2f0193cd 100644 --- a/examples/acados_python/chain_mass/export_chain_mass_integrator.py +++ b/examples/acados_python/chain_mass/export_chain_mass_integrator.py @@ -55,8 +55,8 @@ def export_chain_mass_integrator(n_mass, m, D, L): # set model sim.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx diff --git a/examples/acados_python/chain_mass/minimal_example_sim.py b/examples/acados_python/chain_mass/minimal_example_sim.py index 4a26cda359..6454801873 100644 --- a/examples/acados_python/chain_mass/minimal_example_sim.py +++ b/examples/acados_python/chain_mass/minimal_example_sim.py @@ -64,8 +64,8 @@ sim.model = model Tf = 0.1 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() N = 200 # set simulation time @@ -79,7 +79,7 @@ # create acados_integrator = AcadosSimSolver(sim) -simX = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) # position of last mass xPosFirstMass = np.zeros((3,1)) diff --git a/examples/acados_python/chain_mass/run_nominal_control.py b/examples/acados_python/chain_mass/run_nominal_control.py index 0c804c31f4..c6de7dec56 100644 --- a/examples/acados_python/chain_mass/run_nominal_control.py +++ b/examples/acados_python/chain_mass/run_nominal_control.py @@ -90,8 +90,8 @@ def run_nominal_control(chain_params): # set model ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx Tf = N * Ts @@ -210,8 +210,8 @@ def run_nominal_control(chain_params): #%% actual simulation N_sim = int(np.floor(Tsim/Ts)) - simX = np.ndarray((N_sim+1, nx)) - simU = np.ndarray((N_sim, nu)) + simX = np.zeros((N_sim+1, nx)) + simU = np.zeros((N_sim, nu)) wall_dist = np.zeros((N_sim,)) timings = np.zeros((N_sim,)) diff --git a/examples/acados_python/crane/time_optimal_example.py b/examples/acados_python/crane/time_optimal_example.py index 7633ea5c6e..b9cfabcd84 100644 --- a/examples/acados_python/crane/time_optimal_example.py +++ b/examples/acados_python/crane/time_optimal_example.py @@ -76,8 +76,8 @@ def main(use_cython=True): # N - maximum number of bangs N = 7 Tf = N - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() # set dimensions ocp.dims.N = N diff --git a/examples/acados_python/cstr/main.py b/examples/acados_python/cstr/main.py index 11a972ac69..81a130a615 100644 --- a/examples/acados_python/cstr/main.py +++ b/examples/acados_python/cstr/main.py @@ -55,8 +55,8 @@ def simulate( nx = X_ref.shape[1] nu = U_ref.shape[1] - X = np.ndarray((Nsim + 1, nx)) - U = np.ndarray((Nsim, nu)) + X = np.zeros((Nsim + 1, nx)) + U = np.zeros((Nsim, nu)) timings_solver = np.zeros((Nsim)) timings_integrator = np.zeros((Nsim)) diff --git a/examples/acados_python/external_model/minimal_example_external_ode.py b/examples/acados_python/external_model/minimal_example_external_ode.py index 74c6d3b595..0a0c6947e0 100644 --- a/examples/acados_python/external_model/minimal_example_external_ode.py +++ b/examples/acados_python/external_model/minimal_example_external_ode.py @@ -55,8 +55,8 @@ ocp.solver_options.model_external_shared_lib_name = "external_ode_casadi" Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 30 @@ -127,8 +127,8 @@ stat_fields = ['time_tot', 'time_lin', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter'] for field in stat_fields: print(f"{field} : {ocp_solver.get_stats(field)}") -simX = np.ndarray((N + 1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N + 1, nx)) +simU = np.zeros((N, nu)) for i in range(N): simX[i,:] = ocp_solver.get(i, "x") simU[i,:] = ocp_solver.get(i, "u") diff --git a/examples/acados_python/furuta_pendulum/furuta_model.py b/examples/acados_python/furuta_pendulum/furuta_model.py new file mode 100644 index 0000000000..0c6f59e0eb --- /dev/null +++ b/examples/acados_python/furuta_pendulum/furuta_model.py @@ -0,0 +1,126 @@ +from acados_template import AcadosModel +import casadi as ca +import numpy as np + +from scipy.linalg import block_diag + +def get_furuta_model(): + + # system dimensions + nx = 4 + nu = 1 + + # system parameters + + # Distances + L1 = 0.1035 # 103.5mm + l2 = 0.0955 # 92.1mm + + # mass + m2 = 0.192 # 199g + J2 = 7.653e-04 # kg/mm^2 + g = 9.81 # N/kg + + # inertia arm 1 + J1_ges = 5.3875e-04 + 0.75e-04 # J1 + m1*l1^2 + + # inertia arm 2 + J2_ges = J2 + m2*l2**2 + + # total inertia at motor + J0 = J1_ges + m2*L1**2 + + # damping hub motor + b1 = 40*1e-4 + + # damping coupling between both arms + k = 0.098 + b2 = 2*k*J2_ges + + # applied torques + tau2 = 0 + + # named symbolic variables + theta1 = ca.SX.sym('theta1') # angle around vertical axis (axis next to motor) [rad] + theta2 = ca.SX.sym('theta2') # angle around horizontal axis (axis next to mass) [rad] + dtheta1 = ca.SX.sym('dtheta1') # angular velocity of rod 1 [rad/s] + dtheta2 = ca.SX.sym('dtheta2') # angular velocity of rod 2 [rad/s] + dtheta = ca.vertcat(dtheta1, dtheta2) + tau1 = ca.SX.sym('tau1') # torque acting on first rod [Nm] + + # (unnamed) symbolic variables + x = ca.vertcat(theta1, theta2, dtheta1, dtheta2) + xdot = ca.SX.sym('xdot', nx, 1) + u = tau1 + theta2 = theta2 - np.pi + + # dynamics + sin_theta_2 = ca.sin(theta2) + cos_theta_2 = ca.cos(theta2) + sin_2_theta_2 = ca.sin(2*theta2) + + factor = m2*L1*l2 + + Matrix1 = ca.blockcat([ + [J0 + J2_ges*sin_theta_2**2, factor*cos_theta_2], + [factor*cos_theta_2, J2_ges]]) + Matrix2 = ca.blockcat([ + [b1 + 0.5*dtheta2*J2_ges*sin_2_theta_2, 0.5*dtheta1*J2_ges*sin_2_theta_2 - factor*sin_theta_2*dtheta2], + [-0.5*dtheta1*J2_ges*sin_2_theta_2, b2]]) + + rhs = ca.vertcat(tau1, tau2) - Matrix2 @ ca.vertcat(dtheta1, dtheta2) - ca.vertcat(0, g*m2*l2*sin_theta_2) + + f_expl_expr = ca.vertcat(dtheta, ca.solve(Matrix1, rhs)) + f_impl_expr = ca.vertcat( + dtheta - xdot[:2], + Matrix1 @ xdot[2:] - rhs + ) + + # # constraints + # expr_h = u + + # # cost + # W_x = np.diag([50, 500, 1, 1]) + # W_u = 1000 + # expr_ext_cost_e = x.T @ W_x @ x + # expr_ext_cost = expr_ext_cost_e + u.T @ W_u @ u + + # nonlinear least squares + # cost_expr_y = ca.vertcat(x, u) + # W = np.blo(W_x, W_u) + # model.cost_expr_y_e = x + # model.W_e = W_x * 10 + + # populate structure + model = AcadosModel() + model.name = 'furuta_model' + model.x = x + model.xdot = xdot + model.u = u + model.f_impl_expr = f_impl_expr + model.f_expl_expr = f_expl_expr + return model + # model.expr_h = expr_h + # model.expr_ext_cost = expr_ext_cost + # model.expr_ext_cost_e = expr_ext_cost_e + + # model.cost_expr_y = cost_expr_y + # model.W = W + + + # LQR + # fd = sqrt(eps) + # x0 = Matrix([0, 0, 0, 0]) + # u0 = 0 + # A1 = (f(x0 + Matrix([1, 0, 0, 0])*fd, u0) - f(x0 - Matrix([1, 0, 0, 0])*fd, u0)) / (2*fd) + # A2 = (f(x0 + Matrix([0, 1, 0, 0])*fd, u0) - f(x0 - Matrix([0, 1, 0, 0])*fd, u0)) / (2*fd) + # A3 = (f(x0 + Matrix([0, 0, 1, 0])*fd, u0) - f(x0 - Matrix([0, 0, 1, 0])*fd, u0)) / (2*fd) + # A4 = (f(x0 + Matrix([0, 0, 0, 1])*fd, u0) - f(x0 - Matrix([0, 0, 0, 1])*fd, u0)) / (2*fd) + + # B = (f(x0, u0 + fd) - f(x0, u0 - fd)) / (2*fd) + # A = Matrix([[A1, A2, A3, A4]]) + # Ts = 0.025 + # K_LQR, P_LQR, eig_of_ctrl_system = lqrd(A, B, W_x, W_u, Ts) + +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 new file mode 100644 index 0000000000..3e2685be48 --- /dev/null +++ b/examples/acados_python/furuta_pendulum/integrator_experiment.py @@ -0,0 +1,201 @@ +# -*- 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.; +# + +# authors: Katrin Baumgaertner, Jonathan Frey + +from furuta_model import get_furuta_model + +import numpy as np +from matplotlib.lines import Line2D +import matplotlib.pyplot as plt +from acados_template import latexify_plot, AcadosSim, AcadosSimSolver + +latexify_plot() + +X0 = np.array([0, np.pi/2, 0, 0]) + + +def setup_acados_integrator(model, dt, num_stages=4, num_steps=4, integrator_type="ERK", + newton_iter=20, newton_tol=1e-10): + sim = AcadosSim() + sim.model = model + sim.solver_options.T = dt + sim.solver_options.num_stages = num_stages + sim.solver_options.num_steps = num_steps + sim.solver_options.integrator_type = integrator_type + sim.solver_options.newton_iter = newton_iter + sim.solver_options.newton_tol = newton_tol + sim.solver_options.collocation_type = 'GAUSS_RADAU_IIA' + # sim.solver_options.sens_forw = False + sim.solver_options.sens_adj = False + sim.solver_options.sens_algebraic = False + sim.solver_options.sens_hess = False + sim.solver_options.sim_method_jac_reuse = True + + acados_integrator = AcadosSimSolver(sim, verbose=False) + return acados_integrator + + +def simulate( + integrator: AcadosSimSolver, x0, u_trajectory +): + Nsim = u_trajectory.shape[1] + x_trajectory = np.zeros((integrator.acados_sim.dims.nx, Nsim + 1)) + timings = np.zeros((Nsim + 1,)) + x_trajectory[:, 0] = x0 + for i in range(Nsim): + x_trajectory[:, i + 1] = integrator.simulate(x_trajectory[:, i], u_trajectory[:, i]) + timings[i] = integrator.get('time_tot') + return x_trajectory, timings + +def main(integrator_type = "IRK"): + SAVE_FIG = True + + Tsim = 1.0 + dt_plant = 0.2 + + model = get_furuta_model() + + Nsim = int(Tsim / dt_plant) + if not (Tsim / dt_plant).is_integer(): + print("WARNING: Tsim / dt_plant should be an integer!") + + integrator = setup_acados_integrator( + model, + dt_plant, + num_stages=8, + num_steps=100, + newton_iter=20, + newton_tol=1e-16, + integrator_type="IRK", + ) + + nu = 1 + np.random.seed(0) + u_trajectory = 0.03 * (np.random.random((nu, Nsim)) - 0.5) + X_exact, _ = simulate(integrator, X0, u_trajectory) + del integrator + + # store results for plotting + X_all = [X_exact] + labels_all = ["exact"] + + plt.figure() + + if integrator_type == "ERK": + num_stages_vals = [1, 2, 4] + num_steps_vals = [1, 2, 5, 10, 50, 100] + else: + num_stages_vals = [1, 2, 3, 4] + num_steps_vals = [1, 2, 5, 10, 50] + markers = ["o", "v", "s", "D", "^", ">", "<", "1", "2", "3", "4"] + colors = [plt.cm.tab10(i) for i in range(len(num_stages_vals))] + + ## EXPERIMENT + for i, num_stages in enumerate(num_stages_vals): + for j, num_steps in enumerate(num_steps_vals): + + label = f"{integrator_type}_stages_{num_stages}_steps_{num_steps}" + print(f"Running simulation with {label}") + + model = get_furuta_model() + + integrator = setup_acados_integrator( + model, + dt_plant, + num_stages=num_stages, + num_steps=num_steps, + integrator_type=integrator_type, + ) + + X, timings_integrator = simulate(integrator, X0, u_trajectory) + err_x = np.max(np.abs(X - X_exact)) + + # store all results + X_all.append(X) + labels_all.append(label) + + plt.plot( + [np.mean(timings_integrator * 1e3)], + [err_x], + color=colors[i], + marker=markers[j], + label=label, + ) + + print(f"got: err_x: {err_x}") + + del integrator + + plt.grid() + plt.yscale("log") + plt.xscale("log") + plt.ylim([1e-10, 1e1]) + plt.ylabel(r"error $\Vert x - x^{\mathrm{exact}}\Vert_{\infty}$") + plt.xlabel("mean integration time [ms]") + plt.title(f"{integrator_type}") + + legend_elements = [ + Line2D([0], [0], color=colors[i], lw=4, label="num stages = " + str(num_stages)) + for i, num_stages in enumerate(num_stages_vals) + ] + [ + Line2D( + [0], + [0], + marker=markers[j], + lw=0, + color="k", + label="num steps = " + str(num_steps), + ) + for j, num_steps in enumerate(num_steps_vals) + ] + + plt.legend(handles=legend_elements) + if SAVE_FIG: + fig_filename = f"cstr_integrator_experiment_{integrator_type}.pdf" + plt.savefig( + fig_filename, bbox_inches="tight", transparent=True, pad_inches=0.05 + ) + print(f"\nstored figure in {fig_filename}") + + plt.show() + + # print failed runs + for x, l in zip(X_all, labels_all): + if np.any(np.isnan(x)): + print(f"Run {l} failed with NaN.") + + print(f"final state {X_exact[:, -1]}") + + +if __name__ == "__main__": + main(integrator_type="IRK") + main(integrator_type="ERK") diff --git a/examples/acados_python/furuta_pendulum/main_closed_loop.py b/examples/acados_python/furuta_pendulum/main_closed_loop.py new file mode 100644 index 0000000000..583d2c1ef3 --- /dev/null +++ b/examples/acados_python/furuta_pendulum/main_closed_loop.py @@ -0,0 +1,184 @@ +# -*- 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 acados_template import AcadosOcp, AcadosOcpSolver +from utils import plot_furuta_pendulum +from furuta_model import get_furuta_model +from integrator_experiment import setup_acados_integrator +import numpy as np +import scipy.linalg +from casadi import vertcat + +def setup(x0, umax, dt_0, N_horizon, Tf, RTI=False): + ocp = AcadosOcp() + + model = get_furuta_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + + ocp.dims.N = 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 = 10 + + 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 + + solver_json = 'acados_ocp_' + model.name + '.json' + ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json) + + # setup plant simulator + integrator = setup_acados_integrator(model, dt_0, num_stages=2, num_steps=2, integrator_type="IRK", + newton_iter=20, newton_tol=1e-10) + + return ocp_solver, integrator + + +def main(use_RTI=False): + + 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 + + ocp_solver, integrator = setup(x0, umax, dt_0, N_horizon, Tf, use_RTI) + + nx = ocp_solver.acados_ocp.dims.nx + nu = ocp_solver.acados_ocp.dims.nu + + Nsim = 50 + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) + + 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 = 10 + for _ in range(num_iter_initial): + ocp_solver.solve_for_x0(x0_bar = x0, fail_on_nonzero_status=False) + + # 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, :], fail_on_nonzero_status=False) + + 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}') + + # plot results + plot_furuta_pendulum(np.linspace(0, (Tf/N_horizon)*Nsim, Nsim+1),simX, simU, umax, plt_show=True) + + ocp_solver = None + + +if __name__ == '__main__': + main(use_RTI=False) + main(use_RTI=True) + diff --git a/examples/acados_python/furuta_pendulum/utils.py b/examples/acados_python/furuta_pendulum/utils.py new file mode 100644 index 0000000000..95c976fc68 --- /dev/null +++ b/examples/acados_python/furuta_pendulum/utils.py @@ -0,0 +1,28 @@ +import matplotlib.pyplot as plt +from acados_template import latexify_plot + +latexify_plot() + +def plot_furuta_pendulum(t_sim, X_sim, U_sim, u_max, plt_show=True): + + nx = 4 + nu = 1 + fig, axes = plt.subplots(nrows=nx+nu, ncols=1, sharex=True) + + labels_x = [r'$\theta_1$', r'$\theta_2$', r'$\dot{\theta}_1$', r'$\dot{\theta}_2$'] + + for i, (ax, l) in enumerate(zip(axes, labels_x)): + ax.plot(t_sim, X_sim[:, i], label=l) + ax.set_ylabel(l) + ax.grid() + ax.set_xlim(t_sim[0], t_sim[-1]) + + axes[-1].axhline(u_max, color='k', alpha=0.5, linestyle='dashed') + axes[-1].axhline(-u_max, color='k', alpha=0.5, linestyle='dashed') + axes[-1].stairs(U_sim.ravel(), t_sim) + axes[-1].set_ylabel(r"$\tau_1$") + axes[-1].grid() + axes[-1].set_xlabel(r'$t$') + + if plt_show: + plt.show() \ No newline at end of file diff --git a/examples/acados_python/generic_impl_dyn/minimal_example_ocp_generic_impl_dyn.py b/examples/acados_python/generic_impl_dyn/minimal_example_ocp_generic_impl_dyn.py index 0bea411218..846789d925 100644 --- a/examples/acados_python/generic_impl_dyn/minimal_example_ocp_generic_impl_dyn.py +++ b/examples/acados_python/generic_impl_dyn/minimal_example_ocp_generic_impl_dyn.py @@ -45,8 +45,8 @@ ocp.model = model Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -111,8 +111,8 @@ ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) status = ocp_solver.solve() 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 d5ebaa16a8..4daf3bb9dc 100644 --- a/examples/acados_python/getting_started/minimal_example_closed_loop.py +++ b/examples/acados_python/getting_started/minimal_example_closed_loop.py @@ -44,8 +44,8 @@ def setup(x0, Fmax, N_horizon, Tf, RTI=False): model = export_pendulum_ode_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -111,8 +111,8 @@ def main(use_RTI=False): nu = ocp_solver.acados_ocp.dims.nu Nsim = 100 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) simX[0,:] = x0 diff --git a/examples/acados_python/getting_started/minimal_example_ocp.py b/examples/acados_python/getting_started/minimal_example_ocp.py index 33da8cc8d0..ab5d9d1fc9 100644 --- a/examples/acados_python/getting_started/minimal_example_ocp.py +++ b/examples/acados_python/getting_started/minimal_example_ocp.py @@ -42,8 +42,8 @@ def main(): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() N = 20 # set dimensions @@ -82,8 +82,8 @@ def main(): ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") diff --git a/examples/acados_python/getting_started/minimal_example_sim.py b/examples/acados_python/getting_started/minimal_example_sim.py index 980965ce82..e2d0bc483a 100644 --- a/examples/acados_python/getting_started/minimal_example_sim.py +++ b/examples/acados_python/getting_started/minimal_example_sim.py @@ -45,8 +45,8 @@ def main(): sim.model = model Tf = 0.1 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() N = 200 # set simulation time diff --git a/examples/acados_python/getting_started/utils.py b/examples/acados_python/getting_started/utils.py index 30eeb66fc0..4a5440018a 100644 --- a/examples/acados_python/getting_started/utils.py +++ b/examples/acados_python/getting_started/utils.py @@ -33,10 +33,10 @@ from acados_template import latexify_plot -def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, latexify=False, plt_show=True, X_true_label=None): +def plot_pendulum(t, u_max, U, X_true, X_est=None, Y_measured=None, latexify=False, plt_show=True, X_true_label=None): """ Params: - shooting_nodes: time values of the discretization + t: 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) @@ -53,10 +53,9 @@ def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, N_sim = X_true.shape[0] nx = X_true.shape[1] - Tf = shooting_nodes[N_sim-1] - t = shooting_nodes - + Tf = t[N_sim-1] 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) @@ -76,7 +75,6 @@ def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, plt.xlim(t[0], t[-1]) plt.grid() - states_lables = ['$x$', r'$\theta$', '$v$', r'$\dot{\theta}$'] for i in range(nx): 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 e554a1049e..95a27e9971 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 @@ -78,8 +78,8 @@ def solve_marathos_ocp(setting): model = export_linear_mass_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nu # discretization 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 e3de99637c..340cbab7c9 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 @@ -52,8 +52,8 @@ def setup(x0, Fmax, N_horizon, Tf, algorithm, as_rti_iter=1): model = export_pendulum_ode_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -134,8 +134,8 @@ def main(algorithm='RTI', as_rti_iter=1): nu = ocp_solver.acados_ocp.dims.nu Nsim = 100 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) simX[0,:] = x0 diff --git a/examples/acados_python/pendulum_on_cart/common/utils.py b/examples/acados_python/pendulum_on_cart/common/utils.py index b372d9a11b..5904ada178 100644 --- a/examples/acados_python/pendulum_on_cart/common/utils.py +++ b/examples/acados_python/pendulum_on_cart/common/utils.py @@ -28,7 +28,6 @@ # POSSIBILITY OF SUCH DAMAGE.; # -import os import matplotlib.pyplot as plt import numpy as np from acados_template import latexify_plot @@ -97,6 +96,3 @@ def plot_pendulum(shooting_nodes, u_max, U, X_true, X_est=None, Y_measured=None, plt.subplots_adjust(left=None, bottom=None, right=None, top=None, hspace=0.4) - # avoid plotting when running on Travis - if os.environ.get('ACADOS_ON_CI') is None and plt_show: - plt.show() 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 2235f050e6..4a9bf73d23 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 @@ -46,8 +46,8 @@ def main(use_cython=False): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -106,8 +106,8 @@ def main(use_cython=False): else: ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json) - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) # call SQP_RTI solver in the loop: diff --git a/examples/acados_python/pendulum_on_cart/cython_example_closed_loop.py b/examples/acados_python/pendulum_on_cart/cython_example_closed_loop.py index 7ae24863fb..9fcbca7515 100644 --- a/examples/acados_python/pendulum_on_cart/cython_example_closed_loop.py +++ b/examples/acados_python/pendulum_on_cart/cython_example_closed_loop.py @@ -48,8 +48,8 @@ def main(): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N_horizon = 20 @@ -122,8 +122,8 @@ def main(): # create an integrator with the same settings as used in the OCP solver. # acados_integrator = AcadosSimSolver(ocp, json_file = solver_json) Nsim = 100 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) simX[0,:] = x0 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 29ef7ad588..354e0a6ad5 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 @@ -56,8 +56,8 @@ def create_ocp_solver(): ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -138,10 +138,10 @@ def main(): nu = acados_ocp_solver.acados_ocp.dims.nu Nsim = 100 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) - sens_u = np.ndarray((nu, nx)) - sens_x = np.ndarray((nx, nx)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) + sens_u = np.zeros((nu, nx)) + sens_x = np.zeros((nx, nx)) xcurrent = X0 simX[0,:] = xcurrent diff --git a/examples/acados_python/pendulum_on_cart/mhe/closed_loop_mhe_ocp.py b/examples/acados_python/pendulum_on_cart/mhe/closed_loop_mhe_ocp.py index 06be5fd692..202e1f2db1 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/closed_loop_mhe_ocp.py +++ b/examples/acados_python/pendulum_on_cart/mhe/closed_loop_mhe_ocp.py @@ -69,8 +69,8 @@ # ocp model and solver model = export_pendulum_ode_model() -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() Q_ocp = np.diag([1e3, 1e3, 1e-2, 1e-2]) R_ocp = 1 * np.eye(1) @@ -80,7 +80,7 @@ # mhe model and solver model_mhe = export_mhe_ode_model() -nw = model_mhe.u.size()[0] +nw = model_mhe.u.rows() ny = nx # inverse covariances, R_mhe has to be scaled with h diff --git a/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver.py b/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver.py index ac671ecb0f..82722cebfa 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver.py +++ b/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver.py @@ -44,9 +44,9 @@ def export_mhe_solver(model, N, h, Q, Q0, R): x = ocp_mhe.model.x u = ocp_mhe.model.u - nx = x.size()[0] - nu = u.size()[0] - nparam = model.p.size()[0] + nx = x.rows() + nu = u.rows() + nparam = model.p.rows() ny_0 = 3*nx # h(x), w and arrival cost ny = 2*nx # h(x), w diff --git a/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver_with_param.py b/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver_with_param.py index bd113be822..46aeb7f489 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver_with_param.py +++ b/examples/acados_python/pendulum_on_cart/mhe/export_mhe_solver_with_param.py @@ -42,9 +42,9 @@ def export_mhe_solver_with_param(model, N, h, Q, Q0, R, use_cython=False): ocp_mhe.model = model - nx_augmented = model.x.size()[0] - nu = model.u.size()[0] - nparam = model.p.size()[0] + nx_augmented = model.x.rows() + nu = model.u.rows() + nparam = model.p.rows() nx = nx_augmented-1 ny = R.shape[0] + Q.shape[0] # h(x), w diff --git a/examples/acados_python/pendulum_on_cart/mhe/export_ocp_solver.py b/examples/acados_python/pendulum_on_cart/mhe/export_ocp_solver.py index b2111d0f67..6faca82b23 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/export_ocp_solver.py +++ b/examples/acados_python/pendulum_on_cart/mhe/export_ocp_solver.py @@ -41,8 +41,8 @@ def export_ocp_solver(model, N, h, Q, R, Fmax=80, use_cython=False): ocp.model = model Tf = N*h - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx diff --git a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe.py b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe.py index 73333e6e64..faf49a5b32 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe.py +++ b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe.py @@ -51,8 +51,8 @@ # ocp model and solver model = export_pendulum_ode_model() -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() Q_ocp = np.diag([1e3, 1e3, 1e-2, 1e-2]) R_ocp = 1e-2 *np.eye(1) @@ -62,8 +62,8 @@ # mhe model and solver model_mhe = export_mhe_ode_model() -nx = model_mhe.x.size()[0] -nw = model_mhe.u.size()[0] +nx = model_mhe.x.rows() +nw = model_mhe.u.rows() ny = nx Q0_mhe = 100*np.eye((nx)) @@ -78,9 +78,9 @@ v_stds = [0.1, 0.01, 0.01, 0.01] v_stds = [0, 0, 0, 0] -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) -simY = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) +simY = np.zeros((N+1, nx)) simXest = np.zeros((N+1, nx)) simWest = np.zeros((N, nx)) diff --git a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_noisy_param.py b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_noisy_param.py index 79a580b9c5..aaa51353ef 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_noisy_param.py +++ b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_noisy_param.py @@ -54,8 +54,8 @@ # ocp model and solver model = export_pendulum_ode_model() -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() Q_ocp = np.diag([1e3, 1e3, 1e-2, 1e-2]) R_ocp = 1e-2 *np.eye(1) @@ -65,8 +65,8 @@ # mhe model and solver model_mhe = export_mhe_ode_model_with_noisy_param() -nx_augmented = model_mhe.x.size()[0] -nw = model_mhe.u.size()[0] +nx_augmented = model_mhe.x.rows() +nw = model_mhe.u.rows() ny = nx Q0_mhe = np.diag([0.1, 0.1, 0.1, 0.1, 1]) @@ -79,9 +79,9 @@ v_stds = [0.2, 0.5, 1, 1] v_stds = [0, 0, 0, 0] -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) -simY = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) +simY = np.zeros((N+1, nx)) simXest = np.zeros((N+1, nx)) simWest = np.zeros((N, nx_augmented)) diff --git a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_param.py b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_param.py index c2fc0af643..c62bc82a28 100644 --- a/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_param.py +++ b/examples/acados_python/pendulum_on_cart/mhe/minimal_example_mhe_with_param.py @@ -54,8 +54,8 @@ # ocp model and solver model = export_pendulum_ode_model() -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() Q_ocp = np.diag([1e3, 1e3, 1e-2, 1e-2]) R_ocp = 1e-2 *np.eye(1) @@ -65,8 +65,8 @@ # mhe model and solver model_mhe = export_mhe_ode_model_with_param() -nx_augmented = model_mhe.x.size()[0] -nw = model_mhe.u.size()[0] +nx_augmented = model_mhe.x.rows() +nw = model_mhe.u.rows() ny = nx Q0_mhe = np.diag([0.1, 0.01, 0.01, 0.01, 10]) @@ -79,9 +79,9 @@ v_stds = [0.1, 0.1, 0.1, 0.1] v_stds = [0, 0, 0, 0] -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) -simY = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) +simY = np.zeros((N+1, nx)) simXest = np.zeros((N+1, nx)) simWest = np.zeros((N, nx)) diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py b/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py index dc430ead89..5772f875e1 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_custom_hessian.py @@ -46,8 +46,8 @@ ocp.model = model Tf = 2. -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 40 @@ -115,8 +115,8 @@ ocp_solver.options_set("line_search_use_sufficient_descent", 0) ocp_solver.options_set("full_step_dual", 1) -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) for i, tau in enumerate(np.linspace(0, 1, N+1)): ocp_solver.set(i, 'x', x0*(1-tau) + tau*xf) 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 5b64453e29..748e14a3d8 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 @@ -80,8 +80,8 @@ ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -157,8 +157,8 @@ print("\nusing the make build system") ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() 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 index 022d85dbb8..5e5e4ba8b7 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_sqp_qpDUNES.py @@ -45,8 +45,8 @@ ocp.model = model Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -97,8 +97,8 @@ ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) status = ocp_solver.solve() 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 3be5b357af..91a3dd5220 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 @@ -46,8 +46,8 @@ def main(): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -106,8 +106,8 @@ def main(): ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) # call SQP_RTI solver in the loop: tol = 1e-6 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 76ea5a1c1d..630479a6ef 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 @@ -46,8 +46,8 @@ ocp.model = model Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -102,8 +102,8 @@ ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', cmake_builder=cmake_builder) -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) status = ocp_solver.solve() 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 683061792c..2a785f378e 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 @@ -58,8 +58,8 @@ model = export_pendulum_ode_model() ocp.model = model -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx @@ -122,8 +122,8 @@ # -------------------------------------------------------------------------------- # 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' -simX0 = np.ndarray((N0 + 1, nx)) -simU0 = np.ndarray((N0, nu)) +simX0 = np.zeros((N0 + 1, nx)) +simU0 = np.zeros((N0, nu)) print(80*'-') print(f'solve original code with N = {N0} and Tf = {Tf_01} s:') @@ -151,8 +151,8 @@ new_time_steps1 = np.tile(dt1, (N12,)) # Matlab's equivalent to repmat time1 = np.hstack([0, np.cumsum(new_time_steps1)]) -simX1 = np.ndarray((N12 + 1, nx)) -simU1 = np.ndarray((N12, nu)) +simX1 = np.zeros((N12 + 1, nx)) +simU1 = np.zeros((N12, nu)) ocp_solver.set_new_time_steps(new_time_steps1) print(80*'-') @@ -187,8 +187,8 @@ new_time_steps2 = np.tile(dt2, (N12,)) # Matlab's equivalent to repmat time2 = np.hstack([0, np.cumsum(new_time_steps2)]) -simX2 = np.ndarray((N12 + 1, nx)) -simU2 = np.ndarray((N12, nu)) +simX2 = np.zeros((N12 + 1, nx)) +simU2 = np.zeros((N12, nu)) ocp_solver.set_new_time_steps(new_time_steps2) print(80*'-') 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 8146d57050..14d8d628a3 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 @@ -60,8 +60,8 @@ def main(discretization='shooting_nodes'): ocp.gnsf_model = gnsf_dict Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 15 @@ -146,8 +146,8 @@ def main(discretization='shooting_nodes'): # ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) # change options after creating ocp_solver ocp_solver.options_set("step_length", 0.99999) 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 d447f141b3..a6c57356e1 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 @@ -54,7 +54,7 @@ def formulate_ocp(cost_version: str) -> AcadosOcp: if cost_version in ['EXTERNAL_Z','NLS_Z', 'LS_Z', 'CONL_Z']: model = export_augmented_pendulum_model() - nz = model.z.size()[0] + nz = model.z.rows() else: model = export_pendulum_ode_model() @@ -255,8 +255,8 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot= ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.02, 2000, 2000, 0.02, 0.02, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([2000, 2000, 0.02, 0.02, ])) - simX = np.ndarray((N+1, NX)) - simU = np.ndarray((N, NU)) + simX = np.zeros((N+1, NX)) + simU = np.zeros((N, NU)) status = ocp_solver.solve() 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 38e0b0e81a..fb658859db 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 @@ -58,8 +58,8 @@ def test_initial_h_constraints(constraint_version: str): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -152,8 +152,8 @@ def test_initial_h_constraints(constraint_version: str): ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() if status != 0: diff --git a/examples/acados_python/pendulum_on_cart/ocp/simulink_example.py b/examples/acados_python/pendulum_on_cart/ocp/simulink_example.py index f8b6486db1..d4aa5784a3 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/simulink_example.py +++ b/examples/acados_python/pendulum_on_cart/ocp/simulink_example.py @@ -45,8 +45,8 @@ def main(): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() N = 20 # set dimensions @@ -89,8 +89,8 @@ def main(): # create solver ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', simulink_opts=simulink_opts) - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") 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 74fc7e7226..c7e91ee87f 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 @@ -44,8 +44,8 @@ sim.model = model Tf = 0.1 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() N = 200 # set simulation time @@ -105,7 +105,7 @@ cmake_builder = sim_get_default_cmake_builder() acados_integrator = AcadosSimSolver(sim, cmake_builder=cmake_builder) -simX = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) u0 = np.array([0.0]) acados_integrator.set("u", u0) 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 6d3e25ab1a..4bf7145e7c 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 @@ -45,8 +45,8 @@ def main(build=True, generate=True, use_cmake=True, use_cython=False): sim.model = model Tf = 0.1 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() N = 200 # set simulation time @@ -73,7 +73,7 @@ def main(build=True, generate=True, use_cmake=True, use_cython=False): else: acados_integrator = AcadosSimSolver(sim, cmake_builder=cmake_builder, generate=generate, build=build) - simX = np.ndarray((N+1, nx)) + simX = np.zeros((N+1, nx)) x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) u0 = np.array([0.0]) acados_integrator.set("u", u0) diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py index 10a798664b..ab23718e12 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py @@ -134,8 +134,8 @@ def export_parameter_augmented_ocp( # set dimensions ocp.dims.N = N_horizon - nu = ocp.model.u.size()[0] - nx = ocp.model.x.size()[0] + nu = ocp.model.u.rows() + nx = ocp.model.x.rows() # set cost Q_mat = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) @@ -308,8 +308,8 @@ def main(param_M_as_state: bool, idxp: int, qp_solver_ric_alg: int, eigen_analys # if i < 0: # nx = max(x0.shape) # nu = 1 - # simX = np.ndarray((N_horizon+1, nx)) - # simU = np.ndarray((N_horizon, nu)) + # simX = np.zeros((N_horizon+1, nx)) + # simU = np.zeros((N_horizon, nu)) # for i in range(N_horizon): # simX[i, :] = acados_ocp_solver.get(i, "x") # simU[i, :] = acados_ocp_solver.get(i, "u") 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 fc2255f6a3..6e1341e6db 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 @@ -68,8 +68,8 @@ def create_ocp_description(hessian_approx, linearized_dynamics=False, discrete=F ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx ocp.dims.N = N diff --git a/examples/acados_python/pmsm_example/main.py b/examples/acados_python/pmsm_example/main.py index 846d603040..9ba2eae8e1 100644 --- a/examples/acados_python/pmsm_example/main.py +++ b/examples/acados_python/pmsm_example/main.py @@ -249,8 +249,8 @@ def run_simulation(qp_solver="FULL_CONDENSING_HPIPM", show_plots=False, verbose= ocp.model = model # model dims - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nu + nx ny_e = nx Tf = N * Ts @@ -416,10 +416,10 @@ def run_simulation(qp_solver="FULL_CONDENSING_HPIPM", show_plots=False, verbose= # closed loop simulation Nsim = 20 - simX = np.ndarray((Nsim, nx)) - simU = np.ndarray((Nsim, nu)) - simXR = np.ndarray((Nsim + 1, nx)) - simXRN = np.ndarray((Nsim, nx)) + simX = np.zeros((Nsim, nx)) + simU = np.zeros((Nsim, nu)) + simXR = np.zeros((Nsim + 1, nx)) + simXRN = np.zeros((Nsim, nx)) print("=========================================") print("Mode = ", FORMULATION) diff --git a/examples/acados_python/race_cars/acados_settings.py b/examples/acados_python/race_cars/acados_settings.py index b3eac56625..7af3291749 100644 --- a/examples/acados_python/race_cars/acados_settings.py +++ b/examples/acados_python/race_cars/acados_settings.py @@ -59,8 +59,8 @@ def acados_settings(Tf, N, track_file): model_ac.con_h_expr = constraint.expr # set dimensions - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx diff --git a/examples/acados_python/race_cars/acados_settings_dev.py b/examples/acados_python/race_cars/acados_settings_dev.py index e49f3a59ab..b8ca80a647 100644 --- a/examples/acados_python/race_cars/acados_settings_dev.py +++ b/examples/acados_python/race_cars/acados_settings_dev.py @@ -59,8 +59,8 @@ def acados_settings(Tf, N, track_file): model_ac.con_h_expr = constraint.expr # dimensions - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx diff --git a/examples/acados_python/race_cars/main.py b/examples/acados_python/race_cars/main.py index fcbed94bbd..7cc316a250 100644 --- a/examples/acados_python/race_cars/main.py +++ b/examples/acados_python/race_cars/main.py @@ -55,14 +55,14 @@ constraint, model, acados_solver = acados_settings(Tf, N, track) # dimensions -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu Nsim = int(T * N / Tf) # initialize data structs -simX = np.ndarray((Nsim, nx)) -simU = np.ndarray((Nsim, nu)) +simX = np.zeros((Nsim, nx)) +simU = np.zeros((Nsim, nu)) s0 = model.x0[0] tcomp_sum = 0 tcomp_max = 0 diff --git a/examples/acados_python/rsm_example/main.py b/examples/acados_python/rsm_example/main.py index c145a5bb11..dcd006cbad 100644 --- a/examples/acados_python/rsm_example/main.py +++ b/examples/acados_python/rsm_example/main.py @@ -171,9 +171,9 @@ def create_ocp_solver(tol = 1e-3): model = export_rsm_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] - nz = model.z.size()[0] + nx = model.x.rows() + nu = model.u.rows() + nz = model.z.rows() ny = nu + nx ny_e = nx Tf = N*Ts @@ -305,9 +305,9 @@ def main(): if USE_PLANT: plant = setup_acados_integrator(acados_solver.acados_ocp) - simX = np.ndarray((Nsim, nx)) - simU = np.ndarray((Nsim, nu)) - simY = np.ndarray((Nsim, nu+nx)) + simX = np.zeros((Nsim, nx)) + simU = np.zeros((Nsim, nu)) + simY = np.zeros((Nsim, nu+nx)) times_prep = np.zeros(Nsim) times_feed = np.zeros(Nsim) diff --git a/examples/acados_python/tests/reset_test.py b/examples/acados_python/tests/reset_test.py index 7538bafb8a..5cc973dfac 100644 --- a/examples/acados_python/tests/reset_test.py +++ b/examples/acados_python/tests/reset_test.py @@ -50,8 +50,8 @@ def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_n ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -154,8 +154,8 @@ def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_n ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ])) - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() diff --git a/examples/acados_python/tests/soft_constraint_test.py b/examples/acados_python/tests/soft_constraint_test.py index f3fb80d39d..c01cfe961e 100644 --- a/examples/acados_python/tests/soft_constraint_test.py +++ b/examples/acados_python/tests/soft_constraint_test.py @@ -60,8 +60,8 @@ def run_closed_loop_experiment(soft_constr_type='bx', verbose=False, qp_solver=' ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -147,8 +147,8 @@ def run_closed_loop_experiment(soft_constr_type='bx', verbose=False, qp_solver=' # closed loop Nsim = 20 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) xcurrent = x0 qp_iter = np.zeros(Nsim) sqp_iter = np.zeros(Nsim) diff --git a/examples/acados_python/tests/test_cost_integration_euler.py b/examples/acados_python/tests/test_cost_integration_euler.py index ac701140d3..cc92e7b717 100644 --- a/examples/acados_python/tests/test_cost_integration_euler.py +++ b/examples/acados_python/tests/test_cost_integration_euler.py @@ -52,8 +52,8 @@ def solve_ocp(cost_discretization, cost_variant): model = export_pendulum_ode_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -130,8 +130,8 @@ def solve_ocp(cost_discretization, cost_variant): ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) - simX = np.ndarray((N + 1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N + 1, nx)) + simU = np.zeros((N, nu)) print(80*'-') print(f'solve OCP with cost variant {cost_variant} discretization {cost_discretization} N = {N} and Tf = {Tf} s:') diff --git a/examples/acados_python/tests/test_cost_integration_value.py b/examples/acados_python/tests/test_cost_integration_value.py index c0d7902524..7ff77ac68f 100644 --- a/examples/acados_python/tests/test_cost_integration_value.py +++ b/examples/acados_python/tests/test_cost_integration_value.py @@ -53,8 +53,8 @@ def solve_ocp(cost_variant, num_stages): ocp = AcadosOcp() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -137,8 +137,8 @@ def solve_ocp(cost_variant, num_stages): ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) - simX = np.ndarray((N + 1, nx+1)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N + 1, nx+1)) + simU = np.zeros((N, nu)) print(80*'-') print(f'solve original code with N = {N} and Tf = {Tf} s:') diff --git a/examples/acados_python/tests/test_cython_ctypes.py b/examples/acados_python/tests/test_cython_ctypes.py index cafdb53c45..b62de1ffa7 100644 --- a/examples/acados_python/tests/test_cython_ctypes.py +++ b/examples/acados_python/tests/test_cython_ctypes.py @@ -49,8 +49,8 @@ def main(interface_type='ctypes'): model = export_pendulum_ode_model() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx @@ -128,8 +128,8 @@ def main(interface_type='ctypes'): # -------------------------------------------------------------------------------- # 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' nvariant = 0 - simX0 = np.ndarray((N0 + 1, nx)) - simU0 = np.ndarray((N0, nu)) + simX0 = np.zeros((N0 + 1, nx)) + simU0 = np.zeros((N0, nu)) print(80*'-') print(f'solve original code with N = {N0} and Tf = {Tf_01} s:') @@ -159,8 +159,8 @@ def main(interface_type='ctypes'): # new_time_steps1 = np.tile(dt1, (N12,)) # Matlab's equivalent to repmat # time1 = np.hstack([0, np.cumsum(new_time_steps1)]) - # simX1 = np.ndarray((N12 + 1, nx)) - # simU1 = np.ndarray((N12, nu)) + # simX1 = np.zeros((N12 + 1, nx)) + # simU1 = np.zeros((N12, nu)) # ocp_solver.set_new_time_steps(new_time_steps1) # print(80*'-') @@ -199,8 +199,8 @@ def main(interface_type='ctypes'): # new_time_steps2 = np.tile(dt2, (N12,)) # Matlab's equivalent to repmat # time2 = np.hstack([0, np.cumsum(new_time_steps2)]) - # simX2 = np.ndarray((N12 + 1, nx)) - # simU2 = np.ndarray((N12, nu)) + # simX2 = np.zeros((N12 + 1, nx)) + # simU2 = np.zeros((N12, nu)) # ocp_solver.set_new_time_steps(new_time_steps2) # print(80*'-') diff --git a/examples/acados_python/tests/test_ggn_cost_integration.py b/examples/acados_python/tests/test_ggn_cost_integration.py index 4db7549a42..dc1da9d8d6 100644 --- a/examples/acados_python/tests/test_ggn_cost_integration.py +++ b/examples/acados_python/tests/test_ggn_cost_integration.py @@ -53,8 +53,8 @@ def solve_ocp(cost_discretization, cost_type, num_stages, collocation_type): ocp = AcadosOcp() ocp.model = model - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu Tf = 1.0 @@ -133,8 +133,8 @@ def solve_ocp(cost_discretization, cost_type, num_stages, collocation_type): ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) - simX = np.ndarray((N + 1, nx+1)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N + 1, nx+1)) + simU = np.zeros((N, nu)) print(80*'-') print(f'solve OCP with {cost_type} {cost_discretization} N = {N} and Tf = {Tf} s:') diff --git a/examples/acados_python/tests/test_ocp_setting.py b/examples/acados_python/tests/test_ocp_setting.py index 5a8e350839..654db473d7 100644 --- a/examples/acados_python/tests/test_ocp_setting.py +++ b/examples/acados_python/tests/test_ocp_setting.py @@ -151,8 +151,8 @@ ocp.model = model Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -277,8 +277,8 @@ ocp_solver.set(i, "u", u_init[i]) # solve ocp -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) status = ocp_solver.solve() diff --git a/examples/acados_python/tests/test_osqp.py b/examples/acados_python/tests/test_osqp.py index 52bfe29110..f6e7f48f52 100644 --- a/examples/acados_python/tests/test_osqp.py +++ b/examples/acados_python/tests/test_osqp.py @@ -45,8 +45,8 @@ ocp.model = model Tf = 1.0 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -106,8 +106,8 @@ # ocp_solver.options_set("qp_solver_warm_start", 1) -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) # test setter ocp_solver.set(0, "u", 0.0) diff --git a/examples/acados_python/tests/test_parametric_nonlinear_constraint_h.py b/examples/acados_python/tests/test_parametric_nonlinear_constraint_h.py index bf711c80b5..a763e5f7a8 100644 --- a/examples/acados_python/tests/test_parametric_nonlinear_constraint_h.py +++ b/examples/acados_python/tests/test_parametric_nonlinear_constraint_h.py @@ -51,8 +51,8 @@ u = model.u Tf = 1.0 -nx = x.size()[0] -nu = u.size()[0] +nx = x.rows() +nu = u.rows() ny = nx + nu ny_e = nx N = 20 @@ -155,8 +155,8 @@ ocp_solver.set_params_sparse(i, np.array(range(n_param)), np.zeros(n_param)) ocp_solver.set_params_sparse(i, np.ascontiguousarray([0, 1]), np.array([1.0, 1.0])) -simX = np.ndarray((N+1, nx)) -simU = np.ndarray((N, nu)) +simX = np.zeros((N+1, nx)) +simU = np.zeros((N, nu)) status = ocp_solver.solve() diff --git a/examples/acados_python/tests/test_sim_dae.py b/examples/acados_python/tests/test_sim_dae.py index 79ed45ca77..137752b7dc 100644 --- a/examples/acados_python/tests/test_sim_dae.py +++ b/examples/acados_python/tests/test_sim_dae.py @@ -45,8 +45,8 @@ sim.model = model Tf = 0.1 -nx = model.x.size()[0] -nu = model.u.size()[0] +nx = model.x.rows() +nu = model.u.rows() N = 200 # set simulation time @@ -69,7 +69,7 @@ # create acados_integrator = AcadosSimSolver(sim) -simX = np.ndarray((N+1, nx)) +simX = np.zeros((N+1, nx)) x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) u0_val = 2.0 diff --git a/examples/acados_python/time_varying/piecewiese_polynomial_control_example.py b/examples/acados_python/time_varying/piecewiese_polynomial_control_example.py index d16f86f350..699f47a305 100644 --- a/examples/acados_python/time_varying/piecewiese_polynomial_control_example.py +++ b/examples/acados_python/time_varying/piecewiese_polynomial_control_example.py @@ -203,7 +203,7 @@ def main_mocp(cost_type='NONLINEAR_LS', explicit_symmetric_penalties=True, penal print(f"found optimal cost: {cost_val:.4e}") # get solution - simX = np.ndarray((N_horizon+1, nx)) + simX = np.zeros((N_horizon+1, nx)) simU = [] for i in range(N_horizon): simX[i,:] = ocp_solver.get(i, "x") @@ -253,8 +253,8 @@ def main_ocp(cost_type='NONLINEAR_LS', explicit_symmetric_penalties=True, penalt print(f"found optimal cost: {cost_val:.4e}") # get solution - simX = np.ndarray((N_horizon+1, nx)) - simU = np.ndarray((N_horizon, nu)) + 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") diff --git a/examples/acados_python/time_varying/test_polynomial_controls_and_penalties.py b/examples/acados_python/time_varying/test_polynomial_controls_and_penalties.py index bb43ee63a5..06789ea9c5 100644 --- a/examples/acados_python/time_varying/test_polynomial_controls_and_penalties.py +++ b/examples/acados_python/time_varying/test_polynomial_controls_and_penalties.py @@ -39,8 +39,8 @@ def test_polynomial_controls_and_penalties(): print(f"found optimal cost: {cost_val:.4e}") # get solution - simX = np.ndarray((N_horizon+1, nx)) - simU = np.ndarray((N_horizon, nu)) + 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") diff --git a/examples/acados_python/time_varying/test_time_varying_irk.py b/examples/acados_python/time_varying/test_time_varying_irk.py index 015c98be16..88145933d7 100644 --- a/examples/acados_python/time_varying/test_time_varying_irk.py +++ b/examples/acados_python/time_varying/test_time_varying_irk.py @@ -83,7 +83,7 @@ def main(): sim.model = model deltaT = 1.0 - nx = model.x.size()[0] + nx = model.x.rows() # set simulation time sim.solver_options.T = deltaT diff --git a/examples/acados_python/time_varying/time_accelerating_pendulum_simulation.py b/examples/acados_python/time_varying/time_accelerating_pendulum_simulation.py index b0bbdab37b..3db053e71d 100644 --- a/examples/acados_python/time_varying/time_accelerating_pendulum_simulation.py +++ b/examples/acados_python/time_varying/time_accelerating_pendulum_simulation.py @@ -110,8 +110,8 @@ def main(): sim.model = model deltaT = 0.01 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() N = 800 # set simulation time diff --git a/examples/acados_python/timing_example/reset_timing.py b/examples/acados_python/timing_example/reset_timing.py index f55a6237a7..ea282550f8 100644 --- a/examples/acados_python/timing_example/reset_timing.py +++ b/examples/acados_python/timing_example/reset_timing.py @@ -50,8 +50,8 @@ def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_n ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -182,8 +182,8 @@ def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_n ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ])) - simX = np.ndarray((N+1, nx)) - simU = np.ndarray((N, nu)) + simX = np.zeros((N+1, nx)) + simU = np.zeros((N, nu)) status = ocp_solver.solve() diff --git a/examples/acados_python/zoRO_example/cstr/main.py b/examples/acados_python/zoRO_example/cstr/main.py index c61bd909c2..33fb04d5cd 100644 --- a/examples/acados_python/zoRO_example/cstr/main.py +++ b/examples/acados_python/zoRO_example/cstr/main.py @@ -65,8 +65,8 @@ def simulate( nx = X_ref.shape[1] nu = U_ref.shape[1] - X = np.ndarray((Nsim + 1, nx)) - U = np.ndarray((Nsim, nu)) + X = np.zeros((Nsim + 1, nx)) + U = np.zeros((Nsim, nu)) timings_solver = np.zeros((Nsim)) timings_integrator = np.zeros((Nsim)) diff --git a/examples/acados_python/zoRO_example/pendulum_on_cart/minimal_example_zoro.py b/examples/acados_python/zoRO_example/pendulum_on_cart/minimal_example_zoro.py index 2f805b7a1e..fcdfead1d5 100644 --- a/examples/acados_python/zoRO_example/pendulum_on_cart/minimal_example_zoro.py +++ b/examples/acados_python/zoRO_example/pendulum_on_cart/minimal_example_zoro.py @@ -50,8 +50,8 @@ def main(): ocp.model = model Tf = 1.0 - nx = model.x.size()[0] - nu = model.u.size()[0] + nx = model.x.rows() + nu = model.u.rows() ny = nx + nu ny_e = nx N = 20 @@ -145,8 +145,8 @@ def main(): ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', simulink_opts=simulink_opts) Nsim = 100 - simX = np.ndarray((Nsim+1, nx)) - simU = np.ndarray((Nsim, nu)) + simX = np.zeros((Nsim+1, nx)) + simU = np.zeros((Nsim, nu)) simX[0,:] = x_init # zoro parameters