From 95c341c61306c8767f1e8d7bb2848dac62ddccd2 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 2 Aug 2024 14:42:26 +0200 Subject: [PATCH 01/11] One sided constraint support for HPIPM solvers (#1174) - add `ACADOS_INFTY` in Python and C, `get_acados_infty` in Matlab. - If any constraints have bounds `ACADOS_INFTY`, the constraint is marked as not relevant in the QP representation object `ocp_qp`, by setting the corresponding `d_mask` values to `0`. - The underlying QP solver interface should make use of this mask. - The HPIPM solvers do make use of this natively. - For now the value is `ACADOS_INFTY=1e10` which might can be changed later after some more extensive testing. - Support for that in QP solvers other than HPIPM is not included in this PR and tracked in https://github.com/acados/acados/issues/650 --- acados/ocp_nlp/ocp_nlp_common.c | 1 + acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 39 +++++ acados/ocp_nlp/ocp_nlp_constraints_bgh.h | 1 + acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 49 +++++- acados/ocp_nlp/ocp_nlp_constraints_bgp.h | 3 + acados/ocp_nlp/ocp_nlp_constraints_common.h | 1 + acados/utils/types.h | 3 +- .../tests/one_sided_constraints_test.py | 154 ++++++++++++++++++ .../acados_python/tests/test_cython_ctypes.py | 114 ++----------- interfaces/CMakeLists.txt | 7 +- .../acados_matlab_octave/get_acados_infty.m | 34 ++++ .../acados_template/__init__.py | 2 +- .../acados_template/acados_template/utils.py | 1 + 13 files changed, 302 insertions(+), 107 deletions(-) create mode 100644 examples/acados_python/tests/one_sided_constraints_test.py create mode 100644 interfaces/acados_matlab_octave/get_acados_infty.m diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 0c46d817ca..ce8c1aa1b1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -2114,6 +2114,7 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->constraints[i]->memory_set_idxb_ptr(nlp_mem->qp_in->idxb[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxs_rev_ptr(nlp_mem->qp_in->idxs_rev[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxe_ptr(nlp_mem->qp_in->idxe[i], nlp_mem->constraints[i]); + config->constraints[i]->memory_set_dmask_ptr(nlp_mem->qp_in->d_mask+i, nlp_mem->constraints[i]); } // alias to regularize memory diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index f76e081565..60478c4bf4 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -1123,6 +1123,12 @@ void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, vo memory->z_alg = z_alg; } +void ocp_nlp_constraints_bgh_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) +{ + ocp_nlp_constraints_bgh_memory *memory = memory_; + + memory->dmask = dmask; +} void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_) @@ -1606,6 +1612,9 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model // 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); + // fun = fun * mask + blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + return; } @@ -1644,6 +1653,35 @@ void ocp_nlp_constraints_bgh_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); + // Set dmask for QP: 0 means unconstrained. + for (int i = 0; i < nb+ng+nh; i++) + { + if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) + { + // printf("found upper infinity bound\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + for (int i = nb+ng+nh; i < 2*(nb+ng+nh); i++) + { + if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) + { + // printf("found upper infinity bound\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + for (int i = 2*(nb+ng+nh); i < 2*(nb+ng+nh+ns); i++) + { + if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) + { + // printf("found lower infinity bound on slacks\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + + // fun = fun * mask + blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + return; } @@ -1674,6 +1712,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgh_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgh_memory_set_z_alg_ptr; + config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgh_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgh_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 126d9c5c3e..6582f09344 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -150,6 +150,7 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp memory struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index bf7b5ab77d..29ee7fb4ba 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -629,11 +629,11 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, int nge = dims->nge; int nphie = dims->nphie; - if (!strcmp(field, "lb")) // TODO(fuck_lint) remove !!! + if (!strcmp(field, "lb")) { blasfeo_pack_dvec(nb, value, 1, &model->d, 0); } - else if (!strcmp(field, "ub")) // TODO(fuck_lint) remove !!! + else if (!strcmp(field, "ub")) { blasfeo_pack_dvec(nb, value, 1, &model->d, nb+ng+nphi); } @@ -689,7 +689,7 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, { model->nl_constr_phi_o_r_fun = value; } - else if (!strcmp(field, "lphi")) // TODO(fuck_lint) remove + else if (!strcmp(field, "lphi")) { blasfeo_pack_dvec(nphi, value, 1, &model->d, nb+ng); } @@ -1031,6 +1031,12 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * } +void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) +{ + ocp_nlp_constraints_bgp_memory *memory = memory_; + memory->dmask = dmask; +} + void ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_) { @@ -1460,6 +1466,9 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model // 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); + // fun = fun * mask + blasfeo_dvecmul(2*(nb+ng+nphi), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + return; } @@ -1501,6 +1510,39 @@ void ocp_nlp_constraints_bgp_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); + // Set dmask for QP: 0 means unconstrained. + for (int i = 0; i < nb+ng+nphi; i++) + { + if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) + { + // printf("found upper infinity bound\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + for (int i = nb+ng+nphi; i < 2*(nb+ng+nphi); i++) + { + if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) + { + // printf("found upper infinity bound\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + for (int i = 2*(nb+ng+nphi); i < 2*(nb+ng+nphi+ns); i++) + { + if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) + { + // printf("found lower infinity bound on slacks\n"); + BLASFEO_DVECEL(memory->dmask, i) = 0; + } + } + + // fun = fun * mask + blasfeo_dvecmul(2*(nb+ng+nphi+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + + // printf("BGH mask\n"); + // blasfeo_print_tran_dvec(2*(nb+ng+nphi), memory->dmask, 0); + // blasfeo_print_exp_tran_dvec(2*(nb+ng+nphi), &model->d, 0); + return; } @@ -1532,6 +1574,7 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgp_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgp_memory_set_z_alg_ptr; + config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgp_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgp_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index 03e0665b0b..0e50fc8ea3 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -136,6 +136,7 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dvec *dmask; // pointer to dmask in qp_in struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory @@ -162,6 +163,8 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * // void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // +void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_); +// void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); // void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index 70f5cf6ec2..1454e54c2f 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -74,6 +74,7 @@ typedef struct void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory); void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); + void (*memory_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *memory); void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory); void (*memory_set_idxb_ptr)(int *idxb, void *memory); void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory); diff --git a/acados/utils/types.h b/acados/utils/types.h index f67f850205..4b4b0be121 100644 --- a/acados/utils/types.h +++ b/acados/utils/types.h @@ -56,8 +56,7 @@ extern "C" { #define MAX_STR_LEN 256 #define ACADOS_EPS 1e-12 -#define ACADOS_NEG_INFTY -1.0e9 -#define ACADOS_POS_INFTY +1.0e9 +#define ACADOS_INFTY 1e10 #define UNUSED(x) ((void)(x)) diff --git a/examples/acados_python/tests/one_sided_constraints_test.py b/examples/acados_python/tests/one_sided_constraints_test.py new file mode 100644 index 0000000000..4862512500 --- /dev/null +++ b/examples/acados_python/tests/one_sided_constraints_test.py @@ -0,0 +1,154 @@ +# This test is an extension of the 'minimal_example_ocp_reuse_code.py' example. +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# +import sys + +sys.path.insert(0, '../pendulum_on_cart/common') + +from acados_template import AcadosOcp, AcadosOcpSolver, ACADOS_INFTY +from pendulum_model import export_pendulum_ode_model +import numpy as np +import scipy.linalg +from utils import plot_pendulum + +PLOT = False + +def main(constraint_variant='one_sided'): + + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + nx = model.x.rows() + nu = model.u.rows() + ny = nx + nu + ny_e = nx + + N_horizon = 20 # number of shooting nodes + Tf = 1.0 + + # set dimensions + ocp.dims.N = N_horizon + + # set cost + Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) + R = 2 * np.diag([1e-2]) + + ocp.cost.W_e = Q + ocp.cost.W = scipy.linalg.block_diag(Q, R) + + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vx[:nx, :nx] = np.eye(nx) + + Vu = np.zeros((ny, nu)) + Vu[4, 0] = 1.0 + ocp.cost.Vu = Vu + + ocp.cost.Vx_e = np.eye(nx) + + ocp.cost.yref = np.zeros((ny,)) + ocp.cost.yref_e = np.zeros((ny_e,)) + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + if constraint_variant == 'one_sided': + ocp.constraints.lbx = np.array([-ACADOS_INFTY]) + ocp.constraints.ubx = np.array([+5.0]) + ocp.constraints.idxbx = np.array([0]) + expected_status = 0 + elif constraint_variant == 'one_sided_wrong_infty': + ocp.constraints.lbx = np.array([-0.5*ACADOS_INFTY]) + ocp.constraints.ubx = np.array([+5.0]) + ocp.constraints.idxbx = np.array([0]) + # complementarity residual does not converge to tolerance if infty is too large + expected_status = 2 + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.tol = 1e-7 + + # set prediction horizon + ocp.solver_options.tf = Tf + + # create solver + ocp_solver = AcadosOcpSolver(ocp) + + # solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' + simX0 = np.zeros((N_horizon + 1, nx)) + simU0 = np.zeros((N_horizon, nu)) + + print(80*'-') + print(f'solve original code with N = {N_horizon} and Tf = {Tf} s:') + status = ocp_solver.solve() + ocp_solver.print_statistics() + + if status != expected_status: + raise Exception(f"expected status {expected_status}, got {status} for constraint_variant {constraint_variant}.") + + # get solution + for i in range(N_horizon): + simX0[i, :] = ocp_solver.get(i, "x") + simU0[i, :] = ocp_solver.get(i, "u") + simX0[N_horizon, :] = ocp_solver.get(N_horizon, "x") + + lambdas = [ocp_solver.get(i, "lam") for i in range(1, N_horizon)] + for lam in lambdas: + assert np.all(lam >= 0) + + # if unbounded constraint is defined properly, lambda should be zero + i_infty = 1 + if constraint_variant == 'one_sided': + assert lam[i_infty] == 0 + elif constraint_variant == 'one_sided_wrong_infty': + assert lam[i_infty] != 0 + + if PLOT: + plot_pendulum(np.linspace(0, Tf, N_horizon + 1), Fmax, simU0, simX0, latexify=False, plt_show=True, X_true_label=f'N={N_horizon}, Tf={Tf}') + + ocp_solver = None + +if __name__ == "__main__": + main(constraint_variant='one_sided') + main(constraint_variant='one_sided_wrong_infty') diff --git a/examples/acados_python/tests/test_cython_ctypes.py b/examples/acados_python/tests/test_cython_ctypes.py index b62de1ffa7..c045d526b1 100644 --- a/examples/acados_python/tests/test_cython_ctypes.py +++ b/examples/acados_python/tests/test_cython_ctypes.py @@ -54,15 +54,11 @@ def main(interface_type='ctypes'): ny = nx + nu ny_e = nx - # define the different options for the use-case demonstration - N0 = 20 # original number of shooting nodes - N12 = 15 # change the number of shooting nodes for use-cases 1 and 2 - condN12 = max(1, round(N12/1)) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only) - Tf_01 = 1.0 # original final time and for use-case 1 - Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical) + N_horizon = 20 # number of shooting nodes + Tf = 1.0 # set dimensions - ocp.dims.N = N0 + ocp.dims.N = N_horizon # set cost Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) @@ -95,16 +91,13 @@ def main(interface_type='ctypes'): ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) # set options - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES - # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, - # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' - # ocp.solver_options.print_level = 1 - ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.nlp_solver_type = 'SQP' # set prediction horizon - ocp.solver_options.tf = Tf_01 + ocp.solver_options.tf = Tf print(80*'-') print('generate code and compile...') @@ -125,108 +118,29 @@ def main(interface_type='ctypes'): ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) - # -------------------------------------------------------------------------------- - # 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' + # solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' nvariant = 0 - simX0 = np.zeros((N0 + 1, nx)) - simU0 = np.zeros((N0, nu)) + simX0 = np.zeros((N_horizon + 1, nx)) + simU0 = np.zeros((N_horizon, nu)) print(80*'-') - print(f'solve original code with N = {N0} and Tf = {Tf_01} s:') + print(f'solve original code with N = {N_horizon} and Tf = {Tf} s:') status = ocp_solver.solve() + ocp_solver.print_statistics() if status != 0: - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") raise Exception(f'acados returned status {status}.') # get solution - for i in range(N0): + for i in range(N_horizon): simX0[i, :] = ocp_solver.get(i, "x") simU0[i, :] = ocp_solver.get(i, "u") - simX0[N0, :] = ocp_solver.get(N0, "x") + simX0[N_horizon, :] = ocp_solver.get(N_horizon, "x") - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True) if PLOT:# plot but don't halt - plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}') - - # # -------------------------------------------------------------------------------- - # # 1) now reuse the code but set a new time-steps vector, with a new number of elements - # nvariant = 1 - # dt1 = Tf_01 / N12 - - # new_time_steps1 = np.tile(dt1, (N12,)) # Matlab's equivalent to repmat - # time1 = np.hstack([0, np.cumsum(new_time_steps1)]) - - # simX1 = np.zeros((N12 + 1, nx)) - # simU1 = np.zeros((N12, nu)) - - # ocp_solver.set_new_time_steps(new_time_steps1) - # print(80*'-') - # if ocp.solver_options.qp_solver.startswith('PARTIAL'): - # ocp_solver.update_qp_solver_cond_N(condN12) - # print(f'solve use-case 2 with N = {N12}, cond_N = {condN12} and Tf = {Tf_01} s (instead of {Tf_01} s):') - # X_true_label = f'use-case 1: N={N12}, N_cond = {condN12}' - # else: - # print(f'solve use-case 2 with N = {N12} and Tf = {Tf_01} s (instead of {Tf_01} s):') - # X_true_label = f'use-case 1: N={N12}' - - # status = ocp_solver.solve() - - # if status != 0: - # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # raise Exception(f'acados returned status {status}.') - - # # get solution - # for i in range(N12): - # simX1[i, :] = ocp_solver.get(i, "x") - # simU1[i, :] = ocp_solver.get(i, "u") - # simX1[N12, :] = ocp_solver.get(N12, "x") - - # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True) - - - # if PLOT: - # plot_pendulum(time1, Fmax, simU1, simX1, latexify=False, plt_show=False, X_true_label=X_true_label) - - # # -------------------------------------------------------------------------------- - # # 2) reuse the code again, set a new time-steps vector, only with a different final time - # nvariant = 2 - # dt2 = Tf_2 / N12 - - # new_time_steps2 = np.tile(dt2, (N12,)) # Matlab's equivalent to repmat - # time2 = np.hstack([0, np.cumsum(new_time_steps2)]) - - # simX2 = np.zeros((N12 + 1, nx)) - # simU2 = np.zeros((N12, nu)) - - # ocp_solver.set_new_time_steps(new_time_steps2) - # print(80*'-') - # if ocp.solver_options.qp_solver.startswith('PARTIAL'): - # ocp_solver.update_qp_solver_cond_N(condN12) - # print(f'solve use-case 2 with N = {N12}, cond_N = {condN12} and Tf = {Tf_2} s (instead of {Tf_01} s):') - # else: - # print(f'solve use-case 2 with N = {N12} and Tf = {Tf_2} s (instead of {Tf_01} s):') - # status = ocp_solver.solve() - - # if status != 0: - # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # raise Exception(f'acados returned status {status}.') - - # # get solution - # for i in range(N12): - # simX2[i, :] = ocp_solver.get(i, "x") - # simU2[i, :] = ocp_solver.get(i, "u") - # simX2[N12, :] = ocp_solver.get(N12, "x") - - # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - - # if PLOT: - # plot_pendulum(time2, Fmax, simU2, simX2, latexify=False, plt_show=True, X_true_label=f'use-case 2: Tf={Tf_2} s') - # ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True) - # print(f"timing of last solver call {ocp_solver.get_stats('time_tot')}") + plot_pendulum(np.linspace(0, Tf, N_horizon + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N_horizon}, Tf={Tf}') if __name__ == "__main__": diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 4f022cfa2f..f899273eb1 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -354,6 +354,10 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_regularization_test COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests python regularization_test.py) + add_test(NAME python_one_sided_constraints_test + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests + python one_sided_constraints_test.py) + add_test(NAME python_pmsm_example COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pmsm_example @@ -381,7 +385,8 @@ add_test(NAME python_pendulum_ocp_example_cmake set_tests_properties(python_test_cython_vs_ctypes PROPERTIES DEPENDS python_test_reset) set_tests_properties(python_test_reset PROPERTIES DEPENDS python_test_ocp) set_tests_properties(python_test_ocp PROPERTIES DEPENDS python_test_cost_integration_euler) - set_tests_properties(python_test_cost_integration_euler PROPERTIES DEPENDS python_test_cost_integration_value) + set_tests_properties(python_test_cost_integration_euler PROPERTIES DEPENDS python_one_sided_constraints_test) + set_tests_properties(python_one_sided_constraints_test PROPERTIES DEPENDS python_test_cost_integration_value) set_tests_properties(python_test_cost_integration_value PROPERTIES DEPENDS python_armijo_test) set_tests_properties(python_armijo_test PROPERTIES DEPENDS python_pendulum_soft_constraints_example) set_tests_properties(python_pendulum_soft_constraints_example PROPERTIES DEPENDS python_pendulum_parametric_nonlinear_constraint_h_test) diff --git a/interfaces/acados_matlab_octave/get_acados_infty.m b/interfaces/acados_matlab_octave/get_acados_infty.m new file mode 100644 index 0000000000..f8e86364d1 --- /dev/null +++ b/interfaces/acados_matlab_octave/get_acados_infty.m @@ -0,0 +1,34 @@ +% +% Copyright (c) The acados authors. +% +% This file is part of acados. +% +% The 2-Clause BSD License +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% 1. Redistributions of source code must retain the above copyright notice, +% this list of conditions and the following disclaimer. +% +% 2. Redistributions in binary form must reproduce the above copyright notice, +% this list of conditions and the following disclaimer in the documentation +% and/or other materials provided with the distribution. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE.; + +% + +function infty = get_acados_infty() + infty = 1e10; +end diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index 390b33452b..19063d0293 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -47,7 +47,7 @@ from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \ get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \ casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts, \ - is_empty, get_simulink_default_opts + is_empty, get_simulink_default_opts, ACADOS_INFTY from .builders import ocp_get_default_cmake_builder, sim_get_default_cmake_builder diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index e9eb266b61..a3f872f3d0 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -68,6 +68,7 @@ "win32": "windows" } +ACADOS_INFTY = 1e10 def check_if_square(mat: np.ndarray, name: str): if mat.shape[0] != mat.shape[1]: From fdaa1a78724dd4ca71c65670c08e2e41c0f7c83b Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Thu, 8 Aug 2024 08:04:00 +0200 Subject: [PATCH 02/11] Fix `external_function_external_param_casadi` (#1189) fixes call to `external_function_external_param_casadi_calculate_size` - reported in https://github.com/acados/acados/issues/1186 - introduced in https://github.com/acados/acados/pull/1163 --- acados/utils/external_function_generic.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/acados/utils/external_function_generic.c b/acados/utils/external_function_generic.c index d2f0e653c2..e344419a79 100644 --- a/acados/utils/external_function_generic.c +++ b/acados/utils/external_function_generic.c @@ -1493,8 +1493,7 @@ void external_function_external_param_casadi_assign(external_function_external_p fun->param_mem_is_set = false; - assert((char *) raw_memory + external_function_external_param_casadi_calculate_size(fun, fun->np) >= - c_ptr); + assert((char *) raw_memory + external_function_external_param_casadi_calculate_size(fun) >= c_ptr); return; } From b1dd0acb3466ed6077953ad457a21a7438701d5a Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Fri, 9 Aug 2024 08:59:37 +0200 Subject: [PATCH 03/11] Minor documentation update (#1187) Minor docstring update for * custom update * regularization options --- .../acados_template/acados_ocp_options.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 0a2b5722aa..ad73ab55bc 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -140,11 +140,12 @@ def ext_fun_compile_flags(self): @property def custom_update_filename(self): """ - Filename of the custom C function to update solver data and parameters in between solver calls + Filename of the custom C function to update solver data and parameters in between solver calls. + Compare also `AcadosOcpOptions.custom_update_header_filename`. This file has to implement the functions int custom_update_init_function([model.name]_solver_capsule* capsule); - int custom_update_function([model.name]_solver_capsule* capsule); + int custom_update_function([model.name]_solver_capsule* capsule, double* data, int data_len); int custom_update_terminate_function([model.name]_solver_capsule* capsule); @@ -169,7 +170,7 @@ def custom_templates(self): @property def custom_update_header_filename(self): """ - Header filename of the custom C function to update solver data and parameters in between solver calls + Header filename of the custom C function to update solver data and parameters in between solver calls. This file has to declare the custom_update functions and look as follows: @@ -268,7 +269,7 @@ def regularize_method(self): - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii)) - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) - - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf + - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf, does not support nonlinear constraints - PROJECT_REDUC_HESS: experimental Note: default eps = 1e-4 From f68c05a1f138712652d1204e66e09c197203ecce Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Fri, 9 Aug 2024 11:40:53 +0200 Subject: [PATCH 04/11] Matlab: Getter for condensed QP matrices (#1188) This PR fixes the getter for `qp_solver_cond_H` for partially condensed OCPs. --------- Co-authored-by: Jonathan Frey --- .../getting_started/extensive_example_ocp.m | 4 ++ .../furuta_pendulum/integrator_experiment.py | 2 +- interfaces/acados_matlab_octave/acados_ocp.m | 56 +++++++++++++------ interfaces/acados_matlab_octave/ocp_get.c | 5 +- 4 files changed, 47 insertions(+), 20 deletions(-) diff --git a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m index 163148e745..7963388e16 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -314,6 +314,10 @@ % or for all stages qp_Q = ocp.get('qp_Q'); +% +cond_H = ocp.get('qp_solver_cond_H'); + + %% Plot trajectories figure; hold on; States = {'p', 'theta', 'v', 'dtheta'}; diff --git a/examples/acados_python/furuta_pendulum/integrator_experiment.py b/examples/acados_python/furuta_pendulum/integrator_experiment.py index e7c0082b3d..b1870e65df 100644 --- a/examples/acados_python/furuta_pendulum/integrator_experiment.py +++ b/examples/acados_python/furuta_pendulum/integrator_experiment.py @@ -208,7 +208,7 @@ def plot_results(settings: list[IntegratorSetting], results: list[dict]): marker=markers[j], lw=0, color="k", - label=f"{num_steps} steps", + label=f"{num_steps} steps" if num_steps > 1 else f"{num_steps} step", ) for j, num_steps in enumerate(num_steps_vals) ] + \ diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index 1e15317596..f2a04cc72f 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -265,16 +265,29 @@ function set(obj, field, value, varargin) S = obj.get('qp_S', n); value{n+1} = [R, S; S', Q]; - end return; end + elseif strcmp('pc_hess_block', field) + + if ~strncmp(obj.ocp.solver_options.qp_solver, 'PARTIAL_CONDENSING', length('PARTIAL_CONDENSING')) + error("Getting hessian block of partially condensed QP only works for PARTIAL_CONDENSING QP solvers"); + end + if length(varargin) > 0 + n = varargin{1}; + all_blocks = obj.get('qp_solver_cond_H'); + value = all_blocks{n+1}; + return; + else + value = obj.get('qp_solver_cond_H'); + return; + end else value = obj.t_ocp.get(field, varargin{:}); end % make symmetric (only lower triangular stored internally) - if strcmp('qp_Q', field) || strcmp('qp_R', field) + if strcmp('qp_Q', field) || strcmp('qp_R', field) || strcmp('qp_solver_cond_H', field) if iscell(value) for i=1:length(value) if length(value{i}) > 1 @@ -307,31 +320,40 @@ function reset(obj) obj.t_ocp.reset(); end - function result = qp_diagnostics(obj) + function result = qp_diagnostics(obj, varargin) % Compute some diagnostic values for the last QP. % min_ev: minimum eigenvalue for each Hessian block. % max_ev: maximum eigenvalue for each Hessian block. % condition_number: condition number for each Hessian block. - result = struct(); - result.min_ev = zeros(obj.ocp.dims.N+1, 1); - result.max_ev = zeros(obj.ocp.dims.N+1, 1); - result.condition_number = zeros(obj.ocp.dims.N+1, 1); + if length(varargin) > 0 + partially_condensed_qp = varargin{1}; + else + partially_condensed_qp = false; + end - for n=0:obj.ocp.dims.N - if n < obj.ocp.dims.N - Q = obj.get('qp_Q', n); - R = obj.get('qp_R', n); - S = obj.get('qp_S', n); - hess_block = [R, S; S', Q]; + if partially_condensed_qp + num_blocks = obj.ocp.solver_options.qp_solver_cond_N + 1; + else + num_blocks = obj.ocp.dims.N + 1; + end + result = struct(); + result.min_ev = zeros(num_blocks, 1); + result.max_ev = zeros(num_blocks, 1); + result.condition_number = zeros(num_blocks, 1); + + for n=1:num_blocks + if partially_condensed_qp + disp(['pc_hess_block', num2str(n-1)]); + hess_block = obj.get('pc_hess_block', n-1); else - hess_block = Q; + hess_block = obj.get('hess_block', n-1); end eigvals = eig(hess_block); - result.min_ev(n+1) = min(eigvals); - result.max_ev(n+1) = max(eigvals); - result.condition_number_blockwise(n+1) = max(eigvals) / min(eigvals); + result.min_ev(n) = min(eigvals); + result.max_ev(n) = max(eigvals); + result.condition_number_blockwise(n) = max(eigvals) / min(eigvals); end result.condition_number_global = max(result.max_ev) / min(result.min_ev); end diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index 8dbc4ff6b2..f67457b1b3 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -407,13 +407,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) ocp_qp_in *qp_in = qp_in_; int *nu = qp_in->dim->nu; int *nx = qp_in->dim->nx; + int cond_N = qp_in->dim->N; - mxArray *cell_array = mxCreateCellMatrix(N+1, 1); + mxArray *cell_array = mxCreateCellMatrix(cond_N+1, 1); plhs[0] = cell_array; mxArray *tmp_mat; - for (ii=0; ii<=N; ii++) + for (ii=0; ii<=cond_N; ii++) { tmp_mat = mxCreateNumericMatrix(nu[ii]+nx[ii], nu[ii]+nx[ii], mxDOUBLE_CLASS, mxREAL); double *mat_ptr = mxGetPr( tmp_mat ); From 5b4bc5614620c01b2ef57d9d70dd178162600075 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 9 Aug 2024 14:55:13 +0200 Subject: [PATCH 05/11] Compile acados with `BUILD_TYPE = DEBUG` on Github actions (#1191) - in order to check asserts on CI to avoid issues like https://github.com/acados/acados/issues/1186 --- .github/workflows/full_build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 5cb1dc1bb8..821485cc43 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -10,7 +10,7 @@ on: - '*' env: - BUILD_TYPE: Release + BUILD_TYPE: Debug ACADOS_PYTHON: ON ACADOS_OCTAVE: ON ACADOS_WITH_OSQP: ON From 89339d8dcb615c307fe7cb06510d522436e775cc Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 9 Aug 2024 18:23:10 +0200 Subject: [PATCH 06/11] Matlab example cleanup (#1192) - MATLAB examples: rename `ocp` to `ocp_solver` - MATLAB examples: rename `sim` to `sim_solver` - remove outdated Matlab example code related to non template based backend - MATLAB examples test: clear variables that might contain CasADi objects to avoid SWIG warnings --- .../acados_matlab_octave/control_rates/main.m | 26 +- .../generic_dyn_disc/disc_dyn_example_ocp.m | 77 +-- .../external_cost_example_ocp.m | 76 +-- .../getting_started/extensive_example_ocp.m | 85 +--- .../minimal_example_closed_loop.m | 32 +- .../getting_started/minimal_example_ocp.m | 28 +- .../getting_started/minimal_example_sim.m | 22 +- .../example_closed_loop.m | 28 +- .../linear_mass_spring_model/example_ocp.m | 28 +- .../linear_mass_spring_model/example_sim.m | 18 +- .../linear_mpc/example_closed_loop.m | 20 +- .../lorentz/example_mhe.m | 87 +--- .../lorentz/lorentz_model.m | 2 +- .../lorentz/setup_integrator.m | 6 +- .../masses_chain_model/example_closed_loop.m | 62 +-- .../masses_chain_model/example_ocp.m | 36 +- .../pendulum_dae/example_closed_loop.m | 176 +++---- .../pendulum_dae/example_sim.m | 46 +- .../example_closed_loop.m | 83 ++-- .../pendulum_on_cart_model/example_ocp.m | 34 +- .../example_ocp_custom_hess.m | 35 +- .../example_ocp_param_sens.m | 46 +- .../pendulum_on_cart_model/example_ocp_reg.m | 48 +- .../pendulum_on_cart_model/example_sim.m | 30 +- .../example_solution_sens_closed_loop.m | 24 +- .../experiment_dae_formulation.m | 28 +- .../acados_matlab_octave/race_cars/main.m | 41 +- .../simple_dae_model/example_ocp.m | 22 +- .../swarming/example_closed_loop.m | 42 +- .../swarming/example_ocp.m | 32 +- .../swarming/example_sim.m | 24 +- .../test/run_matlab_tests.m | 1 - .../test/run_test_ocp_pendulum_dae.m | 57 --- .../test/run_tests_templates.m | 63 --- .../test/simulink_init_test.m | 22 +- .../test/simulink_qp_test.m | 20 +- .../test/test_all_examples.m | 8 +- .../acados_matlab_octave/test/test_checks.m | 6 +- .../test/test_mhe_lorentz.m | 6 +- .../acados_matlab_octave/test/test_ocp_OSQP.m | 24 +- .../test/test_ocp_linear_mass_spring.m | 34 +- .../test/test_ocp_pendulum_dae.m | 452 ------------------ .../test/test_ocp_pendulum_on_cart.m | 44 +- .../test/test_ocp_qpdunes.m | 24 +- .../test/test_ocp_simple_dae.m | 24 +- .../test/test_ocp_wtnx6.m | 76 +-- .../acados_matlab_octave/test/test_sens_adj.m | 20 +- .../test/test_sens_forw.m | 36 +- .../test/test_sens_hess.m | 30 +- .../acados_matlab_octave/test/test_sim_dae.m | 32 +- .../test/test_target_selector.m | 18 +- .../wind_turbine_nx6/example_closed_loop.m | 66 +-- .../wind_turbine_nx6/example_ocp.m | 38 +- .../wind_turbine_nx6/example_sim.m | 18 +- .../wind_turbine_nx6/example_simulink.m | 5 - interfaces/CMakeLists.txt | 8 +- 56 files changed, 848 insertions(+), 1628 deletions(-) delete mode 100644 examples/acados_matlab_octave/test/run_test_ocp_pendulum_dae.m delete mode 100644 examples/acados_matlab_octave/test/run_tests_templates.m delete mode 100644 examples/acados_matlab_octave/test/test_ocp_pendulum_dae.m diff --git a/examples/acados_matlab_octave/control_rates/main.m b/examples/acados_matlab_octave/control_rates/main.m index 0628df074b..463bb52f41 100644 --- a/examples/acados_matlab_octave/control_rates/main.m +++ b/examples/acados_matlab_octave/control_rates/main.m @@ -208,7 +208,7 @@ disp(ocp_opts.opts_struct); %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% Simulation Duration = 10; % [s] @@ -223,9 +223,9 @@ cost_sim(1, 1) = 0; % set trajectory initialization (also can use plant: create acados integrator) -ocp.set('init_x', x0 * ones(1,param_scheme_N+1)); -% ocp.set('init_x', zeros(nx,param_scheme_N+1)); -ocp.set('init_u', zeros(nu, param_scheme_N)); +ocp_solver.set('init_x', x0 * ones(1,param_scheme_N+1)); +% ocp_solver.set('init_x', zeros(nx,param_scheme_N+1)); +ocp_solver.set('init_u', zeros(nu, param_scheme_N)); % time-varying reference trajectory x1ref_FUN = @(t) 0.4.*(-(0.5./(1+exp(t./0.1-0.8))) + (1./(1+exp(t./0.1-30))) - 0.4); @@ -242,28 +242,28 @@ x1_ref = x1ref_FUN(t_ref); for j = 0:param_scheme_N-1 y_ref(1) = x1_ref(j+1); - ocp.set('cost_y_ref', y_ref, j); + ocp_solver.set('cost_y_ref', y_ref, j); end y_ref_e(1) = x1_ref(param_scheme_N+1); - ocp.set('cost_y_ref_e', y_ref_e, param_scheme_N); + ocp_solver.set('cost_y_ref_e', y_ref_e, param_scheme_N); % solve ocp - ocp.solve(); - status = ocp.get('status'); % 0 - success + ocp_solver.solve(); + status = ocp_solver.get('status'); % 0 - success if status ~= 0 error('acados returned status %d in closed loop iteration %d. Exiting.', status, i); end % get solution t0 - x0 = ocp.get('x', 0); - u0 = ocp.get('u', 0); + x0 = ocp_solver.get('x', 0); + u0 = ocp_solver.get('u', 0); x_sim(:, i+1) = x0; u_sim(:, i+1) = u0; - cost_sim(1, i+1) = ocp.get_cost(); + cost_sim(1, i+1) = ocp_solver.get_cost(); % update initial state - x0 = ocp.get('x', 1); - ocp.set('constr_x0', x0); + x0 = ocp_solver.get('x', 1); + ocp_solver.set('constr_x0', x0); end tElapsed = toc 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 15d8a5b707..1fcac66a9f 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 @@ -174,39 +174,39 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % initial state -ocp.set('constr_x0', x0); +ocp_solver.set('constr_x0', x0); % set trajectory initialization x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % solve tic; -ocp.solve(); +ocp_solver.solve(); time_ext = toc; % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); % get info -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); % print statistics -ocp.print('stat') +ocp_solver.print('stat') if status~=0 error('ocp_nlp solver returned status nonzero'); @@ -227,52 +227,3 @@ % plot(1:N, utraj); % ylabel('u') % xlabel('sample') - -%% test templated ocp solver -disp('testing templated solver'); -ocp.generate_c_code; -cd c_generated_code/ - -t_ocp = lin_mass_mex_solver; - -% initial state -t_ocp.set('constr_x0', x0); -% t_ocp.set('print_level', print_level) - -% set trajectory initialization -t_ocp.set('init_x', x_traj_init); -t_ocp.set('init_u', u_traj_init); - -t_ocp.solve(); -xt_traj = t_ocp.get('x'); -ut_traj = t_ocp.get('u'); -status = t_ocp.get('status'); - -if status~=0 - error('test_template_pendulum_ocp: solution of templated MEX failed!'); -else - fprintf('\ntest_template_pendulum_ocp: templated MEX success!\n'); -end - -error_X_mex_vs_mex_template = max(max(abs(xt_traj - xtraj))) -error_U_mex_vs_mex_template = max(max(abs(ut_traj - utraj))) - -t_ocp.print('stat') - - -tol_check = 1e-6; - -if any([error_X_mex_vs_mex_template, error_U_mex_vs_mex_template] > tol_check) - error(['test_template_pendulum_exact_hess: solution of templated MEX and original MEX',... - ' differ too much. Should be < tol = ' num2str(tol_check)]); -end - -cost_native_mex = ocp.get_cost() -cost_template_mex = t_ocp.get_cost() - -if any(abs(cost_native_mex - cost_template_mex) > tol_check) - error(['test_template_pendulum_exact_hess: cost value of templated MEX and original MEX',... - ' differ too much. Should be < tol = ' num2str(tol_check)]); -end - -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 792459ee09..a7e8000478 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 @@ -83,7 +83,7 @@ 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_ext_fun_type', 'generic'); ocp_model.set('cost_source_ext_cost', 'generic_ext_cost.c'); ocp_model.set('cost_function_ext_cost', 'ext_cost'); % Generic terminal cost @@ -105,12 +105,12 @@ 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_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.cost_expr_ext_cost_e); + ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e); end % dynamics @@ -133,7 +133,6 @@ 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 %% acados ocp set opts ocp_opts = acados_ocp_opts(); @@ -143,10 +142,9 @@ 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 -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); @@ -155,27 +153,27 @@ %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.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_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); +ocp_solver.set('init_pi', zeros(nx, N)); % change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -ocp.set('constr_lbx', x0, 0); +% ocp_solver.set('field', value, optional: stage_index) +ocp_solver.set('constr_lbx', x0, 0); -ocp.set('p', p); +ocp_solver.set('p', p); % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') %% Plots figure; hold on; @@ -189,45 +187,3 @@ figure stairs([utraj'; utraj(end)]) grid on - -%% go embedded -disp('testing templated solver'); -ocp.generate_c_code; -cd c_generated_code/ - -t_ocp = pendulum_mex_solver; - -% initial state -t_ocp.set('constr_x0', x0); - -% set trajectory initialization -t_ocp.set('init_x', x_traj_init); -t_ocp.set('init_u', u_traj_init); -t_ocp.set('init_pi', zeros(nx, N)); - -% change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -t_ocp.set('constr_lbx', x0, 0); - -t_ocp.set('p', p); - -% solve -t_ocp.solve(); -% get solution -t_utraj = t_ocp.get('u'); -t_xtraj = t_ocp.get('x'); -t_status = t_ocp.get('status'); - -error_X_mex_vs_mex_template = max(max(abs(t_xtraj - xtraj))) -error_U_mex_vs_mex_template = max(max(abs(t_utraj - utraj))) - -t_ocp.print('stat') - -tol_check = 1e-6; - -if any([error_X_mex_vs_mex_template, error_U_mex_vs_mex_template] > tol_check) - error(['test_template_pendulum_exact_hess: solution of templated MEX and original MEX',... - ' differ too much. Should be < tol = ' num2str(tol_check)]); -end - -cd .. \ No newline at end of file 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 7963388e16..b79b0c8763 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -231,7 +231,7 @@ ocp_opts.set('exact_hess_constr', 1); %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % state and input initial guess x_traj_init = zeros(nx, N+1); @@ -246,48 +246,48 @@ time_reg = zeros(n_executions,1); time_qp_sol = zeros(n_executions,1); -ocp.set('print_level', print_level) +ocp_solver.set('print_level', print_level) %% call ocp solver in loop for i=1:n_executions % initial state - ocp.set('constr_x0', x0); + ocp_solver.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)); % multipliers for dynamics equality constraints + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints % solve - ocp.solve(); + ocp_solver.solve(); % get solution - utraj = ocp.get('u'); - xtraj = ocp.get('x'); + utraj = ocp_solver.get('u'); + xtraj = ocp_solver.get('x'); % evaluation - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot(i) = ocp.get('time_tot'); - time_lin(i) = ocp.get('time_lin'); - time_reg(i) = ocp.get('time_reg'); - time_qp_sol(i) = ocp.get('time_qp_sol'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot(i) = ocp_solver.get('time_tot'); + time_lin(i) = ocp_solver.get('time_lin'); + time_reg(i) = ocp_solver.get('time_reg'); + time_qp_sol(i) = ocp_solver.get('time_qp_sol'); if i == 1 || i == n_executions - ocp.print('stat') + ocp_solver.print('stat') end end % get slack values for i = 0:N-1 - sl = ocp.get('sl', i); - su = ocp.get('su', i); + sl = ocp_solver.get('sl', i); + su = ocp_solver.get('su', i); end -sl = ocp.get('sl', N); -su = ocp.get('su', N); +sl = ocp_solver.get('sl', N); +su = ocp_solver.get('su', N); % get cost value -cost_val_ocp = ocp.get_cost(); +cost_val_ocp = ocp_solver.get_cost(); %% get QP matrices: @@ -299,23 +299,23 @@ for k = 1:length(fields) field = fields{k}; disp(strcat(field, " at stage ", num2str(stage), " = ")); - ocp.get(field, stage) + ocp_solver.get(field, stage) end end stage = N; field = 'qp_Q'; disp(strcat(field, " at stage ", num2str(stage), " = ")); -ocp.get(field, stage) +ocp_solver.get(field, stage) field = 'qp_R'; disp(strcat(field, " at stage ", num2str(stage), " = ")); -ocp.get(field, stage) +ocp_solver.get(field, stage) % or for all stages -qp_Q = ocp.get('qp_Q'); +qp_Q = ocp_solver.get('qp_Q'); % -cond_H = ocp.get('qp_solver_cond_H'); +cond_H = ocp_solver.get('qp_solver_cond_H'); %% Plot trajectories @@ -355,36 +355,3 @@ % title( [ strrep(cost_type, '_',' '), ' , sim: ' strrep(sim_method, '_',' '), ... % '; ', strrep(qp_solver, '_', ' ')] ) % end - -%% test templated solver -% if ~ispc() -% % MEX wrapper around templated solver not supported for Windows yet -% % it can be used and tested via Simulink though. -% disp('testing templated solver'); -% ocp.generate_c_code; -% cd c_generated_code/ -% command = strcat('t_ocp = ', model_name, '_mex_solver'); -% eval( command ); -% -% t_ocp.set('print_level', print_level) -% -% % initial state -% t_ocp.set('constr_x0', x0); -% -% % set trajectory initialization -% t_ocp.set('init_x', x_traj_init); -% t_ocp.set('init_u', u_traj_init); -% t_ocp.set('init_pi', zeros(nx, N)) -% -% t_ocp.solve() -% xt_traj = t_ocp.get('x'); -% ut_traj = t_ocp.get('u'); -% -% error_X_mex_vs_mex_template = max(max(abs(xt_traj - xtraj))) -% error_U_mex_vs_mex_template = max(max(abs(ut_traj - utraj))) -% -% t_ocp.print('stat') -% cost_val_t_ocp = t_ocp.get_cost(); -% clear t_ocp -% cd .. -% end 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 25c559d3e0..75e784b6cc 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 @@ -131,11 +131,11 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % set parameter for all stages for i = 0:N - ocp.set('p', 1.); + ocp_solver.set('p', 1.); end %% plant: create acados integrator @@ -157,10 +157,10 @@ sim_opts.set('num_stages', plant_sim_method_num_stages); sim_opts.set('num_steps', plant_sim_method_num_steps); -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % set parameter -sim.set('p', 1.05); +sim_solver.set('p', 1.05); %% simulation N_sim = 150; @@ -185,7 +185,7 @@ for i=1:N_sim % update initial state x0 = x_sim(:,i); - ocp.set('constr_x0', x0); + ocp_solver.set('constr_x0', x0); % compute reference position on the nonuniform grid t = (i-1)*h; @@ -193,30 +193,30 @@ for k=1:N-1 % intermediate stages yref(1) = p_ref(k); - ocp.set('cost_y_ref', yref, k); % last argument is the stage + ocp_solver.set('cost_y_ref', yref, k); % last argument is the stage end yref_e(1) = p_ref(k+1); % terminal stage - ocp.set('cost_y_ref_e', yref_e, N); + ocp_solver.set('cost_y_ref_e', yref_e, N); % solve - ocp.solve(); + ocp_solver.solve(); % get solution - u0 = ocp.get('u', 0); - status = ocp.get('status'); % 0 - success + u0 = ocp_solver.get('u', 0); + status = ocp_solver.get('status'); % 0 - success % set initial state for the simulation - sim.set('x', x0); - sim.set('u', u0); + sim_solver.set('x', x0); + sim_solver.set('u', u0); % simulate one step - sim_status = sim.solve(); + sim_status = sim_solver.solve(); if sim_status ~= 0 disp(['acados integrator returned error status ', num2str(sim_status)]) end % get simulated state - x_sim(:,i+1) = sim.get('xn'); + x_sim(:,i+1) = sim_solver.get('xn'); u_sim(:,i) = u0; end @@ -234,8 +234,8 @@ for i=1:length(States) subplot(length(States), 1, i); grid on; hold on; - plot(ts, x_sim(i,:)); - plot(ts, y_ref(i, :)); + plot(ts, x_sim(i,:)); + plot(ts, y_ref(i, :)); ylabel(States{i}); xlabel('t [s]') legend('closed-loop', 'reference') 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 099cb516de..b38256f0b7 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m @@ -112,7 +112,7 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts, simulink_opts); % solver initial guess x_traj_init = zeros(nx, N+1); @@ -120,25 +120,25 @@ %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.set('constr_x0', x0); % set trajectory initialization -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 +ocp_solver.set('init_x', x_traj_init); % states +ocp_solver.set('init_u', u_traj_init); % inputs +ocp_solver.set('init_pi', zeros(nx, N)); % multipliers for dynamics equality constraints % change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -ocp.set('constr_lbx', x0, 0) +% ocp_solver.set('field', value, optional: stage_index) +ocp_solver.set('constr_lbx', x0, 0) % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') %% plots ts = linspace(0, T, N+1); @@ -156,7 +156,3 @@ ylabel('F [N]') xlabel('t [s]') grid on - -%% go embedded -% to generate templated C code -% ocp.generate_c_code; 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 c12024fce0..5117a3f781 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_sim.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_sim.m @@ -88,7 +88,7 @@ %% create integrator -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% simulate system in loop @@ -98,26 +98,26 @@ for ii=1:N_sim % set initial state - sim.set('x', x_sim(:,ii)); - sim.set('u', u0); + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u0); % initialize implicit integrator if (strcmp(method, 'irk')) - sim.set('xdot', zeros(nx,1)); + sim_solver.set('xdot', zeros(nx,1)); elseif (strcmp(method, 'irk_gnsf')) - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end % forward sensitivities ( dxn_d[x0,u] ) -S_forw = sim.get('S_forw'); +S_forw = sim_solver.get('S_forw'); %% plots % for ii=1:N_sim+1 @@ -130,5 +130,5 @@ legend('p', 'theta', 'v', 'omega'); %% dump gnsf -% sim.model_struct.name = 'pendulum_ode'; -% dump_gnsf_functions(sim.model_struct) +% sim_solver.model_struct.name = 'pendulum_ode'; +% dump_gnsf_functions(sim_solver.model_struct) 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 3301fd5ed5..fabce877d2 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 @@ -170,7 +170,7 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% acados sim model sim_model = acados_sim_model(); @@ -207,7 +207,7 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation n_sim = 100; @@ -223,30 +223,30 @@ for ii=1:n_sim % set x0 - ocp.set('constr_x0', x_sim(:,ii)); + ocp_solver.set('constr_x0', x_sim(:,ii)); % set trajectory initialization - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); % solve OCP - ocp.solve(); + ocp_solver.solve(); % get solution - %x_traj = ocp.get('x'); - %u_traj = ocp.get('u'); - u_sim(:,ii) = ocp.get('u', 0); + %x_traj = ocp_solver.get('x'); + %u_traj = ocp_solver.get('u'); + u_sim(:,ii) = ocp_solver.get('u', 0); % set initial state of sim - sim.set('x', x_sim(:,ii)); + sim_solver.set('x', x_sim(:,ii)); % set input in sim - sim.set('u', u_sim(:,ii)); + sim_solver.set('u', u_sim(:,ii)); % simulate state - sim.solve(); + sim_solver.solve(); % get new state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end avg_time_solve = toc/n_sim @@ -264,7 +264,7 @@ xlabel('sample') -status = ocp.get('status'); +status = ocp_solver.get('status'); if status==0 fprintf('\nsuccess!\n\n'); 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 6fd0cf4a21..8617e0e49a 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 @@ -245,40 +245,40 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % set trajectory initialization x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % solve tic; -ocp.solve(); +ocp_solver.solve(); time_ext = toc %x0(1) = 1.5; -%ocp.set('constr_x0', x0); +%ocp_solver.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'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); % get info -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); % print statistics -ocp.print('stat') +ocp_solver.print('stat') if status==0 fprintf('\nsuccess!\n\n'); 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 9e56c0a16c..98f9905f38 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 @@ -91,36 +91,36 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % (re)set numerical part of model -%sim.set('T', 0.5); +%sim_solver.set('T', 0.5); x0 = ones(nx, 1); %x0(1) = 2.0; tic; -sim.set('x', x0); +sim_solver.set('x', x0); time_set_x = toc u = ones(nu, 1); -sim.set('u', u); +sim_solver.set('u', u); % solve tic; -sim.solve(); +sim_solver.solve(); time_solve = toc % xn -xn = sim.get('xn'); +xn = sim_solver.get('xn'); xn % S_forw -S_forw = sim.get('S_forw'); +S_forw = sim_solver.get('S_forw'); S_forw % Sx -Sx = sim.get('Sx'); +Sx = sim_solver.get('Sx'); Sx % Su -Su = sim.get('Su'); +Su = sim_solver.get('Su'); Su 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 ba680be1dd..ac9ade7df8 100644 --- a/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m +++ b/examples/acados_matlab_octave/linear_mpc/example_closed_loop.m @@ -96,7 +96,7 @@ ocp_opts.set('qp_solver','partial_condensing_hpipm'); ocp_opts.set('parameter_values', zeros(model.nx,1)); % initialize to zero, change later -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% simulate the closed-loop system x0 = zeros(nx,1); % initial state @@ -109,32 +109,32 @@ for i = 1 : nsim % set the current state - ocp.set('constr_x0', x0); + ocp_solver.set('constr_x0', x0); % set the reference - if strcmp(ocp.model_struct.cost_type,'linear_ls') - ocp.set('cost_y_ref', [zeros(nu,1); xr]); % for the stage cost (y=[u;x]) - ocp.set('cost_y_ref_e', xr); % for the terminal cost (y=x) + if strcmp(ocp_solver.model_struct.cost_type,'linear_ls') + ocp_solver.set('cost_y_ref', [zeros(nu,1); xr]); % for the stage cost (y=[u;x]) + ocp_solver.set('cost_y_ref_e', xr); % for the terminal cost (y=x) else - ocp.set('p', xr); % set as the parameter + ocp_solver.set('p', xr); % set as the parameter end % solve the ocp - ocp.solve(); + ocp_solver.solve(); % check the solver output - if ocp.get('status') ~= 0 + if ocp_solver.get('status') ~= 0 warning(['acados ocp solver failed with status ',num2str(status)]); end % apply the first control input to the plant, simulate one step - ctrl = ocp.get('u', 0); + ctrl = ocp_solver.get('u', 0); x0 = model.Ad*x0 + model.Bd*ctrl; % log the data X(:,i+1) = x0; U(:,i) = ctrl; - solve_time_log(i) = ocp.get('time_tot'); + solve_time_log(i) = ocp_solver.get('time_tot'); end disp([newline,'Average solve time: ',num2str(1e3*mean(solve_time_log)),' ms']) diff --git a/examples/acados_matlab_octave/lorentz/example_mhe.m b/examples/acados_matlab_octave/lorentz/example_mhe.m index 6a7e508065..d2cae1da37 100644 --- a/examples/acados_matlab_octave/lorentz/example_mhe.m +++ b/examples/acados_matlab_octave/lorentz/example_mhe.m @@ -39,14 +39,14 @@ nu = model.nu; ny = model.ny; -sim = setup_integrator(model); +sim_solver = setup_integrator(model); estimator = setup_estimator(model); %% Simulation N_sim = 120; -iter_step = 50; +iter_step = 50; step = 3; x0 = [-1 3 4 25]; @@ -60,21 +60,21 @@ x_sim(:,1) = x0; for n=1:N_sim - - % set initial state - sim.set('x', x_sim(:,n)); - % solve - sim.solve(); + % set initial state + sim_solver.set('x', x_sim(:,n)); + + % solve + sim_solver.solve(); + + % get simulated state + x_sim(:,n+1) = sim_solver.get('xn'); - % get simulated state - x_sim(:,n+1) = sim.get('xn'); - % unmodeled step change in x(4) if n == iter_step x_sim(end, n+1) = x_sim(end, n+1) + step; end - + % measurement y_sim(:, n) = x_sim(1, n) + v_std*randn(1, 1); end @@ -87,25 +87,25 @@ yref = zeros(ny + nu, 1); for n=1:N_sim-model.N - + % set measurements yref_0(1:ny) = y_sim(:, n); yref_0(ny+nu+1:end) = x0_bar; - + estimator.set('cost_y_ref', yref_0, 0); - + for i=1:model.N-1 yref(1:ny) = y_sim(:, n+i); estimator.set('cost_y_ref', yref, i); end - + %estimator.set('init_x', x_sim(:, n:n+model.N)) - - % solve + + % solve estimator.solve() x_est(:, n) = estimator.get('x', model.N); - + % update arrival cost (TODO: update P0 as well) x0_bar = estimator.get('x', 1); end @@ -113,13 +113,13 @@ %% Plot ts = model.h*(0:N_sim); -figure; +figure; States = {'x_1', 'x_2', 'x_3', 'p'}; for i=1:length(States) subplot(length(States), 1, i); hold on; - plot(ts, x_sim(i,:)); - plot(ts(model.N+1:end-1), x_est(i,:)); - + plot(ts, x_sim(i,:)); + plot(ts(model.N+1:end-1), x_est(i,:)); + if i == 1 plot(ts(1:end-1), y_sim, 'x'); legend('true', 'est', 'measured'); @@ -129,48 +129,13 @@ xlabel('t [s]'); end -figure; +figure; States = {'abs. error x_1', 'abs. error x_2', 'abs. error x_3', 'abs. error p'}; for i=1:length(States) subplot(length(States), 1, i); hold on; grid on; - - plot(ts(model.N+1:end-1), abs(x_est(i,:) - x_sim(i, model.N+1:end-1))); - - ylabel(States{i}); - xlabel('t [s]'); -end - -%% test templated solver -disp('testing templated solver'); -estimator.generate_c_code; -cd c_generated_code/ -command = strcat('t_ocp = ', estimator.model_struct.name , '_mex_solver'); -eval(command); -% set measurements -yref_0(1:ny) = y_sim(:, 1); -yref_0(ny+nu+1:end) = x0; + plot(ts(model.N+1:end-1), abs(x_est(i,:) - x_sim(i, model.N+1:end-1))); -estimator.set('cost_y_ref', yref_0, 0); -t_ocp.set('cost_y_ref', yref_0, 0); - -for i=1:model.N-1 - yref(1:ny) = y_sim(:, i+1); - estimator.set('cost_y_ref', yref, i); - t_ocp.set('cost_y_ref', yref, i); + ylabel(States{i}); + xlabel('t [s]'); end - -t_ocp.solve() -xt_traj = t_ocp.get('x'); - -estimator.solve() -x_traj = estimator.get('x'); - -max_diff = max(max(abs(xt_traj - x_traj))); -disp(['difference ' num2str(max_diff)]); - -t_ocp.print('stat') -cost_val_t_ocp = t_ocp.get_cost(); -clear t_ocp -cd .. - diff --git a/examples/acados_matlab_octave/lorentz/lorentz_model.m b/examples/acados_matlab_octave/lorentz/lorentz_model.m index 8b860e080f..038ccbb491 100644 --- a/examples/acados_matlab_octave/lorentz/lorentz_model.m +++ b/examples/acados_matlab_octave/lorentz/lorentz_model.m @@ -53,7 +53,7 @@ x_expr(4)*x_expr(1)-x_expr(2)-x_expr(1)*x_expr(3); ... -(8/3)*x_expr(3)+x_expr(1)*x_expr(2); ... w_expr]; - + % store eveything in model struct model = struct(); model.nx = nx; diff --git a/examples/acados_matlab_octave/lorentz/setup_integrator.m b/examples/acados_matlab_octave/lorentz/setup_integrator.m index 4fb1325f66..1e91ff7ce8 100644 --- a/examples/acados_matlab_octave/lorentz/setup_integrator.m +++ b/examples/acados_matlab_octave/lorentz/setup_integrator.m @@ -28,7 +28,7 @@ % POSSIBILITY OF SUCH DAMAGE.; -function [sim] = setup_integrator(model) +function [sim_solver] = setup_integrator(model) model_name = 'lorentz_model_integrator'; sim_model = acados_sim_model(); @@ -38,7 +38,7 @@ sim_model.set('sym_x', model.sym_x); sim_model.set('sym_u', model.sym_u); -% explit integrator (erk) +% explit integrator (erk) sim_model.set('dyn_type', 'explicit'); sim_model.set('dyn_expr_f', model.f_expl_expr); @@ -51,6 +51,6 @@ sim_opts.set('sens_forw', 'true'); % generate forward sensitivities % create acados integrator -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); end diff --git a/examples/acados_matlab_octave/masses_chain_model/example_closed_loop.m b/examples/acados_matlab_octave/masses_chain_model/example_closed_loop.m index 94fb4702f8..280db30308 100644 --- a/examples/acados_matlab_octave/masses_chain_model/example_closed_loop.m +++ b/examples/acados_matlab_octave/masses_chain_model/example_closed_loop.m @@ -207,7 +207,7 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% acados sim model @@ -243,7 +243,7 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation @@ -256,48 +256,48 @@ u_traj_init = zeros(nu, ocp_N); pi_traj_init = zeros(nx, ocp_N); -%ocp.set('init_x', x_traj_init); -%ocp.set('init_u', u_traj_init); -%ocp.set('init_pi', pi_traj_init); +%ocp_solver.set('init_x', x_traj_init); +%ocp_solver.set('init_u', u_traj_init); +%ocp_solver.set('init_pi', pi_traj_init); tic; for ii=1:n_sim % set x0 - ocp.set('constr_x0', x_sim(:,ii)); + ocp_solver.set('constr_x0', x_sim(:,ii)); % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', pi_traj_init); % solve OCP - ocp.set('rti_phase', 1); - ocp.solve(); - ocp.set('rti_phase', 2); - ocp.solve(); + ocp_solver.set('rti_phase', 1); + ocp_solver.solve(); + ocp_solver.set('rti_phase', 2); + ocp_solver.solve(); if 1 - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot = ocp.get('time_tot'); - time_lin = ocp.get('time_lin'); - time_reg = ocp.get('time_reg'); - time_qp_sol = ocp.get('time_qp_sol'); - time_qp_solver_call = ocp.get('time_qp_solver_call'); - qp_iter = ocp.get('qp_iter'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot = ocp_solver.get('time_tot'); + time_lin = ocp_solver.get('time_lin'); + time_reg = ocp_solver.get('time_reg'); + time_qp_sol = ocp_solver.get('time_qp_sol'); + time_qp_solver_call = ocp_solver.get('time_qp_solver_call'); + qp_iter = ocp_solver.get('qp_iter'); fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms] (time_qp_solver_call = %f [ms]), time_reg = %f [ms])\n', status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_qp_solver_call*1e3, time_reg*1e3); % fprintf('%e %d\n', time_qp_solver_call, qp_iter); -% ocp.print('stat'); +% ocp_solver.print('stat'); end % get solution - x_traj = ocp.get('x'); - u_traj = ocp.get('u'); - pi_traj = ocp.get('pi'); + x_traj = ocp_solver.get('x'); + u_traj = ocp_solver.get('u'); + pi_traj = ocp_solver.get('pi'); % shift trajectory for initialization x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; @@ -305,7 +305,7 @@ pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; % get solution for sim - u_sim(:,ii) = ocp.get('u', 0); + u_sim(:,ii) = ocp_solver.get('u', 0); % overwrite control to perturb the system % if(ii<=5) @@ -313,15 +313,15 @@ % end % set initial state of sim - sim.set('x', x_sim(:,ii)); + sim_solver.set('x', x_sim(:,ii)); % set input in sim - sim.set('u', u_sim(:,ii)); + sim_solver.set('u', u_sim(:,ii)); % simulate state - sim.solve(); + sim_solver.solve(); % get new state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end @@ -339,7 +339,7 @@ -status = ocp.get('status'); +status = ocp_solver.get('status'); if status==0 fprintf('\nsuccess!\n\n'); diff --git a/examples/acados_matlab_octave/masses_chain_model/example_ocp.m b/examples/acados_matlab_octave/masses_chain_model/example_ocp.m index b0a87d2104..3dff5f4f57 100644 --- a/examples/acados_matlab_octave/masses_chain_model/example_ocp.m +++ b/examples/acados_matlab_octave/masses_chain_model/example_ocp.m @@ -222,45 +222,45 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % set trajectory initialization x_traj_init = repmat(model.x_ref, 1, N+1); u_traj_init = zeros(nu, N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % set parameter -ocp.set('p', T/N); +ocp_solver.set('p', T/N); % solve nrep = 1; tic; for rep=1:nrep - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.solve(); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.solve(); end time_ext = toc/nrep % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); % statistics -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); -time_qp_solver_call = ocp.get('time_qp_solver_call'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); +time_qp_solver_call = ocp_solver.get('time_qp_solver_call'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms] (time_qp_solver_call = %f [ms]), time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_qp_solver_call*1e3, time_reg*1e3); -ocp.print('stat'); +ocp_solver.print('stat'); %% figures @@ -280,7 +280,7 @@ visualize; end -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); if (strcmp(nlp_solver, 'sqp')) figure(); plot(0: size(stat,1)-1, log10(stat(:,2)), 'r-x'); 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 39d2b55761..2fcdfee3df 100644 --- a/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m +++ b/examples/acados_matlab_octave/pendulum_dae/example_closed_loop.m @@ -35,7 +35,7 @@ % 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'); + error('env.sh has not been sourced! Before executing this example, run: source env.sh'); end %% options @@ -135,9 +135,9 @@ lh = 0; uh = 200; ocp_model.set('constr_lh_0', lh); - ocp_model.set('constr_uh_0', uh); + ocp_model.set('constr_uh_0', uh); ocp_model.set('constr_lh', lh); - ocp_model.set('constr_uh', uh); + ocp_model.set('constr_uh', uh); else nh = 0; end @@ -145,57 +145,57 @@ % symbolics ocp_model.set('sym_x', model.sym_x); if isfield(model, 'sym_u') - ocp_model.set('sym_u', model.sym_u); + ocp_model.set('sym_u', model.sym_u); end if isfield(model, 'sym_xdot') - ocp_model.set('sym_xdot', model.sym_xdot); + ocp_model.set('sym_xdot', model.sym_xdot); end if isfield(model, 'sym_z') - ocp_model.set('sym_z', model.sym_z); + ocp_model.set('sym_z', model.sym_z); end % cost ocp_model.set('cost_type', cost_type); ocp_model.set('cost_type_e', cost_type); if (strcmp(cost_type, 'linear_ls')) - ocp_model.set('cost_Vu', Vu); - ocp_model.set('cost_Vx', Vx); - ocp_model.set('cost_Vx_e', Vx_e); - ocp_model.set('cost_W', W); - ocp_model.set('cost_W_e', W_e); - ocp_model.set('cost_y_ref', yr); - ocp_model.set('cost_y_ref_e', yr_e); + ocp_model.set('cost_Vu', Vu); + ocp_model.set('cost_Vx', Vx); + ocp_model.set('cost_Vx_e', Vx_e); + ocp_model.set('cost_W', W); + ocp_model.set('cost_W_e', W_e); + ocp_model.set('cost_y_ref', yr); + ocp_model.set('cost_y_ref_e', yr_e); 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); + ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); + ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); end % dynamics if (strcmp(ocp_sim_method, 'erk')) - ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); + ocp_model.set('dyn_type', 'explicit'); + ocp_model.set('dyn_expr_f', model.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_type', 'implicit'); + ocp_model.set('dyn_expr_f', model.expr_f_impl); end % constraints ocp_model.set('constr_x0', x0); if (ng>0) - ocp_model.set('constr_C', C); - ocp_model.set('constr_D', D); - ocp_model.set('constr_lg', lg); - ocp_model.set('constr_ug', ug); - ocp_model.set('constr_C_e', C_e); - ocp_model.set('constr_lg_e', lg_e); - ocp_model.set('constr_ug_e', ug_e); + ocp_model.set('constr_C', C); + ocp_model.set('constr_D', D); + ocp_model.set('constr_lg', lg); + ocp_model.set('constr_ug', ug); + ocp_model.set('constr_C_e', C_e); + ocp_model.set('constr_lg_e', lg_e); + ocp_model.set('constr_ug_e', ug_e); else -% ocp_model.set('constr_Jbx', Jbx); -% ocp_model.set('constr_lbx', lbx); -% ocp_model.set('constr_ubx', ubx); - ocp_model.set('constr_Jbu', Jbu); - ocp_model.set('constr_lbu', lbu); - ocp_model.set('constr_ubu', ubu); +% ocp_model.set('constr_Jbx', Jbx); +% ocp_model.set('constr_lbx', lbx); +% ocp_model.set('constr_ubx', ubx); + ocp_model.set('constr_Jbu', Jbu); + ocp_model.set('constr_lbu', lbu); + ocp_model.set('constr_ubu', ubu); end ocp_model.model_struct @@ -210,14 +210,14 @@ ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); if (strcmp(nlp_solver, 'sqp')) - ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); - ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); - ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); - ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); + ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); @@ -228,7 +228,7 @@ %% acados ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); ocp_model.set('name', model_name); @@ -251,7 +251,7 @@ sim_model.set('dyn_expr_f', model.expr_f_impl); sim_model.set('sym_xdot', model.sym_xdot); if isfield(model, 'sym_z') - sim_model.set('sym_z', model.sym_z); + sim_model.set('sym_z', model.sym_z); end %% acados sim opts @@ -269,13 +269,13 @@ sim_opts.set('sens_hess', 'false'); sim_opts.set('jac_reuse', sim_jac_reuse); if (strcmp(sim_method, 'irk_gnsf')) - sim_opts.set('gnsf_detect_struct', gnsf_detect_struct); + sim_opts.set('gnsf_detect_struct', gnsf_detect_struct); end %% acados sim % create integrator -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation N_sim = 99; @@ -302,69 +302,69 @@ tic for ii=1:N_sim - % set initial state - ocp.set('constr_x0', x_sim(:,ii)); - % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); + % set initial state + ocp_solver.set('constr_x0', x_sim(:,ii)); + % set trajectory initialization (if not, set internally using previous solution) + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', pi_traj_init); if ii == 1 - ocp.set('init_z', z_traj_init); - ocp.set('init_xdot', xdot_traj_init); + ocp_solver.set('init_z', z_traj_init); + ocp_solver.set('init_xdot', xdot_traj_init); end - ocp.solve(); - ocp.print('stat'); + ocp_solver.solve(); + ocp_solver.print('stat'); - status = ocp.get('status'); - sqp_iter(ii) = ocp.get('sqp_iter'); - sqp_time(ii) = ocp.get('time_tot'); + status = ocp_solver.get('status'); + sqp_iter(ii) = ocp_solver.get('sqp_iter'); + sqp_time(ii) = ocp_solver.get('time_tot'); if status ~= 0 keyboard end - % get solution for initialization of next NLP - x_traj = ocp.get('x'); - u_traj = ocp.get('u'); - pi_traj = ocp.get('pi'); - z_traj = ocp.get('z'); + % get solution for initialization of next NLP + x_traj = ocp_solver.get('x'); + u_traj = ocp_solver.get('u'); + pi_traj = ocp_solver.get('pi'); + z_traj = ocp_solver.get('z'); - % shift trajectory for initialization - x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; - u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; - pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; - z_traj_init = [z_traj(:,2:end), z_traj(:,end)]; + % shift trajectory for initialization + x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; + u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; + pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; + z_traj_init = [z_traj(:,2:end), z_traj(:,end)]; - u_sim(:,ii) = ocp.get('u', 0); % get control input + u_sim(:,ii) = ocp_solver.get('u', 0); % get control input % initialize implicit integrator if (strcmp(sim_method, 'irk')) - sim.set('xdot', xdot0); - sim.set('z', z0); + sim_solver.set('xdot', xdot0); + sim_solver.set('z', z0); elseif (strcmp(sim_method, 'irk_gnsf')) import casadi.* - x01_gnsf = x0(sim.model_struct.dyn_gnsf_idx_perm_x(1:sim.model_struct.dim_gnsf_nx1)); - x01_dot_gnsf = xdot0(sim.model_struct.dyn_gnsf_idx_perm_x(1:sim.model_struct.dim_gnsf_nx1)); - z0_gnsf = z0(sim.model_struct.dyn_gnsf_idx_perm_z( 1:sim.model_struct.dim_gnsf_nz1 )); - y_in = sim.model_struct.dyn_gnsf_L_x * x01_gnsf ... - + sim.model_struct.dyn_gnsf_L_xdot * x01_dot_gnsf ... - + sim.model_struct.dyn_gnsf_L_z * z0_gnsf; - u_hat = sim.model_struct.dyn_gnsf_L_u * u_sim(:,ii); + x01_gnsf = x0(sim_solver.model_struct.dyn_gnsf_idx_perm_x(1:sim_solver.model_struct.dim_gnsf_nx1)); + x01_dot_gnsf = xdot0(sim_solver.model_struct.dyn_gnsf_idx_perm_x(1:sim_solver.model_struct.dim_gnsf_nx1)); + z0_gnsf = z0(sim_solver.model_struct.dyn_gnsf_idx_perm_z( 1:sim_solver.model_struct.dim_gnsf_nz1 )); + y_in = sim_solver.model_struct.dyn_gnsf_L_x * x01_gnsf ... + + sim_solver.model_struct.dyn_gnsf_L_xdot * x01_dot_gnsf ... + + sim_solver.model_struct.dyn_gnsf_L_z * z0_gnsf; + u_hat = sim_solver.model_struct.dyn_gnsf_L_u * u_sim(:,ii); phi_fun = Function([model_name,'_gnsf_phi_fun'],... - {sim.model_struct.sym_gnsf_y, sim.model_struct.sym_gnsf_uhat},... - {sim.model_struct.dyn_gnsf_expr_phi(:)}); % sim.model_struct.sym_p + {sim_solver.model_struct.sym_gnsf_y, sim_solver.model_struct.sym_gnsf_uhat},... + {sim_solver.model_struct.dyn_gnsf_expr_phi(:)}); % sim_solver.model_struct.sym_p phi_guess = full( phi_fun( y_in, u_hat ) ); - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end - sim.set('x', x_sim(:,ii)); % set initial state - sim.set('u', u_sim(:,ii)); % set input - sim.solve(); % simulate state + sim_solver.set('x', x_sim(:,ii)); % set initial state + sim_solver.set('u', u_sim(:,ii)); % set input + sim_solver.solve(); % simulate state - % get simulated state - x_sim(:,ii+1) = sim.get('xn'); - z_sim(:,ii) = sim.get('zn'); + % get simulated state + x_sim(:,ii+1) = sim_solver.get('xn'); + z_sim(:,ii) = sim_solver.get('zn'); end format short e @@ -412,13 +412,15 @@ xp = x_sim(1,:); yp = x_sim(2,:); check = abs(xp.^2 + yp.^2 - length_pendulum^2); -tol_pendulum = 1e-10; +tol_pendulum = 1e-4; + + +disp(['checking for constant pendulum length, got ' num2str(check)]) if any( max(abs(check)) > tol_pendulum ) - disp(['note: check for constant pendulum length failed, violation >' ... + error(['note: check for constant pendulum length failed, violation >' ... num2str(tol_pendulum)]); end -dist2target = norm( sim.get('xn') - xtarget ) % eval constraint h ax_ = z_sim(1,:); diff --git a/examples/acados_matlab_octave/pendulum_dae/example_sim.m b/examples/acados_matlab_octave/pendulum_dae/example_sim.m index 35d411a59b..e53a467772 100644 --- a/examples/acados_matlab_octave/pendulum_dae/example_sim.m +++ b/examples/acados_matlab_octave/pendulum_dae/example_sim.m @@ -116,7 +116,7 @@ %% acados sim % create integrator -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); N_sim = 100; @@ -131,47 +131,47 @@ for ii=1:N_sim % set initial state - sim.set('x', x_sim(:,ii)); - sim.set('u', u); + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u); % set adjoint seed - sim.set('seed_adj', ones(nx,1)); + sim_solver.set('seed_adj', ones(nx,1)); % initialize implicit integrator if (strcmp(method, 'irk')) - sim.set('xdot', xdot0); - sim.set('z', z0); + sim_solver.set('xdot', xdot0); + sim_solver.set('z', z0); elseif (strcmp(method, 'irk_gnsf')) - y_in = sim.model_struct.dyn_gnsf_L_x * x0 ... - + sim.model_struct.dyn_gnsf_L_xdot * xdot0 ... - + sim.model_struct.dyn_gnsf_L_z * z0; - u_hat = sim.model_struct.dyn_gnsf_L_u * u; + y_in = sim_solver.model_struct.dyn_gnsf_L_x * x0 ... + + sim_solver.model_struct.dyn_gnsf_L_xdot * xdot0 ... + + sim_solver.model_struct.dyn_gnsf_L_z * z0; + u_hat = sim_solver.model_struct.dyn_gnsf_L_u * u; phi_fun = Function([model_name,'_gnsf_phi_fun'],... - {sim.model_struct.sym_gnsf_y, sim.model_struct.sym_gnsf_uhat},... - {sim.model_struct.dyn_gnsf_expr_phi(:)}); % sim.model_struct.sym_p + {sim_solver.model_struct.sym_gnsf_y, sim_solver.model_struct.sym_gnsf_uhat},... + {sim_solver.model_struct.dyn_gnsf_expr_phi(:)}); % sim_solver.model_struct.sym_p phi_guess = full( phi_fun( y_in, u_hat ) ); - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); % forward sensitivities - S_forw = sim.get('S_forw'); - Sx = sim.get('Sx'); - Su = sim.get('Su'); + S_forw = sim_solver.get('S_forw'); + Sx = sim_solver.get('Sx'); + Su = sim_solver.get('Su'); end format short e -xfinal = sim.get('xn')'; -S_adj = sim.get('S_adj')'; -z = sim.get('zn')'; % approximate value of algebraic variables at start of simulation -S_alg = sim.get('S_algebraic'); % sensitivities of algebraic variables z +xfinal = sim_solver.get('xn')'; +S_adj = sim_solver.get('S_adj')'; +z = sim_solver.get('zn')'; % approximate value of algebraic variables at start of simulation +S_alg = sim_solver.get('S_algebraic'); % sensitivities of algebraic variables z simulation_time = toc 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 c37b440bee..94fdcfa713 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 @@ -32,8 +32,6 @@ %% test of native matlab interface clear all -GENERATE_C_CODE = 0; - model_name = 'ocp_pendulum'; % check that env.sh has been run @@ -198,10 +196,6 @@ ocp_model.set('constr_ubu', ubu); end -ocp_model.model_struct - - - %% acados ocp opts ocp_opts = acados_ocp_opts(); ocp_opts.set('compile_interface', compile_interface); @@ -225,17 +219,9 @@ ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); ocp_opts.set('sim_method_num_steps', ocp_sim_method_num_steps); -ocp_opts.opts_struct - - - %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); - -if GENERATE_C_CODE == 1 - ocp.generate_c_code() -end +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% acados sim model sim_model = acados_sim_model(); @@ -257,9 +243,6 @@ 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); @@ -269,13 +252,9 @@ sim_opts.set('method', sim_method); sim_opts.set('sens_forw', sim_sens_forw); -%sim_opts.opts_struct - - - %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation @@ -300,43 +279,41 @@ for ii=1:N_sim % set x0 - ocp.set('constr_x0', x_sim(:,ii)); + ocp_solver.set('constr_x0', x_sim(:,ii)); % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', pi_traj_init); - % use ocp.set to modify numerical data for a certain stage + % use ocp_solver.set to modify numerical data for a certain stage some_stages = 1:10:ocp_N-1; for i = some_stages - if strcmp( ocp.model_struct.cost_type, 'linear_ls') - ocp.set('cost_Vx', Vx, i); + if strcmp(ocp_solver.ocp_solver.cost.cost_type, 'LINEAR_LS') + ocp_solver.set('cost_Vx', Vx, i); end end % solve OCP - ocp.solve(); - - if 1 - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot = ocp.get('time_tot'); - time_lin = ocp.get('time_lin'); - time_qp_sol = ocp.get('time_qp_sol'); - - fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n',... - status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); - if status~=0 - disp('acados ocp solver failed'); - keyboard - end + ocp_solver.solve(); + + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot = ocp_solver.get('time_tot'); + time_lin = ocp_solver.get('time_lin'); + time_qp_sol = ocp_solver.get('time_qp_sol'); + + fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n',... + status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); + if status~=0 + disp('acados ocp solver failed'); + keyboard end % get solution for initialization of next NLP - x_traj = ocp.get('x'); - u_traj = ocp.get('u'); - pi_traj = ocp.get('pi'); + x_traj = ocp_solver.get('x'); + u_traj = ocp_solver.get('u'); + pi_traj = ocp_solver.get('pi'); % shift trajectory for initialization x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; @@ -344,18 +321,18 @@ pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; % get solution for sim - u_sim(:,ii) = ocp.get('u', 0); + u_sim(:,ii) = ocp_solver.get('u', 0); % set initial state of sim - sim.set('x', x_sim(:,ii)); + sim_solver.set('x', x_sim(:,ii)); % set input in sim - sim.set('u', u_sim(:,ii)); + sim_solver.set('u', u_sim(:,ii)); % simulate state - sim.solve(); + sim_solver.solve(); % get new state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end 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 29211d7784..dd69c391f9 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 @@ -234,8 +234,8 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); -%ocp.model_struct +ocp_solver = acados_ocp(ocp_model, ocp_opts); +%ocp_solver.model_struct % set trajectory initialization @@ -246,37 +246,37 @@ u_traj_init = zeros(nu, N); % if not set, the trajectory is initialized with the previous solution -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % change number of sqp iterations -%ocp.set('nlp_solver_max_iter', 20); +%ocp_solver.set('nlp_solver_max_iter', 20); % solve tic; % solve ocp -ocp.solve(); +ocp_solver.solve(); time_ext = toc; % TODO: add getter for internal timing -fprintf(['time for ocp.solve (matlab tic-toc): ', num2str(time_ext), ' s\n']) +fprintf(['time for ocp_solver.solve (matlab tic-toc): ', num2str(time_ext), ' s\n']) % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); %% evaluation -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); -ocp.print('stat'); +ocp_solver.print('stat'); %% figures @@ -296,7 +296,7 @@ xlim([0 N]); legend('F'); -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); if (strcmp(nlp_solver, 'sqp')) figure; plot([0: size(stat,1)-1], log10(stat(:,2)), 'r-x'); diff --git a/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m b/examples/acados_matlab_octave/pendulum_on_cart_model/example_ocp_custom_hess.m index 493fed101f..ef929be030 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 @@ -118,7 +118,6 @@ 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 %% acados ocp set opts ocp_opts = acados_ocp_opts(); @@ -129,10 +128,9 @@ ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); ocp_opts.set('globalization', 'merit_backtracking'); ocp_opts.set('nlp_solver_max_iter', 500); -% ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); x_traj_init = zeros(nx, N+1); @@ -144,25 +142,25 @@ %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.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_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); +% ocp_solver.set('init_pi', zeros(nx, N)) % change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -% ocp.set('constr_lbx', x0, 0) +% ocp_solver.set('field', value, optional: stage_index) +% ocp_solver.set('constr_lbx', x0, 0) % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') %% Plots ts = linspace(0, T, N+1); @@ -180,12 +178,3 @@ ylabel('F [N]') xlabel('t [s]') grid on - -%% go embedded -% to generate templated C code -% ocp.generate_c_code; - -% test MEX template based solver -% cd c_generated_code -% command = strcat('t_ocp = ', model_name, '_mex_solver'); -% eval( command ); 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 ccbec4b718..f3869fe323 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 @@ -231,11 +231,11 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % ocp -% disp('ocp.C_ocp'); -% disp(ocp.C_ocp); -%ocp.model_struct +% disp('ocp_solver.C_ocp'); +% disp(ocp_solver.C_ocp); +%ocp_solver.model_struct % set trajectory initialization %x_traj_init = zeros(nx, N+1); @@ -245,30 +245,30 @@ u_traj_init = zeros(nu, N); % if not set, the trajectory is initialized with the previous solution -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % solve ocp tic; -ocp.solve(); +ocp_solver.solve(); time_ext = toc; % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); %% evaluation -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n',... status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); -ocp.print('stat'); +ocp_solver.print('stat'); %% figures @@ -292,7 +292,7 @@ end % plot residuals over iteraions -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); if 0 && (strcmp(nlp_solver, 'sqp')) figure; plot([0: size(stat,1)-1], log10(stat(:,2)), 'r-x'); @@ -324,10 +324,10 @@ field = 'ex'; % equality constraint on states stage = 0; index = 1; - ocp.eval_param_sens(field, stage, index); + ocp_solver.eval_param_sens(field, stage, index); - sens_u = ocp.get('sens_u'); - sens_x = ocp.get('sens_x'); + sens_u = ocp_solver.get('sens_u'); + sens_x = ocp_solver.get('sens_x'); % plot sensitivity figure @@ -363,14 +363,14 @@ sens_u = zeros(nx, N); % get sensitivities w.r.t. initial state value with index for index = 0:nx-1 - ocp.eval_param_sens(field, stage, index); - sens_u(index+1,:) = ocp.get('sens_u'); + ocp_solver.eval_param_sens(field, stage, index); + sens_u(index+1,:) = ocp_solver.get('sens_u'); end disp('solution sensitivity dU_dx0') disp(sens_u) -% qp_hess = ocp.get('qp_solver_cond_H'); +% qp_hess = ocp_solver.get('qp_solver_cond_H'); % nv = size(qp_hess, 1); % % make full % for jj=1:nv 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 f0e4782580..c61f51a929 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 @@ -239,11 +239,11 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % ocp -% disp('ocp.C_ocp'); -% disp(ocp.C_ocp); -%ocp.model_struct +% disp('ocp_solver.C_ocp'); +% disp(ocp_solver.C_ocp); +%ocp_solver.model_struct % set trajectory initialization @@ -254,35 +254,35 @@ u_traj_init = zeros(nu, N); % if not set, the trajectory is initialized with the previous solution -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % change number of sqp iterations -%ocp.set('nlp_solver_max_iter', 20); +%ocp_solver.set('nlp_solver_max_iter', 20); % solve tic; if 0 % solve ocp - ocp.solve(); + ocp_solver.solve(); else % do one step at the time - ocp.set('nlp_solver_max_iter', 1); + ocp_solver.set('nlp_solver_max_iter', 1); for ii=1:nlp_solver_max_iter disp(['iteration number ', num2str(ii)]) % solve the system using 1 SQP iteration - ocp.solve(); + ocp_solver.solve(); % print 1-iteration stat - ocp.print('stat'); + ocp_solver.print('stat'); % check stability of qp - qp_A = ocp.get('qp_A'); + qp_A = ocp_solver.get('qp_A'); qp_A_eig_max = 0; for jj=1:length(qp_A) tmp_A = qp_A{jj}; @@ -295,7 +295,7 @@ fprintf('A eig max %e\n', qp_A_eig_max); % compute conditioning number and eigenvalues of hessian of (partial) cond qp - qp_cond_H = ocp.get('qp_solver_cond_H'); + qp_cond_H = ocp_solver.get('qp_solver_cond_H'); if iscell(qp_cond_H) for jj=1:length(qp_cond_H) @@ -328,7 +328,7 @@ end % check residuals and terminate if tol is reached - residuals = ocp.get('residuals'); + residuals = ocp_solver.get('residuals'); if residuals(1) < nlp_solver_tol_stat && residuals(2) < nlp_solver_tol_eq && residuals(3) < nlp_solver_tol_ineq && residuals(4) < nlp_solver_tol_comp break end @@ -338,20 +338,20 @@ time_ext = toc; % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); %% evaluation -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); -ocp.print('stat'); +ocp_solver.print('stat'); %% figures @@ -374,7 +374,7 @@ %% plot residual -% stat = ocp.get('stat'); +% stat = ocp_solver.get('stat'); % if (strcmp(nlp_solver, 'sqp')) % figure; % plot([0: size(stat,1)-1], log10(stat(:,2)), 'r-x'); 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 2664c9e466..1f075cbdd4 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 @@ -105,11 +105,11 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % (re)set numerical part of model -%sim.set('T', 0.5); -%sim.C_sim -%sim.C_sim_ext_fun +%sim_solver.set('T', 0.5); +%sim_solver.C_sim +%sim_solver.C_sim_ext_fun N_sim = 100; @@ -121,35 +121,35 @@ for ii=1:N_sim % set initial state - sim.set('x', x_sim(:,ii)); - sim.set('u', u); + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u); % initialize implicit integrator if (strcmp(method, 'irk')) - sim.set('xdot', zeros(nx,1)); + sim_solver.set('xdot', zeros(nx,1)); elseif (strcmp(method, 'irk_gnsf')) - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end simulation_time = toc % xn -xn = sim.get('xn'); +xn = sim_solver.get('xn'); xn % S_forw -S_forw = sim.get('S_forw') -Sx = sim.get('Sx'); -Su = sim.get('Su'); +S_forw = sim_solver.get('S_forw') +Sx = sim_solver.get('Sx'); +Su = sim_solver.get('Su'); %x_sim 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 f57a04cee9..bb0d004998 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 @@ -129,7 +129,7 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); @@ -153,7 +153,7 @@ sim_opts.set('num_stages', plant_sim_method_num_stages); sim_opts.set('num_steps', plant_sim_method_num_steps); -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% Simulation N_sim = 100; @@ -166,14 +166,14 @@ for i=1:N_sim % update initial state xcurrent = x_sim(:,i); - ocp.set('constr_x0', xcurrent); + ocp_solver.set('constr_x0', xcurrent); if i == 1 || i == floor(N_sim/2) % solve - ocp.solve(); + ocp_solver.solve(); % get solution - u0 = ocp.get('u', 0); - status = ocp.get('status'); % 0 - success + u0 = ocp_solver.get('u', 0); + status = ocp_solver.get('status'); % 0 - success x_lin = xcurrent; u_lin = u0; @@ -182,8 +182,8 @@ field = 'ex'; % equality constraint on states stage = 0; for index = 0:nx-1 - ocp.eval_param_sens(field, stage, index); - sens_u(index+1,:) = ocp.get('sens_u'); + ocp_solver.eval_param_sens(field, stage, index); + sens_u(index+1,:) = ocp_solver.get('sens_u'); end else % use feedback policy @@ -192,14 +192,14 @@ end % set initial state - sim.set('x', xcurrent); - sim.set('u', u0); + sim_solver.set('x', xcurrent); + sim_solver.set('u', u0); % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,i+1) = sim.get('xn'); + x_sim(:,i+1) = sim_solver.get('xn'); u_sim(:,i) = u0; end 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 b5b7d8dfea..a97b976be3 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 @@ -207,34 +207,34 @@ %% acados ocp - ocp = acados_ocp(ocp_model, ocp_opts); + ocp_solver = acados_ocp(ocp_model, ocp_opts); % set trajectory initialization x_traj_init = [linspace(0, 0, N+1); linspace(pi, 0, N+1); linspace(0, 0, N+1); linspace(0, 0, N+1)]; u_traj_init = zeros(nu, N); - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); - ocp.solve(); + ocp_solver.solve(); % get solution - u = ocp.get('u'); - x = ocp.get('x'); + u = ocp_solver.get('u'); + x = ocp_solver.get('x'); %% evaluation - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot = ocp.get('time_tot'); - time_lin = ocp.get('time_lin'); - time_reg = ocp.get('time_reg'); - time_qp_sol = ocp.get('time_qp_sol'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot = ocp_solver.get('time_tot'); + time_lin = ocp_solver.get('time_lin'); + time_reg = ocp_solver.get('time_reg'); + time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf(['\nstatus = %d, sqp_iter = %d, time_int = %f [ms]'... ' (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n'],... status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3 ); - ocp.print('stat'); + ocp_solver.print('stat'); if i == 1 % save reference @@ -251,7 +251,7 @@ omegas = xref(4,:); z_fromx = cos(thetas).*sin(thetas).*omegas.^2; if i > 1 - z = ocp.get('z'); + z = ocp_solver.get('z'); err_z_zfromx(i) = norm( z - z_fromx(1:end-1) ); end diff --git a/examples/acados_matlab_octave/race_cars/main.m b/examples/acados_matlab_octave/race_cars/main.m index 0f670ce261..18d7efd0d2 100644 --- a/examples/acados_matlab_octave/race_cars/main.m +++ b/examples/acados_matlab_octave/race_cars/main.m @@ -208,7 +208,7 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% Simulate dt = T / N; @@ -223,14 +223,14 @@ tcomp_sum = 0; tcomp_max = 0; -ocp.set('constr_x0', model.x0); -ocp.set('constr_lbx', model.x0, 0) -ocp.set('constr_ubx', model.x0, 0) +ocp_solver.set('constr_x0', model.x0); +ocp_solver.set('constr_lbx', model.x0, 0) +ocp_solver.set('constr_ubx', model.x0, 0) % set trajectory initialization -ocp.set('init_x', model.x0' * ones(1,N+1)); -ocp.set('init_u', zeros(nu, N)); -ocp.set('init_pi', zeros(nx, N)); +ocp_solver.set('init_x', model.x0' * ones(1,N+1)); +ocp_solver.set('init_u', zeros(nu, N)); +ocp_solver.set('init_pi', zeros(nx, N)); % simulate for i = 1:Nsim @@ -239,17 +239,17 @@ for j = 0:(N-1) yref = [s0 + (sref - s0) * j / N, 0, 0, 0, 0, 0, 0, 0]; % yref=[1,0,0,1,0,0,0,0] - ocp.set('cost_y_ref', yref, j); + ocp_solver.set('cost_y_ref', yref, j); end yref_N = [sref, 0, 0, 0, 0, 0]; % yref_N=np.array([0,0,0,0,0,0]) - ocp.set('cost_y_ref_e', yref_N); + ocp_solver.set('cost_y_ref_e', yref_N); % solve ocp t = tic(); - ocp.solve(); - status = ocp.get('status'); % 0 - success + ocp_solver.solve(); + status = ocp_solver.get('status'); % 0 - success if status ~= 0 && status ~= 2 % borrowed from acados/utils/types.h %statuses = { @@ -261,7 +261,7 @@ % 5: 'ACADOS_READY' error(sprintf('acados returned status %d in closed loop iteration %d. Exiting.', status, i)); end - %ocp.print('stat') + %ocp_solver.print('stat') elapsed = toc(t); @@ -272,8 +272,8 @@ end % get solution - x0 = ocp.get('x', 0); - u0 = ocp.get('u', 0); + x0 = ocp_solver.get('x', 0); + u0 = ocp_solver.get('u', 0); for j = 1:nx simX(i, j) = x0(j); end @@ -282,11 +282,11 @@ end % update initial condition - x0 = ocp.get('x', 1); + x0 = ocp_solver.get('x', 1); % update initial state - ocp.set('constr_x0', x0); - ocp.set('constr_lbx', x0, 0); - ocp.set('constr_ubx', x0, 0); + ocp_solver.set('constr_x0', x0); + ocp_solver.set('constr_lbx', x0, 0); + ocp_solver.set('constr_ubx', x0, 0); s0 = x0(1); % check if one lap is done and break and remove entries beyond @@ -342,8 +342,3 @@ xlabel('t'); ylabel('alat'); xlim([t(1), t(end)]); - - -%% go embedded -% to generate templated C code -% ocp.generate_c_code; 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 cb3edf4cb4..396e60a71b 100644 --- a/examples/acados_matlab_octave/simple_dae_model/example_ocp.m +++ b/examples/acados_matlab_octave/simple_dae_model/example_ocp.m @@ -197,27 +197,27 @@ %% acados ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); -ocp.solve(); +ocp_solver.solve(); -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); -ocp.print('stat') +ocp_solver.print('stat') -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -sqp_time = ocp.get('time_tot'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +sqp_time = ocp_solver.get('time_tot'); %if status ~= 0 % keyboard %end format short e % get solution for initialization of next NLP -x_traj = ocp.get('x'); -u_traj = ocp.get('u'); -pi_traj = ocp.get('pi'); -z_traj = ocp.get('z'); +x_traj = ocp_solver.get('x'); +u_traj = ocp_solver.get('u'); +pi_traj = ocp_solver.get('pi'); +z_traj = ocp_solver.get('z'); diff_x_z = x_traj(:,1:N) - z_traj diff --git a/examples/acados_matlab_octave/swarming/example_closed_loop.m b/examples/acados_matlab_octave/swarming/example_closed_loop.m index 9b88119d9a..9c87906d8f 100644 --- a/examples/acados_matlab_octave/swarming/example_closed_loop.m +++ b/examples/acados_matlab_octave/swarming/example_closed_loop.m @@ -201,7 +201,7 @@ %% Acados ocp % Create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% Acados simulation model @@ -248,7 +248,7 @@ %% Acados simulation % Create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% Closed loop simulation @@ -278,47 +278,47 @@ for k = 1:nb_steps_sim % Set initial condition x0 - ocp.set('constr_x0', x_history(:,k)); -% ocp.set('constr_expr_h', model.expr_h); -% ocp.set('constr_lh', lh); -% ocp.set('constr_uh', uh); + ocp_solver.set('constr_x0', x_history(:,k)); +% ocp_solver.set('constr_expr_h', model.expr_h); +% ocp_solver.set('constr_lh', lh); +% ocp_solver.set('constr_uh', uh); % Set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); % solve OCP - ocp.solve(); + ocp_solver.solve(); - status(k) = ocp.get('status'); - sqp_iter(k) = ocp.get('sqp_iter'); - time_tot(k) = ocp.get('time_tot'); - time_lin(k) = ocp.get('time_lin'); - time_qp_sol(k) = ocp.get('time_qp_sol'); + status(k) = ocp_solver.get('status'); + sqp_iter(k) = ocp_solver.get('sqp_iter'); + time_tot(k) = ocp_solver.get('time_tot'); + time_lin(k) = ocp_solver.get('time_lin'); + time_qp_sol(k) = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_tot = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status(k), sqp_iter(k), time_tot(k)*1e3, time_lin(k)*1e3, time_qp_sol(k)*1e3); % Get solution for initialization of next NLP - x_traj = ocp.get('x'); - u_traj = ocp.get('u'); + x_traj = ocp_solver.get('x'); + u_traj = ocp_solver.get('u'); % Shift trajectory for initialization x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; % Get solution for simulation - u_history(:,k) = ocp.get('u', 0); + u_history(:,k) = ocp_solver.get('u', 0); % Set initial state of simulation - sim.set('x', x_history(:,k)); + sim_solver.set('x', x_history(:,k)); % set input in sim - sim.set('u', u_history(:,k)); + sim_solver.set('u', u_history(:,k)); % Simulate state - sim.solve(); + sim_solver.solve(); % Get new state - x_history(:,k+1) = sim.get('xn'); + x_history(:,k+1) = sim_solver.get('xn'); end diff --git a/examples/acados_matlab_octave/swarming/example_ocp.m b/examples/acados_matlab_octave/swarming/example_ocp.m index 9cca85dc5d..9f25c84b48 100644 --- a/examples/acados_matlab_octave/swarming/example_ocp.m +++ b/examples/acados_matlab_octave/swarming/example_ocp.m @@ -210,7 +210,7 @@ %% Acados ocp % Create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % Set trajectory initialization step_mat = repmat((0:1:nb_steps),3*N,1); @@ -218,37 +218,37 @@ x_traj_init = [pos0_traj; ... v_ref*repmat(u_ref,N,nb_steps+1)]; u_traj_init = zeros(nu, nb_steps); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % Solve tic; -ocp.solve(); +ocp_solver.solve(); simulation_time = toc; disp(strcat('Simulation time: ',num2str(simulation_time))); %x0(1) = 1.5; -%ocp.set('constr_x0', x0); -%ocp.set('cost_y_ref', 1); +%ocp_solver.set('constr_x0', x0); +%ocp_solver.set('cost_y_ref', 1); % If not set, the trajectory is initialized with the previous solution % Get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); %% Statistics -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); % time_ext = %f [ms], time_ext*1e3, -ocp.print('stat'); +ocp_solver.print('stat'); %% Extract trajectories @@ -287,7 +287,7 @@ if (strcmp(nlp_solver, 'sqp')) figure; - stat = ocp.get('stat'); + stat = ocp_solver.get('stat'); plot([0: sqp_iter], log10(stat(:,2)), 'r-x'); hold on plot([0: sqp_iter], log10(stat(:,3)), 'b-x'); diff --git a/examples/acados_matlab_octave/swarming/example_sim.m b/examples/acados_matlab_octave/swarming/example_sim.m index 9ce1cfa9e0..22a583c1b3 100644 --- a/examples/acados_matlab_octave/swarming/example_sim.m +++ b/examples/acados_matlab_octave/swarming/example_sim.m @@ -100,11 +100,11 @@ %% Acados simulation % Create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % (Re)set numerical part of model -%sim.set('T', 0.5); -%sim.C_sim -%sim.C_sim_ext_fun +%sim_solver.set('T', 0.5); +%sim_solver.C_sim +%sim_solver.C_sim_ext_fun x_history = zeros(nb_steps+1, nx); x_history(1,:) = x0'; @@ -112,22 +112,22 @@ tic for k = 1:nb_steps % Set initial state - sim.set('x', x_history(k,:)); - sim.set('u', u); + sim_solver.set('x', x_history(k,:)); + sim_solver.set('u', u); % Solve - sim.solve(); + sim_solver.solve(); % Get simulated state - x_history(k+1,:) = sim.get('xn'); + x_history(k+1,:) = sim_solver.get('xn'); end simulation_time = toc; disp(strcat('Simulation time: ',num2str(simulation_time))); -xn = sim.get('xn'); -S_forw = sim.get('S_forw'); -Sx = sim.get('Sx'); -Su = sim.get('Su'); +xn = sim_solver.get('xn'); +S_forw = sim_solver.get('S_forw'); +Sx = sim_solver.get('Sx'); +Su = sim_solver.get('Su'); %% Plot variables diff --git a/examples/acados_matlab_octave/test/run_matlab_tests.m b/examples/acados_matlab_octave/test/run_matlab_tests.m index 32815378a8..fc3a5c07ca 100644 --- a/examples/acados_matlab_octave/test/run_matlab_tests.m +++ b/examples/acados_matlab_octave/test/run_matlab_tests.m @@ -26,7 +26,6 @@ test_names = ["run_test_dim_check", "run_test_ocp_mass_spring", % "run_test_ocp_pendulum", -"run_test_ocp_pendulum_dae", "run_test_ocp_simple_dae", "run_test_ocp_wtnx6", % "run_test_sim_adj", diff --git a/examples/acados_matlab_octave/test/run_test_ocp_pendulum_dae.m b/examples/acados_matlab_octave/test/run_test_ocp_pendulum_dae.m deleted file mode 100644 index 79d1117adf..0000000000 --- a/examples/acados_matlab_octave/test/run_test_ocp_pendulum_dae.m +++ /dev/null @@ -1,57 +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.; - -% - -addpath(pwd) - -%% check that environment variables are provided -try - check_casadi_availibility(); - require_env_variable('LD_LIBRARY_PATH'); - require_env_variable('ACADOS_INSTALL_DIR'); - if is_octave() - require_env_variable('OCTAVE_PATH'); - else - require_env_variable('MATLABPATH'); - end -catch exception - exit_with_error(exception); -end - - - -%% ocp tests -try - test_ocp_pendulum_dae; -catch exception - exit_with_error(exception); -end - -fprintf('\nrun_tests_ocp: success!\n\n'); diff --git a/examples/acados_matlab_octave/test/run_tests_templates.m b/examples/acados_matlab_octave/test/run_tests_templates.m deleted file mode 100644 index 600fbe9da3..0000000000 --- a/examples/acados_matlab_octave/test/run_tests_templates.m +++ /dev/null @@ -1,63 +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.; - -% - -addpath(pwd) - -%% check that environment variables are provided -try - check_casadi_availibility(); - require_env_variable('LD_LIBRARY_PATH'); - require_env_variable('ACADOS_INSTALL_DIR'); - if is_octave() - require_env_variable('OCTAVE_PATH'); - else - require_env_variable('MATLABPATH'); - end -catch exception - exit_with_error(exception); -end - - - -%% ocp tests -try - test_template_pendulum_ocp('auto'); - test_template_pendulum_ocp('nonlinear_ls'); - test_template_pendulum_ocp('ext_cost'); - test_template_pendulum_exact_hess; - test_template_pendulum_gnsf; - test_template_ocp_linear_dae; - test_template_disc_dyn_ocp_linear; -catch exception - exit_with_error(exception); -end - -fprintf('\nrun_tests_templates: success!\n\n'); diff --git a/examples/acados_matlab_octave/test/simulink_init_test.m b/examples/acados_matlab_octave/test/simulink_init_test.m index 088f5fc7c4..e11d065f07 100644 --- a/examples/acados_matlab_octave/test/simulink_init_test.m +++ b/examples/acados_matlab_octave/test/simulink_init_test.m @@ -45,7 +45,7 @@ %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts, simulink_opts); % solver initial guess x_traj_init = rand(nx, N+1); @@ -54,19 +54,19 @@ %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.set('constr_x0', x0); % set trajectory initialization -ocp.set('init_x', x_traj_init); % states -ocp.set('init_u', u_traj_init); % inputs -ocp.set('init_pi', pi_init); % multipliers for dynamics equality constraints +ocp_solver.set('init_x', x_traj_init); % states +ocp_solver.set('init_u', u_traj_init); % inputs +ocp_solver.set('init_pi', pi_init); % multipliers for dynamics equality constraints % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); -pi_all = ocp.get('pi'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); +pi_all = ocp_solver.get('pi'); if norm(pi_init - pi_all) > 1e-10 disp('pi initialization in MEX failed') @@ -78,8 +78,8 @@ disp('x initialization in MEX failed') end -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') %% simulink test cd c_generated_code diff --git a/examples/acados_matlab_octave/test/simulink_qp_test.m b/examples/acados_matlab_octave/test/simulink_qp_test.m index 0db700c141..c23795a919 100644 --- a/examples/acados_matlab_octave/test/simulink_qp_test.m +++ b/examples/acados_matlab_octave/test/simulink_qp_test.m @@ -42,7 +42,7 @@ %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts, simulink_opts); % solver initial guess x_traj_init = rand(nx, N+1); @@ -51,21 +51,21 @@ %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.set('constr_x0', x0); % set trajectory initialization -ocp.set('init_x', x_traj_init); % states -ocp.set('init_u', u_traj_init); % inputs -ocp.set('init_pi', pi_init); % multipliers for dynamics equality constraints +ocp_solver.set('init_x', x_traj_init); % states +ocp_solver.set('init_u', u_traj_init); % inputs +ocp_solver.set('init_pi', pi_init); % multipliers for dynamics equality constraints % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') %% simulink test cd c_generated_code diff --git a/examples/acados_matlab_octave/test/test_all_examples.m b/examples/acados_matlab_octave/test/test_all_examples.m index 956fdb6072..9b4f9f9d2b 100644 --- a/examples/acados_matlab_octave/test/test_all_examples.m +++ b/examples/acados_matlab_octave/test/test_all_examples.m @@ -92,6 +92,9 @@ [dir, file, extension] = fileparts(targets{idx}); testpath = getenv("TEST_DIR"); + % clear variables that might contain CasADi objects to avoid SWIG + % warnings + clear ocp_solver ocp ocp_model model sim sim_model sim_solver params save(strcat(testpath, "/test_workspace.mat")) setenv("LD_RUN_PATH", strcat(testpath, "/", dir, "/c_generated_code")) @@ -117,10 +120,11 @@ if exist(code_gen_dir, 'dir') rmdir(code_gen_dir, 's') end - close all; clc; + close all; + % clc; end -clc; +% clc; fail = false; disp('Succesful tests: ') for idx = 1:length(targets) diff --git a/examples/acados_matlab_octave/test/test_checks.m b/examples/acados_matlab_octave/test/test_checks.m index a5653e8e29..a2d5fce266 100644 --- a/examples/acados_matlab_octave/test/test_checks.m +++ b/examples/acados_matlab_octave/test/test_checks.m @@ -86,12 +86,12 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % Note: this does not work with gnsf, because it needs to be available % in the precomputation phase -% sim.set('T', Ts); +% sim_solver.set('T', Ts); %% test check, this should fail! % set initial state -sim.set('x', zeros(nx+1, 1)); +sim_solver.set('x', zeros(nx+1, 1)); diff --git a/examples/acados_matlab_octave/test/test_mhe_lorentz.m b/examples/acados_matlab_octave/test/test_mhe_lorentz.m index 2f6279f9b5..616a5e0d12 100644 --- a/examples/acados_matlab_octave/test/test_mhe_lorentz.m +++ b/examples/acados_matlab_octave/test/test_mhe_lorentz.m @@ -65,13 +65,13 @@ for n=1:N_sim % set initial state - sim.set('x', x_sim(:,n)); + sim_solver.set('x', x_sim(:,n)); % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,n+1) = sim.get('xn'); + x_sim(:,n+1) = sim_solver.get('xn'); % unmodeled step change in x(4) if n == iter_step diff --git a/examples/acados_matlab_octave/test/test_ocp_OSQP.m b/examples/acados_matlab_octave/test/test_ocp_OSQP.m index c6f174c276..a93039d8f8 100644 --- a/examples/acados_matlab_octave/test/test_ocp_OSQP.m +++ b/examples/acados_matlab_octave/test/test_ocp_OSQP.m @@ -100,32 +100,32 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.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_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); +ocp_solver.set('init_pi', zeros(nx, N)) % change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -ocp.set('constr_lbx', x0, 0) +% ocp_solver.set('field', value, optional: stage_index) +ocp_solver.set('constr_lbx', x0, 0) % solve -ocp.solve(); +ocp_solver.solve(); % get solutionn -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') if status == 0 disp('test_ocp_OSQP: success!'); 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 1e7eddcdbf..811fb56c90 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 @@ -222,52 +222,52 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); % set trajectory initialization x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % solve tic; -ocp.solve(); +ocp_solver.solve(); time_ext = toc; % store and load iterate filename = 'iterate.json'; -ocp.store_iterate(filename, true); -ocp.load_iterate(filename); +ocp_solver.store_iterate(filename, true); +ocp_solver.load_iterate(filename); delete(filename) % test QP dump filename = 'qp.json'; -ocp.dump_last_qp_to_json('qp.json') +ocp_solver.dump_last_qp_to_json('qp.json') delete(filename) % test qp_diagnostics -qp_diagnostics_result = ocp.qp_diagnostics(); +qp_diagnostics_result = ocp_solver.qp_diagnostics(); % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); % get info -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); % print statistics -ocp.print('stat') +ocp_solver.print('stat') if status~=0 error('ocp_nlp solver returned status nonzero'); diff --git a/examples/acados_matlab_octave/test/test_ocp_pendulum_dae.m b/examples/acados_matlab_octave/test/test_ocp_pendulum_dae.m deleted file mode 100644 index b51f5733a6..0000000000 --- a/examples/acados_matlab_octave/test/test_ocp_pendulum_dae.m +++ /dev/null @@ -1,452 +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.; - -% - -%% test of native matlab interface - -addpath('../pendulum_dae/'); - -%% options -compile_interface = 'auto'; % true, false -codgen_model = 'true'; % true, false -% compile_interface = 'auto'; % true, false -% codgen_model = 'false'; % true, false -% simulation -gnsf_detect_struct = 'true'; % true, false -sim_method = 'irk'; % irk, irk_gnsf, [erk] -sim_sens_forw = 'false'; % true, false -sim_jac_reuse = 'false'; % true, false -sim_num_stages = 3; -sim_num_steps = 3; -sim_newton_iter = 10; -model_name = 'pend_dae'; - -% ocp -ocp_N = 50; -nlp_solver = 'sqp_rti'; % sqp, sqp_rti -nlp_solver_exact_hessian = 'false'; -regularize_method = 'project_reduc_hess'; % no_regularize, project,... - % project_reduc_hess, mirror, convexify -nlp_solver_max_iter = 100; -qp_solver = 'partial_condensing_hpipm'; - % full_condensing_hpipm, partial_condensing_hpipm -qp_solver_cond_N = 5; -qp_solver_warm_start = 0; -qp_solver_cond_ric_alg = 0; % 0: dont factorize hessian in the condensing; 1: factorize -qp_solver_ric_alg = 0; % HPIPM specific -ocp_sim_method = 'irk'; % irk, irk_gnsf -ocp_sim_method_num_stages = 4; -ocp_sim_method_num_steps = 1; -ocp_sim_method_newton_iter = 3; -cost_type = 'linear_ls'; % linear_ls, ext_cost - -%% model -model = pendulum_dae_model(); -% sym_x = [xpos, ypos, alpha, vx, vy, valpha] -length_pendulum = 5; -xsteady = [ 0; -length_pendulum; 0; 0; 0; 0]; -xtarget = [ 0; +length_pendulum; pi; 0; 0; 0]; -% xtarget = xsteady; -uref = 0; - -alpha0 = -.01; -xp0 = length_pendulum * sin(alpha0); -yp0 = - length_pendulum * cos(alpha0); -x0 = [ xp0; yp0; alpha0; 0; 0; 0]; -% x0 = xsteady + 1e-4 * ones(nx,1); - -h = 0.05; -T = ocp_N*h; - -disp('state') -disp(model.sym_x) - -nx = length(model.sym_x); -nu = length(model.sym_u); -nz = length(model.sym_z); - -ny = nu+nx; % number of outputs in lagrange term -ny_e = nx; % number of outputs in mayer term - -ng = 0; % number of general linear constraints intermediate stages -ng_e = 0; % number of general linear constraints final stage -nbx = 0; % number of bounds on state x - -nbu = nu; % number of bounds on controls u -nh_e = 0; - -% cost -% linear least square cost: y^T * W * y, where y = Vx * x + Vu * u + Vz * z - y_ref -Vx = eye(ny, nx); % state-to-output matrix in lagrange term -Vu = zeros(ny, nu); -Vu(nx:end, nx:end) = eye(nu); % input-to-output matrix in lagrange term -Vx_e = Vx(1:ny_e,:); % state-to-output matrix in mayer term -% weight matrix in lagrange term -W = diag([1e3 * ones(3,1);... % xpos, ypos, alpha - ones(3,1); ... %speeds - 1e2 ]); % control -W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in mayer term -Vz = zeros(ny, nz); -yr = [xtarget; uref]; % output reference in lagrange term -yr_e = xtarget; % output reference in mayer term - -% constraints -Jbu = eye(nbu, nu); -lbu = -80*ones(nu, 1); -ubu = 80*ones(nu, 1); - - -%% acados ocp model -ocp_model = acados_ocp_model(); -ocp_model.set('T', T); - -constraint_h = 1; -if constraint_h - nh = length(model.expr_h); - ocp_model.set('constr_expr_h', model.expr_h); - lh = 0; - uh = 200; - ocp_model.set('constr_lh', lh); - ocp_model.set('constr_uh', uh); -else - nh = 0; -end - -% symbolics -ocp_model.set('sym_x', model.sym_x); -if isfield(model, 'sym_u') - ocp_model.set('sym_u', model.sym_u); -end -if isfield(model, 'sym_xdot') - ocp_model.set('sym_xdot', model.sym_xdot); -end -if isfield(model, 'sym_z') - ocp_model.set('sym_z', model.sym_z); -end - -% cost -ocp_model.set('cost_type', cost_type); -ocp_model.set('cost_type_e', cost_type); -if (strcmp(cost_type, 'linear_ls')) - ocp_model.set('cost_Vu', Vu); - ocp_model.set('cost_Vz', Vz); - ocp_model.set('cost_Vx', Vx); - ocp_model.set('cost_Vx_e', Vx_e); - ocp_model.set('cost_W', W); - ocp_model.set('cost_W_e', W_e); - ocp_model.set('cost_y_ref', yr); - ocp_model.set('cost_y_ref_e', yr_e); -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); -end - -% dynamics -if (strcmp(ocp_sim_method, 'erk')) - ocp_model.set('dyn_type', 'explicit'); - ocp_model.set('dyn_expr_f', model.expr_f_expl); -else % irk - ocp_model.set('dyn_type', 'implicit'); - ocp_model.set('dyn_expr_f', model.expr_f_impl); -end -% constraints -ocp_model.set('constr_x0', x0); -if (ng>0) - ocp_model.set('constr_C', C); - ocp_model.set('constr_D', D); - ocp_model.set('constr_lg', lg); - ocp_model.set('constr_ug', ug); - ocp_model.set('constr_C_e', C_e); - ocp_model.set('constr_lg_e', lg_e); - ocp_model.set('constr_ug_e', ug_e); -else -% ocp_model.set('constr_Jbx', Jbx); -% ocp_model.set('constr_lbx', lbx); -% ocp_model.set('constr_ubx', ubx); - ocp_model.set('constr_Jbu', Jbu); - ocp_model.set('constr_lbu', lbu); - ocp_model.set('constr_ubu', ubu); -end -% ocp_model.model_struct - -%% acados ocp opts -ocp_opts = acados_ocp_opts(); -ocp_opts.set('compile_interface', compile_interface); -ocp_opts.set('codgen_model', codgen_model); -ocp_opts.set('param_scheme_N', ocp_N); -ocp_opts.set('nlp_solver', nlp_solver); -ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); -ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('ext_fun_compile_flags', ''); - -tol_sqp = 1e-10; -if (strcmp(nlp_solver, 'sqp')) - ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); - ocp_opts.set('nlp_solver_tol_stat', tol_sqp); - ocp_opts.set('nlp_solver_tol_eq', tol_sqp); - ocp_opts.set('nlp_solver_tol_ineq', tol_sqp); - ocp_opts.set('nlp_solver_tol_comp', tol_sqp); -end -ocp_opts.set('qp_solver', qp_solver); -if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); - ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); - ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); - ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); -end -ocp_opts.set('sim_method', ocp_sim_method); -ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); -ocp_opts.set('sim_method_num_steps', ocp_sim_method_num_steps); -ocp_opts.set('sim_method_newton_iter', ocp_sim_method_newton_iter); -% ocp_opts.opts_struct - - -%% acados ocp -ocp = acados_ocp(ocp_model, ocp_opts); - - -%% acados sim model -sim_model = acados_sim_model(); -sim_model.set('name', model_name); -sim_model.set('T', h); % simulation time - -sim_model.set('sym_x', model.sym_x); -if isfield(model, 'sym_u') - sim_model.set('sym_u', model.sym_u); -end -if isfield(model, 'sym_p') - sim_model.set('sym_p', model.sym_p); -end - -% Note: DAEs can only be used with implicit integrator -sim_model.set('dyn_type', 'implicit'); -sim_model.set('dyn_expr_f', model.expr_f_impl); -sim_model.set('sym_xdot', model.sym_xdot); -if isfield(model, 'sym_z') - sim_model.set('sym_z', model.sym_z); -end - -%% acados sim opts -sim_opts = acados_sim_opts(); -sim_opts.set('compile_interface', compile_interface); -sim_opts.set('codgen_model', codgen_model); -sim_opts.set('num_stages', sim_num_stages); -sim_opts.set('num_steps', sim_num_steps); -sim_opts.set('newton_iter', sim_newton_iter); -sim_opts.set('method', sim_method); -sim_opts.set('sens_forw', sim_sens_forw); -sim_opts.set('sens_adj', 'true'); -sim_opts.set('sens_algebraic', 'true'); -sim_opts.set('output_z', 'true'); -sim_opts.set('sens_hess', 'false'); -sim_opts.set('jac_reuse', sim_jac_reuse); -if (strcmp(sim_method, 'irk_gnsf')) - sim_opts.set('gnsf_detect_struct', gnsf_detect_struct); -end - - -%% acados sim -% create integrator -sim = acados_sim(sim_model, sim_opts); - -%% closed loop simulation -N_sim = 99; - -x_sim = zeros(nx, N_sim+1); -x_sim(:,1) = x0; % initial state -u_sim = zeros(nu, N_sim); -z_sim = zeros(nz, N_sim); - -% initialization -xdot0 = zeros(nx, 1); -z0 = zeros(nz, 1); - -% set trajectory initialization -x_traj_init = repmat(x0, 1, ocp_N + 1); -u_traj_init = zeros(nu, ocp_N); -pi_traj_init = zeros(nx, ocp_N); -z_traj_init = 0*ones(nz, ocp_N); -xdot_traj_init = 0*ones(nx, ocp_N); -if strcmp(ocp_sim_method, 'irk_gnsf') - gnsf_init_traj = zeros(ocp.model_struct.dim_gnsf_nout, ocp_N); -end - -sqp_iter = zeros(N_sim,1); -sqp_time = zeros(N_sim,1); - -tic -for ii=1:N_sim - - % set initial state - ocp.set('constr_x0', x_sim(:,ii)); - % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); - if ii == 1 - if strcmp(ocp_sim_method, 'irk') - ocp.set('init_z', z_traj_init); - ocp.set('init_xdot', xdot_traj_init); - elseif strcmp(ocp_sim_method, 'irk_gnsf') - ocp.set('init_gnsf_phi', gnsf_init_traj); - end - end - - ocp.solve(); -% ocp.print('stat') - - status = ocp.get('status'); - sqp_iter(ii) = ocp.get('sqp_iter'); - sqp_time(ii) = ocp.get('time_tot'); - if status ~= 0 - error('ocp solver returned status nonzero'); - end - - % get solution for initialization of next NLP - x_traj = ocp.get('x'); - u_traj = ocp.get('u'); - pi_traj = ocp.get('pi'); - z_traj = ocp.get('z'); - - % shift trajectory for initialization - x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; - u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; - pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; - z_traj_init = [z_traj(:,2:end), z_traj(:,end)]; - - u_sim(:,ii) = ocp.get('u', 0); % get control input - % initialize implicit integrator - if (strcmp(sim_method, 'irk')) - sim.set('xdot', xdot0); - sim.set('z', z0); - elseif (strcmp(sim_method, 'irk_gnsf')) - import casadi.* - x01_gnsf = x0(sim.model_struct.dyn_gnsf_idx_perm_x(1:sim.model_struct.dim_gnsf_nx1)); - x01_dot_gnsf = xdot0(sim.model_struct.dyn_gnsf_idx_perm_x(1:sim.model_struct.dim_gnsf_nx1)); - z0_gnsf = z0(sim.model_struct.dyn_gnsf_idx_perm_z( 1:sim.model_struct.dim_gnsf_nz1 )); - y_in = sim.model_struct.dyn_gnsf_L_x * x01_gnsf ... - + sim.model_struct.dyn_gnsf_L_xdot * x01_dot_gnsf ... - + sim.model_struct.dyn_gnsf_L_z * z0_gnsf; - u_hat = sim.model_struct.dyn_gnsf_L_u * u_sim(:,ii); - phi_fun = Function([model_name,'_gnsf_phi_fun'],... - {sim.model_struct.sym_gnsf_y, sim.model_struct.sym_gnsf_uhat},... - {sim.model_struct.dyn_gnsf_expr_phi(:)}); % sim.model_struct.sym_p - - phi_guess = full( phi_fun( y_in, u_hat ) ); - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', phi_guess(n_out,1)); - end - - sim.set('x', x_sim(:,ii)); % set initial state - sim.set('u', u_sim(:,ii)); % set input - sim.solve(); % simulate state - - % get simulated state - x_sim(:,ii+1) = sim.get('xn'); - z_sim(:,ii) = sim.get('zn'); - - -end -format short e - -toc - -if 0 - % trajectory plot - figure; - subplot(4, 1, 1); - plot(1:N_sim+1, x_sim(1:2,:)); - legend('xpos', 'ypos'); - - subplot(4, 1, 2); - plot(1:N_sim+1, x_sim(3,:)); - legend('alpha') - - subplot(4, 1, 3); - plot(1:N_sim+1, x_sim(4:6,:)); - legend('vx', 'vy', 'valpha'); - - subplot(4,1,4); - plot(1:N_sim+1, [0 u_sim]); - legend('u') - - if is_octave() - waitforbuttonpress; - end - - % iterations, CPU time - figure(); - subplot(2, 1, 1); - plot(1:N_sim, sqp_iter) - ylabel('# iterations') - xlabel('ocp instance') - subplot(2, 1, 2); - plot(1:N_sim, sqp_time) - ylabel('CPU time') - xlabel('ocp instance') - if is_octave() - waitforbuttonpress; - end -end - -% check consistency -xp = x_sim(1,:); -yp = x_sim(2,:); -check = abs(xp.^2 + yp.^2 - length_pendulum^2); -tol_pendulum = 1e-10; - -dist2target = norm( sim.get('xn') - xtarget ); -%requ_dist2target = 1e-4; -requ_dist2target = 1e-3; - - -fprintf(['test_ocp_pendulum_dae: check for constant pendulum length.\nDeviation by maximum of\t', ... - num2str(max(abs(check))), '\n']); -if any( max(abs(check)) > tol_pendulum ) - error(['test_ocp_pendulum_dae: check for constant pendulum length failed, violation' ... - num2str(max(abs(check))), '>', num2str(tol_pendulum)]); -elseif dist2target > requ_dist2target - error(['test_ocp_pendulum_dae: system should have reached desired state up to accuracy ' ... - num2str(requ_dist2target,'%e') ' is ' num2str(dist2target,'%e')]); -else - disp('test_ocp_pendulum_dae: SUCCESS'); -end - -% eval constraint h -ax_ = z_sim(1,:); -ay_ = z_sim(2,:); -h_vals = ax_.^2 + ay_.^2; -h_violations = [ uh - h_vals; h_vals - lh ]; -max_h_violation = max(abs( h_violations( h_violations < 0 ))); -if isempty(max_h_violation) - max_h_violation = 0; -end -disp(['maximal constraint h violation ' num2str( max_h_violation, '%e' ) ]) - 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 424d635d19..c36dca0fdf 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 @@ -227,7 +227,7 @@ %% acados ocp % create ocp - ocp = acados_ocp(ocp_model, ocp_opts); + ocp_solver = acados_ocp(ocp_model, ocp_opts); % set trajectory initialization %x_traj_init = zeros(nx, N+1); @@ -235,49 +235,49 @@ x_traj_init = [linspace(0, 0, N+1); linspace(pi, 0, N+1); linspace(0, 0, N+1); linspace(0, 0, N+1)]; u_traj_init = zeros(nu, N); - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); % change number of sqp iterations - ocp.set('nlp_solver_max_iter', 20); + ocp_solver.set('nlp_solver_max_iter', 20); % modify numerical data for a certain stage some_stages = 1:10:N-1; for i = some_stages if (strcmp(cost_type, 'linear_ls')) - ocp.set('cost_Vx', Vx, i); % cost_y_ref, cost_Vu, cost_Vx, cost_W, cost_Z, cost_Zl,... + ocp_solver.set('cost_Vx', Vx, i); % cost_y_ref, cost_Vu, cost_Vx, cost_W, cost_Z, cost_Zl,... % cost_Zu, cost_z, cost_zl, cost_zu; - ocp.set('cost_Vu', Vu, i); - ocp.set('cost_y_ref', yr, i); + ocp_solver.set('cost_Vu', Vu, i); + ocp_solver.set('cost_y_ref', yr, i); end if ng > 0 - ocp.set('constr_C', C, i); - ocp.set('constr_D', D, i); - ocp.set('constr_ug', ubu, i); - ocp.set('constr_lg', lbu, i); + ocp_solver.set('constr_C', C, i); + ocp_solver.set('constr_D', D, i); + ocp_solver.set('constr_ug', ubu, i); + ocp_solver.set('constr_lg', lbu, i); end end % solve tic; - ocp.solve(); + ocp_solver.solve(); time_ext=toc; % get solution - utraj = ocp.get('u'); - xtraj = ocp.get('x'); + utraj = ocp_solver.get('u'); + xtraj = ocp_solver.get('x'); %% evaluation - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot = ocp.get('time_tot'); - time_lin = ocp.get('time_lin'); - time_reg = ocp.get('time_reg'); - time_qp_sol = ocp.get('time_qp_sol'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot = ocp_solver.get('time_tot'); + time_lin = ocp_solver.get('time_lin'); + time_reg = ocp_solver.get('time_reg'); + time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); - stat = ocp.get('stat'); - ocp.print('stat') + stat = ocp_solver.get('stat'); + ocp_solver.print('stat') if itest == 1 utraj_ref = utraj; diff --git a/examples/acados_matlab_octave/test/test_ocp_qpdunes.m b/examples/acados_matlab_octave/test/test_ocp_qpdunes.m index a771cd7861..f9b0955761 100644 --- a/examples/acados_matlab_octave/test/test_ocp_qpdunes.m +++ b/examples/acados_matlab_octave/test/test_ocp_qpdunes.m @@ -125,33 +125,33 @@ % ... see ocp_opts.opts_struct to see what other fields can be set %% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); x_traj_init = zeros(nx, N+1); u_traj_init = zeros(nu, N); %% call ocp solver % update initial state -ocp.set('constr_x0', x0); +ocp_solver.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_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); +ocp_solver.set('init_pi', zeros(nx, N)) % change values for specific shooting node using: -% ocp.set('field', value, optional: stage_index) -ocp.set('constr_lbx', x0, 0) +% ocp_solver.set('field', value, optional: stage_index) +ocp_solver.set('constr_lbx', x0, 0) % solve -ocp.solve(); +ocp_solver.solve(); % get solution -utraj = ocp.get('u'); -xtraj = ocp.get('x'); +utraj = ocp_solver.get('u'); +xtraj = ocp_solver.get('x'); -status = ocp.get('status'); % 0 - success -ocp.print('stat') +status = ocp_solver.get('status'); % 0 - success +ocp_solver.print('stat') if status == 0 disp('test_ocp_qpDUNES: success!'); diff --git a/examples/acados_matlab_octave/test/test_ocp_simple_dae.m b/examples/acados_matlab_octave/test/test_ocp_simple_dae.m index 1bd0f0076b..8c9a950694 100644 --- a/examples/acados_matlab_octave/test/test_ocp_simple_dae.m +++ b/examples/acados_matlab_octave/test/test_ocp_simple_dae.m @@ -192,28 +192,28 @@ %% acados ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); -ocp.reset(); -ocp.solve(); +ocp_solver.reset(); +ocp_solver.solve(); -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); -ocp.print('stat') +ocp_solver.print('stat') -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -sqp_time = ocp.get('time_tot'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +sqp_time = ocp_solver.get('time_tot'); %if status ~= 0 % keyboard %end format short e % get solution for initialization of next NLP -x_traj = ocp.get('x'); -u_traj = ocp.get('u'); -pi_traj = ocp.get('pi'); -z_traj = ocp.get('z'); +x_traj = ocp_solver.get('x'); +u_traj = ocp_solver.get('u'); +pi_traj = ocp_solver.get('pi'); +z_traj = ocp_solver.get('z'); diff_x_z = x_traj(:,1:N) - z_traj; max_diff_x_z = max(max(abs(diff_x_z))); diff --git a/examples/acados_matlab_octave/test/test_ocp_wtnx6.m b/examples/acados_matlab_octave/test/test_ocp_wtnx6.m index 71fff340ca..9d1cf62226 100644 --- a/examples/acados_matlab_octave/test/test_ocp_wtnx6.m +++ b/examples/acados_matlab_octave/test/test_ocp_wtnx6.m @@ -298,9 +298,9 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %ocp -%ocp.C_ocp +%ocp_solver.C_ocp %% acados sim model sim_model = acados_sim_model(); @@ -342,7 +342,7 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation @@ -373,51 +373,51 @@ tic % set x0 - ocp.set('constr_x0', x_sim(:,ii)); + ocp_solver.set('constr_x0', x_sim(:,ii)); % set parameter for jj=0:ocp_N-1 - ocp.set('p', wind0_ref(:,ii+jj), jj); + ocp_solver.set('p', wind0_ref(:,ii+jj), jj); end % set reference (different at each stage) for jj=0:ocp_N-1 - ocp.set('cost_y_ref', y_ref(:,ii+jj), jj); + ocp_solver.set('cost_y_ref', y_ref(:,ii+jj), jj); end - ocp.set('cost_y_ref_e', y_ref(1:ny_e,ii+ocp_N)); + ocp_solver.set('cost_y_ref_e', y_ref(1:ny_e,ii+ocp_N)); % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', pi_traj_init); % solve - ocp.solve(); + ocp_solver.solve(); % get solution - x = ocp.get('x'); - u = ocp.get('u'); - pi = ocp.get('pi'); + x = ocp_solver.get('x'); + u = ocp_solver.get('u'); + pi = ocp_solver.get('pi'); % store first input - u_sim(:,ii) = ocp.get('u', 0); + u_sim(:,ii) = ocp_solver.get('u', 0); % set initial state of sim - sim.set('x', x_sim(:,ii)); + sim_solver.set('x', x_sim(:,ii)); % set input in sim - sim.set('u', u_sim(:,ii)); + sim_solver.set('u', u_sim(:,ii)); % set parameter - sim.set('p', wind0_ref(:,ii)); + sim_solver.set('p', wind0_ref(:,ii)); % simulate state - sim.solve(); + sim_solver.solve(); % get new state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); % shift trajectory for initialization % x_traj_init = [x(:,2:ocp_N+1), zeros(nx, 1)]; x_traj_init = [x(:,2:ocp_N+1), x(:,ocp_N+1)]; -% x_traj_init = [x(:,2:ocp_N+1), sim.get('xn')]; +% x_traj_init = [x(:,2:ocp_N+1), sim_solver.get('xn')]; % u_traj_init = [u(:,2:ocp_N), zeros(nu, 1)]; u_traj_init = [u(:,2:ocp_N), u(:,ocp_N)]; pi_traj_init = [pi(:,2:ocp_N), pi(:,ocp_N)]; @@ -426,17 +426,17 @@ electrical_power = 0.944*97/100*x(1,1)*x(6,1); - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot(ii) = ocp.get('time_tot'); - time_lin(ii) = ocp.get('time_lin'); - time_qp_sol(ii) = ocp.get('time_qp_sol'); - sqp_stats = ocp.get('stat'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot(ii) = ocp_solver.get('time_tot'); + time_lin(ii) = ocp_solver.get('time_lin'); + time_qp_sol(ii) = ocp_solver.get('time_qp_sol'); + sqp_stats = ocp_solver.get('stat'); qp_iter = sqp_stats(:,7); sqp_iter_sim(ii) = sqp_iter; if status ~= 0 - ocp.print() + ocp_solver.print() error(['ocp_nlp solver returned status ', num2str(status), '!= 0 in simulation instance ', num2str(ii)]); end @@ -444,26 +444,26 @@ status, sqp_iter, sum(qp_iter), time_ext(ii)*1e3, time_tot(ii)*1e3, time_lin(ii)*1e3, time_qp_sol(ii)*1e3, electrical_power); if 0 - ocp.print('stat') + ocp_solver.print('stat') end end % test setter -ocp.set('cost_z', ones(2,1), 1) -ocp.set('cost_Z', ones(2,1), 1) -ocp.set('cost_zl', ones(2,1), N-1) +ocp_solver.set('cost_z', ones(2,1), 1) +ocp_solver.set('cost_Z', ones(2,1), 1) +ocp_solver.set('cost_zl', ones(2,1), N-1) % get slack values for i = 0:N-1 - sl = ocp.get('sl', i); - su = ocp.get('su', i); + sl = ocp_solver.get('sl', i); + su = ocp_solver.get('su', i); % test setters - ocp.set('sl', sl, i); - ocp.set('su', su, i); + ocp_solver.set('sl', sl, i); + ocp_solver.set('su', su, i); end -sl = ocp.get('sl', N); -su = ocp.get('su', N); +sl = ocp_solver.get('sl', N); +su = ocp_solver.get('su', N); diff --git a/examples/acados_matlab_octave/test/test_sens_adj.m b/examples/acados_matlab_octave/test/test_sens_adj.m index df02933c70..b42884d56e 100644 --- a/examples/acados_matlab_octave/test/test_sens_adj.m +++ b/examples/acados_matlab_octave/test/test_sens_adj.m @@ -90,21 +90,21 @@ %% acados sim % create sim - sim = acados_sim(sim_model, sim_opts); + sim_solver = acados_sim(sim_model, sim_opts); % Note: this does not work with gnsf, because it needs to be available % in the precomputation phase - % sim.set('T', Ts); + % sim_solver.set('T', Ts); % set initial state - sim.set('x', x0); - sim.set('u', u); + sim_solver.set('x', x0); + sim_solver.set('u', u); % solve - sim.solve(); + sim_solver.solve(); - xn = sim.get('xn'); - S_forw_ind = sim.get('S_forw'); + xn = sim_solver.get('xn'); + S_forw_ind = sim_solver.get('S_forw'); %% compute forward sensitivities through adjoint sensitivities with unit seeds % sim_opts.set('sens_forw', 'false'); @@ -115,11 +115,11 @@ % set seed dx = zeros(nx, 1); dx(ii) = 1.0; - sim.set('seed_adj', dx); + sim_solver.set('seed_adj', dx); - sim.solve(); + sim_solver.solve(); - S_adj = sim.get('S_adj'); + S_adj = sim_solver.get('S_adj'); S_forw_adj(ii,:) = S_adj; end diff --git a/examples/acados_matlab_octave/test/test_sens_forw.m b/examples/acados_matlab_octave/test/test_sens_forw.m index b7456c95f1..75da6267b3 100644 --- a/examples/acados_matlab_octave/test/test_sens_forw.m +++ b/examples/acados_matlab_octave/test/test_sens_forw.m @@ -105,29 +105,29 @@ %% acados sim % create sim - sim = acados_sim(sim_model, sim_opts); + sim_solver = acados_sim(sim_model, sim_opts); % Note: this does not work with gnsf, because it needs to be available % in the precomputation phase - % sim.set('T', Ts); + % sim_solver.set('T', Ts); % set initial state - sim.set('x', x0); - sim.set('u', u); + sim_solver.set('x', x0); + sim_solver.set('u', u); % initialize implicit integrator if (strcmp(method, 'irk')) - sim.set('xdot', zeros(nx,1)); + sim_solver.set('xdot', zeros(nx,1)); elseif (strcmp(method, 'irk_gnsf')) - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end % solve - sim.solve(); + sim_solver.solve(); - xn = sim.get('xn'); - S_forw_ind = sim.get('S_forw'); + xn = sim_solver.get('xn'); + S_forw_ind = sim_solver.get('S_forw'); %% compute forward sensitivities using finite differences sim_opts.set('sens_forw', 'false'); @@ -139,11 +139,11 @@ dx = zeros(nx, 1); dx(ii) = 1.0; - sim.set('x', x0+FD_epsilon*dx); - sim.set('u', u); - sim.solve(); + sim_solver.set('x', x0+FD_epsilon*dx); + sim_solver.set('u', u); + sim_solver.solve(); - xn_tmp = sim.get('xn'); + xn_tmp = sim_solver.get('xn'); S_forw_fd(:,ii) = (xn_tmp - xn) / FD_epsilon; end @@ -151,11 +151,11 @@ du = zeros(nu, 1); du(ii) = 1.0; - sim.set('x', x0); - sim.set('u', u+FD_epsilon*du); - sim.solve(); + sim_solver.set('x', x0); + sim_solver.set('u', u+FD_epsilon*du); + sim_solver.solve(); - xn_tmp = sim.get('xn'); + xn_tmp = sim_solver.get('xn'); S_forw_fd(:,nx+ii) = (xn_tmp - xn) / FD_epsilon; end diff --git a/examples/acados_matlab_octave/test/test_sens_hess.m b/examples/acados_matlab_octave/test/test_sens_hess.m index 79beabf0d4..164b3bc271 100644 --- a/examples/acados_matlab_octave/test/test_sens_hess.m +++ b/examples/acados_matlab_octave/test/test_sens_hess.m @@ -97,7 +97,7 @@ %% acados sim % create sim - sim = acados_sim(sim_model, sim_opts); + sim_solver = acados_sim(sim_model, sim_opts); % compute hessian sensitivities using internal numerical differentiation S_hess_ind = zeros(nx+nu, nx+nu, nx); @@ -107,34 +107,34 @@ for jj=1:nx % loop over unit seeds % set initial state - sim.set('x', x0); - sim.set('u', u); + sim_solver.set('x', x0); + sim_solver.set('u', u); % internal numerical differentiation seed lambda = zeros(nx, 1); lambda(jj) = 1.0; - sim.set('seed_adj', lambda); + sim_solver.set('seed_adj', lambda); % solve - sim.solve(); + sim_solver.solve(); % S_hess - S_hess = sim.get('S_hess'); + S_hess = sim_solver.get('S_hess'); S_hess_ind(:, :, jj) = S_hess; % S_adj - S_adj = sim.get('S_adj'); + S_adj = sim_solver.get('S_adj'); %% asymmetric finite differences for ii=1:nx dx = zeros(nx, 1); dx(ii) = 1.0; - sim.set('x', x0+FD_epsilon*dx); - sim.set('u', u); + sim_solver.set('x', x0+FD_epsilon*dx); + sim_solver.set('u', u); - sim.solve(); - S_adj_tmp = sim.get('S_adj'); + sim_solver.solve(); + S_adj_tmp = sim_solver.get('S_adj'); S_hess_fd(:, ii, jj) = (S_adj_tmp - S_adj) / FD_epsilon; end @@ -143,11 +143,11 @@ du = zeros(nu, 1); du(ii) = 1.0; - sim.set('x', x0); - sim.set('u', u+FD_epsilon*du); + sim_solver.set('x', x0); + sim_solver.set('u', u+FD_epsilon*du); - sim.solve(); - S_adj_tmp = sim.get('S_adj'); + sim_solver.solve(); + S_adj_tmp = sim_solver.get('S_adj'); S_hess_fd(:, nx+ii, jj) = (S_adj_tmp - S_adj) / FD_epsilon; end end diff --git a/examples/acados_matlab_octave/test/test_sim_dae.m b/examples/acados_matlab_octave/test/test_sim_dae.m index ddb47ae2dc..e1a3525d70 100644 --- a/examples/acados_matlab_octave/test/test_sim_dae.m +++ b/examples/acados_matlab_octave/test/test_sim_dae.m @@ -110,12 +110,12 @@ %% acados sim % create integrator - sim = acados_sim(sim_model, sim_opts); + sim_solver = acados_sim(sim_model, sim_opts); N_sim = 100; % set initial state - sim.set('x', x0); - sim.set('u', u); + sim_solver.set('x', x0); + sim_solver.set('u', u); x_sim = zeros(nx, N_sim+1); x_sim(:,1) = x0; @@ -124,32 +124,32 @@ for ii=1:N_sim % set initial state - sim.set('x', x_sim(:,ii)); - sim.set('u', u); + sim_solver.set('x', x_sim(:,ii)); + sim_solver.set('u', u); % set adjoint seed - sim.set('seed_adj', ones(nx,1)); + sim_solver.set('seed_adj', ones(nx,1)); % initialize implicit integrator if (strcmp(method, 'irk')) - sim.set('xdot', zeros(nx,1)); - sim.set('z', zeros(nz,1)); + sim_solver.set('xdot', zeros(nx,1)); + sim_solver.set('z', zeros(nz,1)); elseif (strcmp(method, 'irk_gnsf')) - n_out = sim.model_struct.dim_gnsf_nout; - sim.set('phi_guess', zeros(n_out,1)); + n_out = sim_solver.model_struct.dim_gnsf_nout; + sim_solver.set('phi_guess', zeros(n_out,1)); end % solve - sim.solve(); + sim_solver.solve(); % get simulated state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); end - S_forw = sim.get('S_forw'); - S_adj = sim.get('S_adj')'; - z = sim.get('zn')'; % approximate value of algebraic variables at start of simulation - S_alg = sim.get('S_algebraic'); % sensitivities of algebraic variables z + S_forw = sim_solver.get('S_forw'); + S_adj = sim_solver.get('S_adj')'; + z = sim_solver.get('zn')'; % approximate value of algebraic variables at start of simulation + S_alg = sim_solver.get('S_algebraic'); % sensitivities of algebraic variables z required_accuracy = 1e-13; if i_method == 1 diff --git a/examples/acados_matlab_octave/test/test_target_selector.m b/examples/acados_matlab_octave/test/test_target_selector.m index e203fc98c6..daa290697b 100644 --- a/examples/acados_matlab_octave/test/test_target_selector.m +++ b/examples/acados_matlab_octave/test/test_target_selector.m @@ -139,8 +139,8 @@ simulink_opts.outputs.CPU_time = 0; simulink_opts.outputs.x1 = 0; -%% create ocp solver -ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts); +% create solver +ocp_solver = acados_ocp(ocp_model, ocp_opts, simulink_opts); % initialize % 2 working initializations @@ -148,17 +148,17 @@ % x_traj_init = repmat(x_ipopt, 1, 2) + eps * ones(nx, N+1); x_traj_init = repmat(StatesAndInputs0, 1, 2); -%% call ocp solver % set trajectory initialization -ocp.set('init_x', x_traj_init); -% ocp.set('init_pi', zeros(nx, N)) +ocp_solver.set('init_x', x_traj_init); +% ocp_solver.set('init_pi', zeros(nx, N)) -ocp.solve(); -disp(['acados ocp solver returned status ', ocp.get('status')]); % 0 - success -ocp.print('stat') +% call ocp solver +ocp_solver.solve(); +disp(['acados ocp solver returned status ', ocp_solver.get('status')]); % 0 - success +ocp_solver.print('stat') % get solution -x_acados = ocp.get('x', 0); +x_acados = ocp_solver.get('x', 0); % [x_acados, x_ipopt] diff_acados_ipopt = norm(x_acados-x_ipopt) diff --git a/examples/acados_matlab_octave/wind_turbine_nx6/example_closed_loop.m b/examples/acados_matlab_octave/wind_turbine_nx6/example_closed_loop.m index 7549ea891f..a5da9e5b73 100644 --- a/examples/acados_matlab_octave/wind_turbine_nx6/example_closed_loop.m +++ b/examples/acados_matlab_octave/wind_turbine_nx6/example_closed_loop.m @@ -297,7 +297,7 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %% acados sim model sim_model = acados_sim_model(); @@ -340,7 +340,7 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); %% closed loop simulation @@ -367,68 +367,68 @@ tic % set x0 - ocp.set('constr_x0', x_sim(:,ii)); + ocp_solver.set('constr_x0', x_sim(:,ii)); % set parameter for jj=0:ocp_N-1 - ocp.set('p', wind0_ref(:,ii+jj), jj); + ocp_solver.set('p', wind0_ref(:,ii+jj), jj); end % set reference (different at each stage) for jj=0:ocp_N-1 - ocp.set('cost_y_ref', y_ref(:,ii+jj), jj); + ocp_solver.set('cost_y_ref', y_ref(:,ii+jj), jj); end - ocp.set('cost_y_ref_e', y_ref(1:ny_e,ii+ocp_N)); + ocp_solver.set('cost_y_ref_e', y_ref(1:ny_e,ii+ocp_N)); % set trajectory initialization (if not, set internally using previous solution) - ocp.set('init_x', x_traj_init); - ocp.set('init_u', u_traj_init); - ocp.set('init_pi', pi_traj_init); + ocp_solver.set('init_x', x_traj_init); + ocp_solver.set('init_u', u_traj_init); + ocp_solver.set('init_pi', pi_traj_init); % solve - ocp.solve(); + ocp_solver.solve(); % get solution - x = ocp.get('x'); - u = ocp.get('u'); - pi = ocp.get('pi'); + x = ocp_solver.get('x'); + u = ocp_solver.get('u'); + pi = ocp_solver.get('pi'); -%ocp.print('stat'); +%ocp_solver.print('stat'); %return % store first input - u_sim(:,ii) = ocp.get('u', 0); + u_sim(:,ii) = ocp_solver.get('u', 0); % set initial state of sim - sim.set('x', x_sim(:,ii)); + sim_solver.set('x', x_sim(:,ii)); % set input in sim - sim.set('u', u_sim(:,ii)); + sim_solver.set('u', u_sim(:,ii)); % set parameter - sim.set('p', wind0_ref(:,ii)); + sim_solver.set('p', wind0_ref(:,ii)); % simulate state - sim.solve(); + sim_solver.solve(); % get new state - x_sim(:,ii+1) = sim.get('xn'); + x_sim(:,ii+1) = sim_solver.get('xn'); % x_sim(:,ii+1) = x(:,2); -% (x(:,2) - sim.get('xn'))' +% (x(:,2) - sim_solver.get('xn'))' % simulate to initialize last stage % set initial state of sim -% sim.set('x', x(:,ocp_N+1)); +% sim_solver.set('x', x(:,ocp_N+1)); % set input in sim -% sim.set('u', zeros(nu, 1)); -% sim.set('u', u(:,ocp_N)); +% sim_solver.set('u', zeros(nu, 1)); +% sim_solver.set('u', u(:,ocp_N)); % set parameter -% sim.set('p', wind0_ref(:,ii+ocp_N)); +% sim_solver.set('p', wind0_ref(:,ii+ocp_N)); % simulate state -% sim.solve(); +% sim_solver.solve(); % shift trajectory for initialization % x_traj_init = [x(:,2:ocp_N+1), zeros(nx, 1)]; x_traj_init = [x(:,2:ocp_N+1), x(:,ocp_N+1)]; -% x_traj_init = [x(:,2:ocp_N+1), sim.get('xn')]; +% x_traj_init = [x(:,2:ocp_N+1), sim_solver.get('xn')]; % u_traj_init = [u(:,2:ocp_N), zeros(nu, 1)]; u_traj_init = [u(:,2:ocp_N), u(:,ocp_N)]; pi_traj_init = [pi(:,2:ocp_N), pi(:,ocp_N)]; @@ -440,11 +440,11 @@ electrical_power = 0.944*97/100*x(1,1)*x(6,1); - status = ocp.get('status'); - sqp_iter = ocp.get('sqp_iter'); - time_tot = ocp.get('time_tot'); - time_lin = ocp.get('time_lin'); - time_qp_sol = ocp.get('time_qp_sol'); + status = ocp_solver.get('status'); + sqp_iter = ocp_solver.get('sqp_iter'); + time_tot = ocp_solver.get('time_tot'); + time_lin = ocp_solver.get('time_lin'); + time_qp_sol = ocp_solver.get('time_qp_sol'); sqp_iter_sim(ii) = sqp_iter; @@ -452,7 +452,7 @@ status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, electrical_power); if 0 - ocp.print('stat') + ocp_solver.print('stat') end end diff --git a/examples/acados_matlab_octave/wind_turbine_nx6/example_ocp.m b/examples/acados_matlab_octave/wind_turbine_nx6/example_ocp.m index 07de89e76f..dc486b850a 100644 --- a/examples/acados_matlab_octave/wind_turbine_nx6/example_ocp.m +++ b/examples/acados_matlab_octave/wind_turbine_nx6/example_ocp.m @@ -282,9 +282,9 @@ %% acados ocp % create ocp -ocp = acados_ocp(ocp_model, ocp_opts); +ocp_solver = acados_ocp(ocp_model, ocp_opts); %ocp -%ocp.C_ocp +%ocp_solver.C_ocp %% solution @@ -297,28 +297,28 @@ tic -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +ocp_solver.set('init_x', x_traj_init); +ocp_solver.set('init_u', u_traj_init); % set x0 -ocp.set('constr_x0', x0_ref); +ocp_solver.set('constr_x0', x0_ref); % set parameter nn = 1; -ocp.set('p', wind0_ref(:,nn)); +ocp_solver.set('p', wind0_ref(:,nn)); % set reference -ocp.set('cost_y_ref', y_ref(:,nn)); -ocp.set('cost_y_ref_e', y_ref(1:ny_e,nn)); +ocp_solver.set('cost_y_ref', y_ref(:,nn)); +ocp_solver.set('cost_y_ref_e', y_ref(1:ny_e,nn)); % solve disp('before solve') -ocp.solve(); +ocp_solver.solve(); disp('after solve') % get solution -u = ocp.get('u'); -x = ocp.get('x'); +u = ocp_solver.get('u'); +x = ocp_solver.get('x'); time_ext = toc; @@ -330,16 +330,16 @@ % statistics -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_reg = ocp.get('time_reg'); -time_qp_sol = ocp.get('time_qp_sol'); +status = ocp_solver.get('status'); +sqp_iter = ocp_solver.get('sqp_iter'); +time_tot = ocp_solver.get('time_tot'); +time_lin = ocp_solver.get('time_lin'); +time_reg = ocp_solver.get('time_reg'); +time_qp_sol = ocp_solver.get('time_qp_sol'); fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); -ocp.print('stat'); +ocp_solver.print('stat'); % figures @@ -365,7 +365,7 @@ ylabel('electrical power'); %legend('F'); -stat = ocp.get('stat'); +stat = ocp_solver.get('stat'); if (strcmp(nlp_solver, 'sqp')) figure; plot([0: size(stat,1)-1], log10(stat(:,2)), 'r-x'); diff --git a/examples/acados_matlab_octave/wind_turbine_nx6/example_sim.m b/examples/acados_matlab_octave/wind_turbine_nx6/example_sim.m index 6fda313b3a..5c8f0ca669 100644 --- a/examples/acados_matlab_octave/wind_turbine_nx6/example_sim.m +++ b/examples/acados_matlab_octave/wind_turbine_nx6/example_sim.m @@ -96,7 +96,7 @@ %% acados sim % create sim -sim = acados_sim(sim_model, sim_opts); +sim_solver = acados_sim(sim_model, sim_opts); % to avoid unstable behavior introduce a small pi-contorller for rotor speed tracking uctrl = 0.0; @@ -118,14 +118,14 @@ u(2) = max(u(2) - uctrl, 0); % update state, input, parameter - sim.set('x', x_sim(:,nn)); - sim.set('u', u); - sim.set('p', Usim(nn,3)); + sim_solver.set('x', x_sim(:,nn)); + sim_solver.set('u', u); + sim_solver.set('p', Usim(nn,3)); % solve - sim.solve(); + sim_solver.solve(); - x_sim(:,nn+1) = sim.get('xn'); + x_sim(:,nn+1) = sim_solver.get('xn'); % update PI contoller ctrlErr = statesFAST(nn+1,1) - x_sim(1,nn+1); @@ -139,9 +139,9 @@ %statesFAST(1:nsim+1,:)' x_sim(:,1:nsim+1) -%S_forw = sim.get('S_forw'); -%Sx = sim.get('Sx'); -%Su = sim.get('Su'); +%S_forw = sim_solver.get('S_forw'); +%Sx = sim_solver.get('Sx'); +%Su = sim_solver.get('Su'); fprintf('\nsuccess!\n\n'); diff --git a/examples/acados_matlab_octave/wind_turbine_nx6/example_simulink.m b/examples/acados_matlab_octave/wind_turbine_nx6/example_simulink.m index edd38f82ef..da0403b5e3 100644 --- a/examples/acados_matlab_octave/wind_turbine_nx6/example_simulink.m +++ b/examples/acados_matlab_octave/wind_turbine_nx6/example_simulink.m @@ -5,11 +5,6 @@ % example_closed_loop; - -%% Render templated Code for the model contained in ocp object -% -ocp.generate_c_code; - %% Compile Sfunctions cd c_generated_code diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index f899273eb1..bcdd4d484d 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -59,9 +59,6 @@ if(ACADOS_OCTAVE) add_test(NAME octave_test_ocp_wtnx6 COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test octave --no-gui --no-window-system ./run_test_ocp_wtnx6.m) - add_test(NAME octave_test_ocp_pendulum_dae - COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test - octave --no-gui --no-window-system ./run_test_ocp_pendulum_dae.m) add_test(NAME octave_test_ocp_mass_spring COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_matlab_octave/test octave --no-gui --no-window-system ./run_test_ocp_mass_spring.m) @@ -100,9 +97,8 @@ if(ACADOS_OCTAVE) set_tests_properties(octave_test_sim_hess PROPERTIES DEPENDS octave_test_sim_dae) set_tests_properties(octave_test_sim_dae PROPERTIES DEPENDS octave_test_ocp_pendulum) set_tests_properties(octave_test_ocp_pendulum PROPERTIES DEPENDS octave_test_ocp_wtnx6) - set_tests_properties(octave_test_ocp_wtnx6 PROPERTIES DEPENDS octave_test_ocp_pendulum_dae) - set_tests_properties(octave_test_ocp_pendulum_dae PROPERTIES DEPENDS octave_test_ocp_mass_spring) - set_tests_properties(octave_test_ocp_mass_spring PROPERTIES DEPENDS octave_test_ocp_simple_dae ) + set_tests_properties(octave_test_ocp_wtnx6 PROPERTIES DEPENDS octave_test_ocp_mass_spring) + set_tests_properties(octave_test_ocp_mass_spring PROPERTIES DEPENDS octave_test_ocp_simple_dae) set_tests_properties(octave_test_ocp_simple_dae PROPERTIES DEPENDS octave_test_target_selector) if(ACADOS_WITH_OSQP) set_tests_properties(octave_test_target_selector PROPERTIES DEPENDS octave_test_OSQP) From f0f474cbe0de01f24b99243226ed74942e876d8e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 12 Aug 2024 11:09:37 +0200 Subject: [PATCH 07/11] Use `d_mask` in DAQP interface to handle one-sided constraints (#1175) See https://github.com/acados/acados/pull/1174 and https://github.com/acados/acados/issues/650 --- acados/dense_qp/dense_qp_daqp.c | 31 +++++++++++++++++++ .../tests/one_sided_constraints_test.py | 24 +++++++++----- 2 files changed, 48 insertions(+), 7 deletions(-) diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c index 7b543a8628..640dff7ca7 100644 --- a/acados/dense_qp/dense_qp_daqp.c +++ b/acados/dense_qp/dense_qp_daqp.c @@ -511,6 +511,37 @@ static void dense_qp_daqp_update_memory(dense_qp_in *qp_in, const dense_qp_daqp_ mem->idxv_to_idxb[idxb[ii]] = ii; } + // printf("DAQP: dmask\n"); + // blasfeo_print_tran_dvec(2*(nb+ng), qp_in->d_mask, 0); + for (int ii = 0; ii < nb; ii++) + { + // NOTE: DAQP always works with double sided constraints, so currently one can not only ignore the upper bound or the lower bound. + + // "ignore" bounds that are marked as unconstrained in qp_in via dmask + if (BLASFEO_DVECEL(qp_in->d_mask, ii) == 0.0) + { + work->qp->blower[idxb[ii]] = -DAQP_INF; + } + if (BLASFEO_DVECEL(qp_in->d_mask, ii+ng+nb) == 0.0) + { + work->qp->bupper[idxb[ii]] = +DAQP_INF; + } + } + // ignore some general linear constraints. + for (int ii = 0; ii < ng; ii++) + { + if (BLASFEO_DVECEL(qp_in->d_mask, nb+ii) == 0.0) + { + work->qp->blower[ii+nv] = -DAQP_INF; + } + if (BLASFEO_DVECEL(qp_in->d_mask, 2*nb+ng+ii) == 0.0) + { + work->qp->bupper[ii+nv] = +DAQP_INF; + } + } + + + // Mark equality constraints for (int ii = 0; ii < ne; ii++) { diff --git a/examples/acados_python/tests/one_sided_constraints_test.py b/examples/acados_python/tests/one_sided_constraints_test.py index 4862512500..5215fcf4e6 100644 --- a/examples/acados_python/tests/one_sided_constraints_test.py +++ b/examples/acados_python/tests/one_sided_constraints_test.py @@ -40,7 +40,8 @@ PLOT = False -def main(constraint_variant='one_sided'): +def main(constraint_variant='one_sided', + qp_solver='PARTIAL_CONDENSING_HPIPM'): # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -99,11 +100,14 @@ def main(constraint_variant='one_sided'): ocp.constraints.lbx = np.array([-0.5*ACADOS_INFTY]) ocp.constraints.ubx = np.array([+5.0]) ocp.constraints.idxbx = np.array([0]) - # complementarity residual does not converge to tolerance if infty is too large - expected_status = 2 + if qp_solver == 'FULL_CONDENSING_DAQP': + expected_status = 0 + elif qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']: + # complementarity residual does not converge to tolerance if infty is not defined properly + expected_status = 2 # set options - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.qp_solver = qp_solver ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP' @@ -141,7 +145,8 @@ def main(constraint_variant='one_sided'): i_infty = 1 if constraint_variant == 'one_sided': assert lam[i_infty] == 0 - elif constraint_variant == 'one_sided_wrong_infty': + elif (constraint_variant == 'one_sided_wrong_infty' and + qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']): assert lam[i_infty] != 0 if PLOT: @@ -150,5 +155,10 @@ def main(constraint_variant='one_sided'): ocp_solver = None if __name__ == "__main__": - main(constraint_variant='one_sided') - main(constraint_variant='one_sided_wrong_infty') + for qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP']: + for constraint_variant in ['one_sided', 'one_sided_wrong_infty']: + print(80*'-') + print(f'constraint_variant = {constraint_variant}, qp_solver = {qp_solver}') + main(constraint_variant=constraint_variant, qp_solver=qp_solver) + + # main(constraint_variant='one_sided', qp_solver='PARTIAL_CONDENSING_HPIPM') From 648530f4ddb35a517dc49921dc8ab7afa1555072 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 16 Aug 2024 09:09:13 +0200 Subject: [PATCH 08/11] Minor Python changes from aligning Matlab (#1197) - add `json_file` property with defaults in class, as this is dumped to json. - set `Tsim` in `make_consistent` - formatting - remove redundant check for terminal cost --- .../acados_template/acados_multiphase_ocp.py | 32 ++++++++++++------- .../acados_template/acados_ocp.py | 25 +++++++++++---- .../acados_template/acados_ocp_solver.py | 22 +++++-------- .../acados_template/acados_template/utils.py | 5 +-- 4 files changed, 51 insertions(+), 33 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index af7c12b56f..e2f7fe1297 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -29,7 +29,7 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from typing import Union +from typing import Union, List import numpy as np from copy import deepcopy @@ -144,7 +144,7 @@ def __init__(self, N_list: list): self.phases_dims = [AcadosOcpDims() for _ in range(n_phases)] - self.dummy_ocp_list = [] + self.dummy_ocp_list: List[AcadosOcp] = [] # NOTE: this is the same for AcadosOcp self.solver_options = AcadosOcpOptions() @@ -166,6 +166,8 @@ def __init__(self, N_list: list): self.__parameter_values = [np.array([]) for _ in range(n_phases)] + self.__json_file = 'mocp.json' + self.code_export_directory = 'c_generated_code' """Path to where code will be exported. Default: `c_generated_code`.""" @@ -188,6 +190,14 @@ def parameter_values(self, parameter_values): raise Exception('parameter_values must be a list of length n_phases.') self.__parameter_values = parameter_values + @property + def json_file(self): + """Name of the json file where the problem description is stored.""" + return self.__json_file + + @json_file.setter + def json_file(self, json_file): + self.__json_file = json_file def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None: """ @@ -208,7 +218,7 @@ def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None: print(f"WARNING: set_phase: Phase {phase_idx} contains non-default solver options: {non_default_opts}, which will be ignored.\n", "Solver options need to be set via AcadosMultiphaseOcp.solver_options or mocp_opts instead.") - # set stage + # set phase self.model[phase_idx] = ocp.model self.cost[phase_idx] = ocp.cost self.constraints[phase_idx] = ocp.constraints @@ -319,9 +329,9 @@ def to_dict(self) -> dict: return ocp_dict - def dump_to_json(self, json_file: str) -> None: + def dump_to_json(self) -> None: ocp_nlp_dict = self.to_dict() - with open(json_file, 'w') as f: + with open(self.json_file, 'w') as f: json.dump(ocp_nlp_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True) return @@ -352,7 +362,7 @@ def __get_template_list(self, cmake_builder=None) -> list: return template_list - def render_templates(self, json_file: str, cmake_builder=None): + def render_templates(self, cmake_builder=None): # model templates for i, dummy_ocp in enumerate(self.dummy_ocp_list): @@ -361,11 +371,11 @@ def render_templates(self, json_file: str, cmake_builder=None): template_list = dummy_ocp._get_external_function_header_templates() # dump dummy_ocp - tmp_json_file = 'tmp_ocp.json' - dummy_ocp.dump_to_json(json_file=tmp_json_file) - tmp_json_path = os.path.abspath(tmp_json_file) + dummy_ocp.json_file = 'tmp_ocp.json' + dummy_ocp.dump_to_json() + tmp_json_path = os.path.abspath(dummy_ocp.json_file) - # renter templates + # render templates for tup in template_list: output_dir = self.code_export_directory if len(tup) <= 2 else tup[2] render_template(tup[0], tup[1], output_dir, tmp_json_path) @@ -373,7 +383,7 @@ def render_templates(self, json_file: str, cmake_builder=None): print("rendered model templates successfully") # check json file - json_path = os.path.abspath(json_file) + json_path = os.path.abspath(self.json_file) if not os.path.exists(json_path): raise Exception(f'Path "{json_path}" not found!') diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index f9e97e8ab2..074c7451d6 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -109,6 +109,7 @@ def __init__(self, acados_path=''): self.__parameter_values = np.array([]) self.__problem_class = 'OCP' + self.__json_file = "acados_ocp.json" self.code_export_directory = 'c_generated_code' """Path to where code will be exported. Default: `c_generated_code`.""" @@ -130,6 +131,14 @@ def parameter_values(self, parameter_values): raise Exception('Invalid parameter_values value. ' + f'Expected numpy array, got {type(parameter_values)}.') + @property + def json_file(self): + """Name of the json file where the problem description is stored.""" + return self.__json_file + + @json_file.setter + def json_file(self, json_file): + self.__json_file = json_file def make_consistent(self) -> None: """ @@ -306,9 +315,10 @@ def make_consistent(self) -> None: "Note: There is also the option to use the external cost module with a numerical hessian approximation (see `ext_cost_num_hess`).\n" "OR the option to provide a symbolic custom hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + # cost integration supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] if opts.cost_discretization == 'INTEGRATOR' and \ - any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type, cost.cost_type_e]]): + any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): raise Exception('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') ## constraints @@ -732,6 +742,9 @@ def make_consistent(self) -> None: raise Exception(f'Inconsistent discretization: {opts.tf}'\ f' = tf != sum(opts.time_steps) = {tf}.') + # set integrator time automatically + opts.Tsim = opts.time_steps[0] + # num_steps if isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == 1: opts.sim_method_num_steps = opts.sim_method_num_steps.item() @@ -817,7 +830,7 @@ def make_consistent(self) -> None: raise Exception(f'DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N, got qp solver {opts.qp_solver} and qp_solver_cond_N {opts.qp_solver_cond_N}, N {dims.N}.') if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]): raise Exception('DDP only supports initial state constraints, got path constraints.') - if any([dims.ng_e, dims.nphi_e, dims.nh_e]): + if any([dims.ng_e, dims.nphi_e, dims.nh_e]): raise Exception('DDP only supports initial state constraints, got terminal constraints.') # Set default parameters for globalization @@ -933,10 +946,10 @@ def __get_template_list(self, cmake_builder=None) -> list: return template_list - def render_templates(self, json_file: str, cmake_builder=None): + def render_templates(self, cmake_builder=None): # check json file - json_path = os.path.abspath(json_file) + json_path = os.path.abspath(self.json_file) if not os.path.exists(json_path): raise Exception(f'Path "{json_path}" not found!') @@ -955,8 +968,8 @@ def render_templates(self, json_file: str, cmake_builder=None): return - def dump_to_json(self, json_file: str) -> None: - with open(json_file, 'w') as f: + def dump_to_json(self) -> None: + with open(self.json_file, 'w') as f: json.dump(self.to_dict(), f, default=make_object_json_dumpable, indent=4, sort_keys=True) return diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 1d7c1d4b3d..2a04f509a0 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -109,13 +109,10 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': acados_ocp.remove_x0_elimination() - # set integrator time automatically - acados_ocp.solver_options.Tsim = acados_ocp.solver_options.time_steps[0] - # generate code (external functions and templated code) acados_ocp.generate_external_functions() - acados_ocp.dump_to_json(json_file) - acados_ocp.render_templates(json_file, cmake_builder=cmake_builder) + acados_ocp.dump_to_json() + acados_ocp.render_templates(cmake_builder=cmake_builder) # copy custom update function if acados_ocp.solver_options.custom_update_filename != "" and acados_ocp.solver_options.custom_update_copy: @@ -186,22 +183,19 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file= self.solver_created = False - if isinstance(acados_ocp, AcadosOcp): - if json_file is None: - json_file = 'acados_ocp_nlp.json' - elif isinstance(acados_ocp, AcadosMultiphaseOcp): - if json_file is None: - json_file = 'mocp.json' - else: + if not (isinstance(acados_ocp, AcadosOcp) or isinstance(acados_ocp, AcadosMultiphaseOcp)): raise Exception('acados_ocp should be of type AcadosOcp or AcadosMultiphaseOcp.') + if json_file is not None: + acados_ocp.json_file = json_file + if generate: - self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) + self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) else: acados_ocp.make_consistent() # load json, store options in object - with open(json_file, 'r') as f: + with open(acados_ocp.json_file, 'r') as f: acados_ocp_json = json.load(f) if isinstance(acados_ocp, AcadosOcp): self.N = acados_ocp_json['dims']['N'] diff --git a/interfaces/acados_template/acados_template/utils.py b/interfaces/acados_template/acados_template/utils.py index a3f872f3d0..9c67a5d2bf 100644 --- a/interfaces/acados_template/acados_template/utils.py +++ b/interfaces/acados_template/acados_template/utils.py @@ -216,7 +216,7 @@ def get_shared_lib_prefix(): else: return 'lib' -def get_tera(): +def get_tera() -> str: tera_path = get_tera_exec_path() acados_path = get_acados_path() @@ -258,6 +258,7 @@ def get_tera(): if not os.path.exists(tera_dir): print(f"Creating directory {tera_dir}") os.makedirs(tera_dir) + # Download tera print(f"Dowloading {url}") with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file: @@ -265,7 +266,7 @@ def get_tera(): print("Successfully downloaded t_renderer.") # make executable os.chmod(tera_path, 0o755) - print("Successfully downloaded t_renderer.") + print("Successfully made t_renderer executable.") return tera_path From c07fcf3b631c9dd2dba220063e7f45da9a663b91 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 16 Aug 2024 09:25:34 +0200 Subject: [PATCH 09/11] MEX: always use CMake for generated code (#1198) Since we only test Matlab/Octave on CI with Linux, we should use CMake to be closer to testing Windows workflow. --- .../+acados_template_mex/compile_ocp_shared_lib.m | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) 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 19ab18e234..dd7275d510 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 @@ -33,7 +33,20 @@ function compile_ocp_shared_lib(export_dir) return_dir = pwd; cd(export_dir); if isunix - [ status, result ] = system('make shared_lib'); + %% old code for make + % [ status, result ] = system('make shared_lib'); + % if status + % cd(return_dir); + % error('Building templated code as shared library failed.\nGot status %d, result: %s',... + % status, result); + % end + [ status, result ] = system('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_ACADOS_OCP_SOLVER_LIB=ON -S . -B .'); + if status + cd(return_dir); + error('Generating buildsystem failed.\nGot status %d, result: %s',... + status, result); + end + [ status, result ] = system('cmake --build . --config Release'); if status cd(return_dir); error('Building templated code as shared library failed.\nGot status %d, result: %s',... From 920d1df735dd4cda683fa701ebc49ec33e622656 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 16 Aug 2024 09:33:10 +0200 Subject: [PATCH 10/11] Add references to documentation (#1200) - Preprint on: 'Multi-Phase Optimal Control Problems for Efficient Nonlinear Model Predictive Control with acados' - Nonlinear MPC for Quadrotors in Close-Proximity Flight with Neural Network Downwash Prediction, shared in https://discourse.acados.org/t/acados-featured-work-collection/265/11 --------- Co-authored-by: sandmaennchen --- docs/citing/index.md | 21 ++++++++++++++++++++- docs/list_of_projects/index.md | 16 ++++++++++------ 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/docs/citing/index.md b/docs/citing/index.md index c37f08791d..605b4bf3fa 100644 --- a/docs/citing/index.md +++ b/docs/citing/index.md @@ -29,6 +29,25 @@ Additionally it describes the [`casados-integrator`](https://github.com/FreyJo/c } ``` +### Multi-Phase Optimal Control Problems for Efficient Nonlinear Model Predictive Control with acados +Computationally efficient nonlinear model predictive control relies on elaborate discrete-time optimal control problem (OCP) formulations trading off accuracy with respect to the continuous-time problem and associated computational burden. Such formulations, however, are in general not easy to implement within specialized software frameworks tailored to numerical optimal control. This paper introduces a new multi-phase OCP interface for the open-source software acados allowing to conveniently formulate such problems and generate fast solvers that can be used for nonlinear model predictive control (NMPC). While multi-phase OCP (MOCP) formulations occur naturally in many applications, this paper focuses on MOCP formulations that can be used to efficiently approximate standard continuous-time OCPs in the context of NMPC. + + +This feature can be used via the `AcadosMultiphaseOcp` class. + + +```latex +@misc{Frey2024MultiPhase, + title={Multi-Phase Optimal Control Problems for Efficient Nonlinear Model Predictive Control with acados}, + author={Jonathan Frey and Katrin Baumgärtner and Gianluca Frison and Moritz Diehl}, + year={2024}, + eprint={2408.07382}, + archivePrefix={arXiv}, + primaryClass={math.OC}, + url={https://arxiv.org/abs/2408.07382}, +} +``` + ### Advanced-Step Real-Time Iterations (AS-RTI) Advanced-step real-time iterations provide an extension to the classic real-time iteration algorithm, which allows to performs additional multi-level iterations in the preparation phase, such as inexact or zero-order SQP iterations on a problem with a predicted state estimate. @@ -80,4 +99,4 @@ Jens Geisler and Axel Schild and Moritz Diehl}, Booktitle = ECC, Year = {2019}, } -``` \ No newline at end of file +``` diff --git a/docs/list_of_projects/index.md b/docs/list_of_projects/index.md index 5a0e92c567..bc2dacde20 100644 --- a/docs/list_of_projects/index.md +++ b/docs/list_of_projects/index.md @@ -36,22 +36,26 @@ It also provides a higher level interface to `acados`, which is based on the Mat - [Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://arxiv.org/abs/2105.05998) - [Video to Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://www.dropbox.com/sh/mkr4pftcug6jlo7/AABNqu1AsGED2WSR8IqvaiUla?dl=0) -- [Continuous Control Set Nonlinear Model Predictive Control of Reluctance Synchronous Machines - IEEE Transactions on Control System Technology -- Andrea Zanelli et al 2021](https://ieeexplore.ieee.org/document/9360312) +- [Continuous Control Set Nonlinear Model Predictive Control of Reluctance Synchronous Machines - IEEE Transactions on Control System Technology](https://ieeexplore.ieee.org/document/9360312) - [Steering Action-aware Adaptive Cruise Control for Teleoperated Driving](https://ieeexplore.ieee.org/document/9945081) - [with public code on Github which has been applied on an F1TENTH vehicle](https://github.com/TUMFTM/tod_control/tree/2b67e8411e2ba1c5ddeb879d564ed28a989aebce/tod_shared_control) +- [Real-Time Neural MPC: Deep Learning Model Predictive Control for Quadrotors and Agile Robotic Platforms](https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=10049101) + + ### other -- [Contraction Properties of the Advanced Step Real-Time Iteration for NMPC at the IFAC World Congress 2020](https://cdn.syscop.de/publications/Nurkanovic2020b.pdf) +- [Contraction Properties of the Advanced Step Real-Time Iteration for NMPC — IFAC World Congress 2020](https://cdn.syscop.de/publications/Nurkanovic2020b.pdf) -- [Real-Time Nonlinear Model Predictive Control for Microgrid Operation at the American Control Conference 2020](https://cdn.syscop.de/publications/Nurkanovic2020a.pdf) +- [Real-Time Nonlinear Model Predictive Control for Microgrid Operation — American Control Conference 2020](https://cdn.syscop.de/publications/Nurkanovic2020a.pdf) - [Optimization-based Primary and Secondary Control of Microgrids](https://www.researchgate.net/profile/Armin_Nurkanovic/publication/341622767_Optimization-based_Primary_and_Secondary_Control_of_Microgrids/links/5f10519a299bf1e548ba5e77/Optimization-based-Primary-and-Secondary-Control-of-Microgrids.pdf) -- [TuneMPC—A Tool for Economic Tuning ofTracking (N)MPC Problems](https://cdn.syscop.de/publications/DeSchutter2020.pdf) +- [TuneMPC — A Tool for Economic Tuning of Tracking (N)MPC Problems](https://cdn.syscop.de/publications/DeSchutter2020.pdf) -- [Model predictive control of wind turbine fatigue via online rainflow-counting on stress history and prediction](https://iopscience.iop.org/article/10.1088/1742-6596/1618/2/022041/pdf) +- [Model Predictive Control of Wind Turbine Fatigue via Online Rainflow-Counting on Stress History and Prediction](https://iopscience.iop.org/article/10.1088/1742-6596/1618/2/022041/pdf) -- [Embedded Real-Time Nonlinear Model Predictive Control for the Thermal Torque Derating of an Electric Vehicle, Winkler et al, IFAC 2021](https://cdn.syscop.de/publications/Winkler2021.pdf) +- [Embedded Real-Time Nonlinear Model Predictive Control for the Thermal Torque Derating of an Electric Vehicle — IFAC 2021](https://cdn.syscop.de/publications/Winkler2021.pdf) +- [Nonlinear MPC for Quadrotors in Close-Proximity Flight with Neural Network Downwash Prediction](https://arxiv.org/abs/2304.07794) From 5c06b3a9fb99117b62f7a05efa40bc638f9929ba Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Fri, 16 Aug 2024 10:49:49 +0200 Subject: [PATCH 11/11] Print note if QP solver does not support one-sided constraints (#1202) --- interfaces/acados_matlab_octave/acados_ocp.m | 7 +++++++ .../acados_template/acados_template/acados_ocp_solver.py | 3 +++ 2 files changed, 10 insertions(+) diff --git a/interfaces/acados_matlab_octave/acados_ocp.m b/interfaces/acados_matlab_octave/acados_ocp.m index f2a04cc72f..152c0bf7c1 100644 --- a/interfaces/acados_matlab_octave/acados_ocp.m +++ b/interfaces/acados_matlab_octave/acados_ocp.m @@ -204,6 +204,13 @@ obj.ocp = setup_ocp(obj, simulink_opts); ocp_generate_c_code(obj.ocp); + % TODO move this somewhere else in new interface ? + if ~(strcmp(obj.ocp.solver_options.qp_solver, "FULL_CONDENSING_HPIPM") || ... + strcmp(obj.ocp.solver_options.qp_solver, "PARTIAL_CONDENSING_HPIPM") || ... + strcmp(obj.ocp.solver_options.qp_solver, "FULL_CONDENSING_DAQP")) + disp(['NOTE: The selected QP solver ', obj.ocp.solver_options.qp_solver, ' does not support one-sided constraints yet.']); + end + % templated MEX return_dir = pwd(); obj.code_gen_dir = obj.ocp.code_export_directory; diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 2a04f509a0..2b181e1d10 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -109,6 +109,9 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': acados_ocp.remove_x0_elimination() + if acados_ocp.solver_options.qp_solver in ['FULL_CONDENSING_QPOASES', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP']: + print(f"NOTE: The selected QP solver {acados_ocp.solver_options.qp_solver} does not support one-sided constraints yet.") + # generate code (external functions and templated code) acados_ocp.generate_external_functions() acados_ocp.dump_to_json()