diff --git a/.github/workflows/c_test_blasfeo_reference.yml b/.github/workflows/c_test_blasfeo_reference.yml index 5ce48890a7..80f59c89f7 100644 --- a/.github/workflows/c_test_blasfeo_reference.yml +++ b/.github/workflows/c_test_blasfeo_reference.yml @@ -19,6 +19,7 @@ env: ACADOS_OCTAVE_TEMPLATE: OFF ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON + ACADOS_WITH_DAQP: ON ACADOS_WITH_QPDUNES: ON @@ -67,7 +68,7 @@ jobs: # Note the current convention is to use the -S and -B options here to specify source # and build directories, but this is only available with CMake 3.13 and higher. # The CMake binaries on the Github Actions machines are (as of this writing) 3.12 - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DACADOS_OCTAVE_TEMPLATE=$ACADOS_OCTAVE_TEMPLATE -DLA=REFERENCE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DACADOS_OCTAVE_TEMPLATE=$ACADOS_OCTAVE_TEMPLATE -DLA=REFERENCE - name: Build & Install diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 74a3541ef9..fa7f9dd86c 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -19,6 +19,7 @@ env: ACADOS_OCTAVE_TEMPLATE: ON ACADOS_WITH_OSQP: ON ACADOS_WITH_QPOASES: ON + ACADOS_WITH_DAQP: ON ACADOS_WITH_QPDUNES: OFF @@ -67,7 +68,7 @@ jobs: # Note the current convention is to use the -S and -B options here to specify source # and build directories, but this is only available with CMake 3.13 and higher. # The CMake binaries on the Github Actions machines are (as of this writing) 3.12 - run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DACADOS_OCTAVE_TEMPLATE=$ACADOS_OCTAVE_TEMPLATE + run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DACADOS_OCTAVE_TEMPLATE=$ACADOS_OCTAVE_TEMPLATE - name: Build & Install diff --git a/.gitmodules b/.gitmodules index 50e4dc75f1..faa2dfd448 100644 --- a/.gitmodules +++ b/.gitmodules @@ -36,3 +36,6 @@ [submodule "examples/acados_python/tests/test_data"] path = examples/acados_python/tests/test_data url = https://github.com/acados/test_data +[submodule "external/daqp"] + path = external/daqp + url = https://github.com/darnstrom/daqp.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 923df17ab5..9c3dd4d475 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ option(ACADOS_EXAMPLES "Compile Examples" OFF) option(ACADOS_LINT "Compile Lint" OFF) # Extarnal libs option(ACADOS_WITH_QPOASES "qpOASES solver" OFF) +option(ACADOS_WITH_DAQP "DAQP solver" OFF) option(ACADOS_WITH_HPMPC "HPMPC solver" OFF) option(ACADOS_WITH_QORE "QORE solver" OFF) option(ACADOS_WITH_OOQP "OOQP solver" OFF) @@ -191,6 +192,7 @@ if(CMAKE_BUILD_TYPE MATCHES WithExternalLibs) set(ACADOS_WITH_QORE ON CACHE BOOL "Add QORE solver") set(ACADOS_WITH_OOQP ON CACHE BOOL "Add OOQP solver") set(ACADOS_WITH_QPOASES ON CACHE BOOL "Add qpOASES solver") + set(ACADOS_WITH_DAQP ON CACHE BOOL "Add DAQP solver") set(ACADOS_WITH_QPDUNES ON CACHE BOOL "Add qpDUNES solver") set(ACADOS_WITH_OSQP ON CACHE BOOL "Add OSQP solver") endif() @@ -297,6 +299,9 @@ endif() if(${ACADOS_WITH_QPOASES}) set(LINK_FLAG_QPOASES -lqpOASES_e) endif() +if(${ACADOS_WITH_DAQP}) + set(LINK_FLAG_DAQP -ldaqp) +endif() if(${ACADOS_WITH_QPDUNES}) set(LINK_FLAG_QPDUNES -lqpdunes) endif() @@ -317,6 +322,7 @@ endif() file(WRITE lib/link_libs.json {\n) file(APPEND lib/link_libs.json \t\"openmp\":\ \"${LINK_FLAG_OPENMP}\",\n) file(APPEND lib/link_libs.json \t\"qpoases\":\ \"${LINK_FLAG_QPOASES}\",\n) +file(APPEND lib/link_libs.json \t\"daqp\":\ \"${LINK_FLAG_DAQP}\",\n) file(APPEND lib/link_libs.json \t\"qpdunes\":\ \"${LINK_FLAG_QPDUNES}\",\n) file(APPEND lib/link_libs.json \t\"osqp\":\ \"${LINK_FLAG_OSQP}\",\n) file(APPEND lib/link_libs.json \t\"hpmpc\":\ \"${LINK_FLAG_HPMPC}\",\n) diff --git a/acados/CMakeLists.txt b/acados/CMakeLists.txt index c2f5a5284a..b1bcfd7b35 100644 --- a/acados/CMakeLists.txt +++ b/acados/CMakeLists.txt @@ -57,6 +57,10 @@ if(NOT ACADOS_WITH_QPOASES) list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_qpoases.c") endif() +if(NOT ACADOS_WITH_DAQP) + list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_daqp.c") +endif() + if(NOT ACADOS_WITH_QPDUNES) list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_qpdunes.c") endif() @@ -109,6 +113,12 @@ if(ACADOS_WITH_QPOASES) target_compile_definitions(acados PUBLIC USE_ACADOS_TYPES) endif() +if(ACADOS_WITH_DAQP) + target_link_libraries(acados PUBLIC daqp) + + target_compile_definitions(acados PUBLIC ACADOS_WITH_DAQP) +endif() + if(ACADOS_WITH_QPDUNES) target_link_libraries(acados PUBLIC qpdunes) @@ -173,6 +183,7 @@ configure_package_config_file(${PROJECT_SOURCE_DIR}/cmake/acadosConfig.cmake.in PATH_VARS ACADOS_WITH_HPMPC ACADOS_WITH_QORE ACADOS_WITH_QPOASES + ACADOS_WITH_DAQP ACADOS_WITH_QPDUNES ACADOS_WITH_OSQP ACADOS_WITH_OOQP) diff --git a/acados/dense_qp/Makefile b/acados/dense_qp/Makefile index e5d5b2bf01..75ed1826db 100644 --- a/acados/dense_qp/Makefile +++ b/acados/dense_qp/Makefile @@ -41,6 +41,9 @@ OBJS += dense_qp_hpipm.o ifeq ($(ACADOS_WITH_QPOASES), 1) OBJS += dense_qp_qpoases.o endif +ifeq ($(ACADOS_WITH_DAQP), 1) +OBJS += dense_qp_daqp.o +endif ifeq ($(ACADOS_WITH_QORE), 1) OBJS += dense_qp_qore.o endif diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c new file mode 100644 index 0000000000..8884a449ce --- /dev/null +++ b/acados/dense_qp/dense_qp_daqp.c @@ -0,0 +1,772 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * 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.; + */ + + +// external +#include +#include +#include +#include +// blasfeo +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_blas.h" + +// daqp +#define SOFT_WEIGHTS +#include "daqp/include/types.h" +#include "daqp/include/api.h" +#include "daqp/include/daqp.h" +#include "daqp/include/utils.h" + +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/dense_qp/dense_qp_daqp.h" +#include "acados/utils/mem.h" +#include "acados/utils/timing.h" +#include "acados/utils/print.h" +#include "acados/utils/math.h" + +#include "acados_c/dense_qp_interface.h" + + +/************************************************ + * opts + ************************************************/ + +acados_size_t dense_qp_daqp_opts_calculate_size(void *config_, dense_qp_dims *dims) +{ + acados_size_t size = 0; + size += sizeof(dense_qp_daqp_opts); + size += sizeof(DAQPSettings); + + make_int_multiple_of(8, &size); + + return size; +} + + + +void *dense_qp_daqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory) +{ + dense_qp_daqp_opts *opts; + + char *c_ptr = (char *) raw_memory; + + opts = (dense_qp_daqp_opts *) c_ptr; + c_ptr += sizeof(dense_qp_daqp_opts); + + opts->daqp_opts = (DAQPSettings *) c_ptr; + c_ptr += sizeof(DAQPSettings); + + assert((char *) raw_memory + dense_qp_daqp_opts_calculate_size(config_, dims) >= c_ptr); + + return (void *) opts; +} + + + +void dense_qp_daqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_) +{ + dense_qp_daqp_opts *opts = (dense_qp_daqp_opts *) opts_; + daqp_default_settings(opts->daqp_opts); + opts->warm_start=1; + return; +} + + + +void dense_qp_daqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_) +{ + return; +} + + + +void dense_qp_daqp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + dense_qp_daqp_opts *opts = opts_; + if (!strcmp(field, "tol_stat")) + { + // DAQP always "aims" at a stationary point + } + else if (!strcmp(field, "tol_eq")) + { + // Equality constraints are explicitly + // handled by the working set + } + else if (!strcmp(field, "tol_ineq")) + { + double *tol = value; + opts->daqp_opts->primal_tol = *tol; + } + else if (!strcmp(field, "tol_comp")) + { + // Complementary slackness is implicitly + // handled by the worlking set + } + else if (!strcmp(field, "iter_max")) + { + int *iter_max= value; + opts->daqp_opts->iter_limit = *iter_max; + } + else if (!strcmp(field, "warm_start")) + { + int *warm_start = value; + opts->warm_start = *warm_start; + } + else + { + printf("\nerror: dense_qp_daqp_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + +/************************************************ + * memory + ************************************************/ + +static acados_size_t daqp_workspace_calculate_size(int n, int m, int ms, int ns) +{ + acados_size_t size = 0; + + size += sizeof(DAQPWorkspace); + size += sizeof(DAQPProblem); + + size += n * n * sizeof(c_float); // H + size += 1 * n * sizeof(c_float); // f + size += n * (m-ms) * sizeof(c_float); // A + size += 2 * m * sizeof(c_float); // bupper/blower + + size += n * (m-ms) * sizeof(c_float); // M + size += 2 * m * sizeof(c_float); // dupper/dlower + size += (n+1)*n/2 * sizeof(c_float); // Rinv + size += n * sizeof(c_float); // v + size += m * sizeof(c_float); // scaling + + size += 2 * n * sizeof(c_float); // x & xold + size += 2*(n+ns+1) * sizeof(c_float); // lam & lam_star + size += n * sizeof(c_float); // u + + size += (n+ns+2)*(n+ns+1)/2 * sizeof(c_float); // L + size += (n+ns+1) * sizeof(c_float); // D + + size += 2*(n+ns+1) * sizeof(c_float); //xldl & zldl + + size += 4 * m * sizeof(c_float); // d_ls, d_us, rho_ls, rho_us + + size += m * sizeof(int); // work->sense + size += (n+ns+1) * sizeof(int); // WS + + make_int_multiple_of(8, &size); + + return size; +} + + +acados_size_t dense_qp_daqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) +{ + int n = dims->nv; + int m = dims->nv + dims->ng + dims->ne; + int ms = dims->nv; + int nb = dims->nb; + int ns = dims->ns; + + acados_size_t size = sizeof(dense_qp_daqp_memory); + + size += daqp_workspace_calculate_size(n, m, ms, ns); + + size += nb * 2 * sizeof(c_float); // lb_tmp & ub_tmp + size += nb * 1 * sizeof(int); // idbx + size += n * 1 * sizeof(int); // idxv_to_idxb; + size += ns * 1 * sizeof(int); // idbs + size += m * 1 * sizeof(int); // idxdaqp_to_idxs; + + size += ns * 6 * sizeof(c_float); // Zl,Zu,zl,zu,d_ls,d_us + make_int_multiple_of(8, &size); + + return size; +} + + +static void *daqp_workspace_assign(int n, int m, int ms, int ns, void *raw_memory) +{ + DAQPWorkspace *work; + char *c_ptr = (char *) raw_memory; + + work = (DAQPWorkspace *) c_ptr; + c_ptr += sizeof(DAQPWorkspace); + + work->qp = (DAQPProblem *) c_ptr; + c_ptr += sizeof(DAQPProblem); + + align_char_to(8, &c_ptr); + + // double + work->qp->H = (c_float*) c_ptr; + c_ptr += n * n * sizeof(c_float); + + work->qp->f = (c_float*) c_ptr; + c_ptr += 1 * n * sizeof(c_float); + + work->qp->A = (c_float*) c_ptr; + c_ptr += n * (m-ms) * sizeof(c_float); + + work->qp->bupper = (c_float*) c_ptr; + c_ptr += 1 * m * sizeof(c_float); + + work->qp->blower = (c_float*) c_ptr; + c_ptr += 1 * m * sizeof(c_float); + + work->M = (c_float *) c_ptr; + c_ptr += n * (m - ms) * sizeof(c_float); + + work->dupper = (c_float *) c_ptr; + c_ptr += 1 * m * sizeof(c_float); + + work->dlower = (c_float *) c_ptr; + c_ptr += 1 * m * sizeof(c_float); + + work->Rinv = (c_float *) c_ptr; + c_ptr += (n + 1) * n / 2 * sizeof(c_float); + + work->v = (c_float *) c_ptr; + c_ptr += n * sizeof(c_float); + + work->scaling = (c_float *) c_ptr; + c_ptr += m * sizeof(c_float); + + work->x = (c_float *) c_ptr; + c_ptr += n * sizeof(c_float); + + work->xold = (c_float *) c_ptr; + c_ptr += n * sizeof(c_float); + + work->lam = (c_float *) c_ptr; + c_ptr += (n+ns+1) * sizeof(c_float); + + work->lam_star = (c_float *) c_ptr; + c_ptr += (n+ns+1) * sizeof(c_float); + + work->u = (c_float *) c_ptr; + c_ptr += n * sizeof(c_float); + + work->D = (c_float *) c_ptr; + c_ptr += (n+ns+1) * sizeof(c_float); + + work->xldl = (c_float *) c_ptr; + c_ptr += (n+ns+1) * sizeof(c_float); + + work->zldl = (c_float *) c_ptr; + c_ptr += (n+ns+1) * sizeof(c_float); + + work->L = (c_float *) c_ptr; + c_ptr += (n+ns+2)*(n+ns+1)/2 * sizeof(c_float); + + work->d_ls = (c_float *) c_ptr; + c_ptr += m * sizeof(c_float); + + work->d_us = (c_float *) c_ptr; + c_ptr += m * sizeof(c_float); + + work->rho_ls = (c_float *) c_ptr; + c_ptr += m * sizeof(c_float); + + work->rho_us = (c_float *) c_ptr; + c_ptr += m * sizeof(c_float); + + // ints + work->sense = (int *) c_ptr; + c_ptr += m * sizeof(int); + + work->WS= (int *) c_ptr; + c_ptr += (n+ns+1) * sizeof(int); + + // Initialize constants of workspace + work->qp->nb = 0; + work->qp->bin_ids = NULL; + + work->n = n; + work->m = m; + work->ms = ms; + work->fval = -1; + work->n_active = 0; + work->iterations = 0; + work->sing_ind = 0; + work->soft_slack = 0; + + work->bnb = NULL; // No need to solve MIQP + + // initialize d_ls, d_us and sense + for (int ii=0; iid_ls[ii] = 0; + work->d_us[ii] = 0; + work->sense[ii] = 0; + } + + return work; +} + + +void *dense_qp_daqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, + void *raw_memory) +{ + dense_qp_daqp_memory *mem; + + int n = dims->nv; + int m = dims->nv + dims->ng + dims->ne; + int ms = dims->nv; + int nb = dims->nb; + int ns = dims->ns; + + // char pointer + char *c_ptr = (char *) raw_memory; + + mem = (dense_qp_daqp_memory *) c_ptr; + c_ptr += sizeof(dense_qp_daqp_memory); + + assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); + + // Assign raw memory to workspace + mem->daqp_work = daqp_workspace_assign(n, m, ms, ns, c_ptr); + c_ptr += daqp_workspace_calculate_size(n, m, ms, ns); + + assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); + + mem->lb_tmp = (c_float *) c_ptr; + c_ptr += nb * 1 * sizeof(c_float); + + mem->ub_tmp = (c_float *) c_ptr; + c_ptr += nb * 1 * sizeof(c_float); + + mem->idxb = (int *) c_ptr; + c_ptr += nb * 1 * sizeof(int); + + mem->idxv_to_idxb = (int *) c_ptr; + c_ptr += n * 1 * sizeof(int); + + mem->idxs= (int *) c_ptr; + c_ptr += ns * 1 * sizeof(int); + + mem->idxdaqp_to_idxs = (int *) c_ptr; + c_ptr += m * 1 * sizeof(int); + + mem->Zl = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + mem->Zu = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + mem->zl = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + mem->zu = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + mem->d_ls = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + mem->d_us = (c_float *) c_ptr; + c_ptr += ns * 1 * sizeof(c_float); + + assert((char *) raw_memory + dense_qp_daqp_memory_calculate_size(config_, dims, opts_) >= + c_ptr); + + return mem; +} + + + +void dense_qp_daqp_memory_get(void *config_, void *mem_, const char *field, void* value) +{ + // qp_solver_config *config = config_; + dense_qp_daqp_memory *mem = mem_; + + if (!strcmp(field, "time_qp_solver_call")) + { + double *tmp_ptr = value; + *tmp_ptr = mem->time_qp_solver_call; + } + else if (!strcmp(field, "iter")) + { + int *tmp_ptr = value; + *tmp_ptr = mem->iter; + } + else + { + printf("\nerror: dense_qp_daqp_memory_get: field %s not available\n", field); + exit(1); + } + + return; + +} + +/************************************************ + * workspace + ************************************************/ + +acados_size_t dense_qp_daqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) +{ + return 0; +} + + +/************************************************ + * functions + ************************************************/ + +// NOTE on transcription of acados dense QP into DAQP formulation: + + +// DAQP constraints are: [bounds on ALL x; linear constraints (ng); equality constraints (ne)] + +// A_DAQP = [C; A] +// blower = [lower bounds on ALL x (-INF if not set); lg; b_eq] +// bupper = [upper bounds on ALL x (+INF if not set); ug; b_eq] + + + +static void dense_qp_daqp_update_memory(dense_qp_in *qp_in, const dense_qp_daqp_opts *opts, dense_qp_daqp_memory *mem) +{ + // extract dense qp size + DAQPWorkspace * work = mem->daqp_work; + int nv = qp_in->dim->nv; + int nb = qp_in->dim->nb; + int ns = qp_in->dim->ns; + int ng = qp_in->dim->ng; + int ne = qp_in->dim->ne; + + // extract daqp data + double *lb_tmp = mem->lb_tmp; + double *ub_tmp = mem->ub_tmp; + int *idxb = mem->idxb; + int *idxs = mem->idxs; + + // fill in the upper triangular of H in dense_qp + blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); + + // extract data from qp_in in row-major + d_dense_qp_get_all_rowmaj(qp_in, work->qp->H, work->qp->f, // objective + work->qp->A+nv*ng, work->qp->bupper+nv+ng, // equalities + idxb, lb_tmp, ub_tmp, // bounds + work->qp->A, work->qp->blower+nv, work->qp->bupper+nv, // general linear constraints + mem->Zl, mem->Zu, mem->zl, mem->zu, idxs, mem->d_ls, mem->d_us // slacks + ); + + // printf("\nDAQP: matrix A\n"); + // int m = qp_in->dim->nv + qp_in->dim->ng + qp_in->dim->ne; + // int ms = qp_in->dim->nv; + // d_print_exp_tran_mat(nv, m-ms, work->qp->A, nv); + + // "Unignore" all general linear inequalites (ng) + for (int ii = nv; ii < nv+ng; ii++) + SET_MUTABLE(ii); + + // Setup upper/lower bounds + for (int ii = 0; ii < nv; ii++) + { + // "ignore" bounds that are not in acados dense QP + work->qp->blower[ii] = -DAQP_INF; + work->qp->bupper[ii] = +DAQP_INF; + SET_IMMUTABLE(ii); + } + for (int ii = 0; ii < nb; ii++) + { + // "Unignore" bounds that are in acados dense QP and set bound values + work->qp->blower[idxb[ii]] = lb_tmp[ii]; + work->qp->bupper[idxb[ii]] = ub_tmp[ii]; + SET_MUTABLE(idxb[ii]); + mem->idxv_to_idxb[idxb[ii]] = ii; + } + + // Mark equality constraints + for (int ii = 0; ii < ne; ii++) + { + // NOTE: b_eq values are ONLY in bupper, but sense status is default upper, thus fine. + work->sense[nv+ng+ii] &= ACTIVE+IMMUTABLE; + // SET_ACTIVE(nv+ng+ii); + // SET_IMMUTABLE(nv+ng+ii); + } + + // Soft constraints + int idxdaqp; // index of soft constraint within DAQP ordering + for (int ii = 0; ii < ns; ii++) + { + idxdaqp = idxs[ii] < nb ? idxb[idxs[ii]] : nv+idxs[ii]-nb; + mem->idxdaqp_to_idxs[idxdaqp] = ii; + + SET_SOFT(idxdaqp); + + // Quadratic slack penalty needs to be nonzero in DAQP + mem->Zl[ii] = MAX(1e-8,mem->Zl[ii]); + mem->Zu[ii] = MAX(1e-8,mem->Zu[ii]); + + // Setup soft weight used in DAQP + work->rho_ls[idxdaqp] = 1/mem->Zl[ii]; + work->rho_us[idxdaqp] = 1/mem->Zu[ii]; + + // Shift QP to handle linear terms on slack + // DAQP penalizes soft slacks s using a quadratic penalty s' s, bounded by s >= d_l + // (instead of weighting the soft slacks in the objective as in acados, + // the soft slacks are weighted in the constraint by rho=1/Z). + // To remove the linear term from acados we use the transformation + // s_daqp = (Z*s_acados+z/Z), + // which will shift blower/bupper and scale the nominal slack bounds with 1/Z + work->qp->blower[idxdaqp]+=mem->zl[ii]/mem->Zl[ii]; + work->qp->bupper[idxdaqp]-=mem->zu[ii]/mem->Zu[ii]; + + work->d_ls[idxdaqp] = MAX(0,mem->zl[ii]+mem->Zl[ii]*mem->d_ls[ii]); + work->d_us[idxdaqp] = MAX(0,mem->zu[ii]+mem->Zu[ii]*mem->d_us[ii]); + + // The default state in DAQP is that the soft slacks are active at their bounds + // => shift bupper/blower with these bounds + work->qp->blower[idxdaqp] -= work->d_ls[idxdaqp]/mem->Zl[ii]; + work->qp->bupper[idxdaqp] += work->d_us[idxdaqp]/mem->Zu[ii]; + } +} + + + +static void dense_qp_daqp_fill_output(dense_qp_daqp_memory *mem, const dense_qp_out *qp_out, const dense_qp_in *qp_in) +{ + int *idxv_to_idxb = mem->idxv_to_idxb; + int *idxs = mem->idxs; + int *idxb = mem->idxb; + int i; + int nv = qp_in->dim->nv; + int nb = qp_in->dim->nb; + int ng = qp_in->dim->ng; + int ns = qp_in->dim->ns; + DAQPWorkspace *work = mem->daqp_work; + + struct blasfeo_dvec *v = qp_out->v; + struct blasfeo_dvec *lambda = qp_out->lam; + + // print DAQP solution before expansion: + // printf("\n\nDAQP solution\n"); + // printf("------------------\n"); + // printf("\nx (primals):\n\n"); + // for (i = 0; ix[i]); + // printf("\nlambda (duals):\n\n"); + // for (i = 0; in_active; i++) + // printf("%e\t", work->lam_star[i]); + // printf("\n\n"); + + // primal variables + blasfeo_pack_dvec(nv, work->x, 1, v, 0); + + + // dual variables + blasfeo_dvecse(2 * nb + 2 * ng + 2 * ns, 0.0, lambda, 0); + c_float lam; + for (i = 0; i < work->n_active; i++) + { + lam = work->lam_star[i]; + if (work->WS[i] < nv) // bound constraint + { + if (lam >= 0.0) + BLASFEO_DVECEL(lambda, nb+ng+idxv_to_idxb[work->WS[i]]) = lam; + else + BLASFEO_DVECEL(lambda, idxv_to_idxb[work->WS[i]]) = -lam; + } + else if (work->WS[i] < nv+ng)// general constraint + { + if (lam >= 0.0) + BLASFEO_DVECEL(lambda, 2*nb+ng+work->WS[i]-nv) = lam; + else + BLASFEO_DVECEL(lambda, nb+work->WS[i]-nv) = -lam; + } + else // equality constraint + BLASFEO_DVECEL(qp_out->pi, work->WS[i]-nv-ng) = lam; + } + + // soft slacks + int idx; + for (i = 0; i < ns; i++) + { + idx = idxs[i] < nb ? idxb[idxs[i]] : nv+idxs[i]-nb; + // shift back QP + work->qp->blower[idx]-=(mem->zl[i]-work->d_ls[idx]/work->scaling[idx])/mem->Zl[i]; + work->qp->bupper[idx]+=(mem->zu[i]-work->d_us[idx]/work->scaling[idx])/mem->Zu[i]; + + // lower + if (BLASFEO_DVECEL(lambda, idxs[i]) == 0) // inactive soft => active slack bound + { + BLASFEO_DVECEL(v, nv+i) = mem->d_ls[i]; + BLASFEO_DVECEL(lambda, 2*nb+2*ng+i) = mem->d_ls[i]/mem->Zl[i]+mem->zl[i]; + } + else + { // if soft active => compute slack directly from equality + BLASFEO_DVECEL(v, nv+i) = work->qp->blower[idx]; + if (idxqp->A[disp] * BLASFEO_DVECEL(v, j); + } + } + // compute dual variable from stationarity condition + BLASFEO_DVECEL(lambda, 2*(nb+ng)+i) = mem->Zl[i] * BLASFEO_DVECEL(v, nv+i) + mem->zl[i] + - BLASFEO_DVECEL(lambda, idxs[i]); + } + + // upper + if (BLASFEO_DVECEL(lambda, idxs[i]+nb+ng) == 0) // inactive soft => active slack bound + { + BLASFEO_DVECEL(v, nv+ns+i) = mem->d_us[i]; + BLASFEO_DVECEL(lambda, 2*nb+2*ng+ns+i) = mem->d_us[i]/mem->Zu[i]+mem->zu[i]; + } + else + { // if soft active => compute slack directly from equality + BLASFEO_DVECEL(v, nv+ns+i) = -work->qp->bupper[idx]; + if (idxqp->A[disp] * BLASFEO_DVECEL(v, j); + } + // compute dual variable from stationarity condition + BLASFEO_DVECEL(lambda, 2*(nb+ng)+ns+i) = mem->Zu[i] * BLASFEO_DVECEL(v, nv+ns+i) + mem->zu[i] + - BLASFEO_DVECEL(lambda, idxs[i]+nb+ng); + } + } +} + + + +int dense_qp_daqp(void* config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_) +{ + qp_info *info = (qp_info *) qp_out->misc; + acados_timer tot_timer, qp_timer, interface_timer; + + acados_tic(&tot_timer); + acados_tic(&interface_timer); + + // cast structures + dense_qp_daqp_opts *opts = (dense_qp_daqp_opts *) opts_; + dense_qp_daqp_memory *memory = (dense_qp_daqp_memory *) memory_; + + // Move data into daqp workspace + dense_qp_daqp_update_memory(qp_in,opts,memory); + info->interface_time = acados_toc(&interface_timer); + + // Extract workspace and update settings + DAQPWorkspace* work = memory->daqp_work; + work->settings = opts->daqp_opts; + + // === Solve starts === + acados_tic(&qp_timer); + if (opts->warm_start==0) deactivate_constraints(work); + // setup LDP + int update_mask,daqp_status; + update_mask= (opts->warm_start==2) ? + UPDATE_v+UPDATE_d: UPDATE_Rinv+UPDATE_M+UPDATE_v+UPDATE_d; + update_ldp(update_mask,work); + // solve LDP + if (opts->warm_start==1) + activate_constraints(work); + + // TODO: shift active set? - not in SQP but would be nice as an option in SQP_RTI. + + daqp_status = daqp_ldp(memory->daqp_work); + ldp2qp_solution(work); + + // extract primal and dual solution + dense_qp_daqp_fill_output(memory,qp_out,qp_in); + info->solve_QP_time = acados_toc(&qp_timer); + + acados_tic(&interface_timer); + + // compute slacks + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; + + // log solve info + info->interface_time += acados_toc(&interface_timer); + info->total_time = acados_toc(&tot_timer); + info->num_iter = memory->daqp_work->iterations; + memory->time_qp_solver_call = info->solve_QP_time; + memory->iter = memory->daqp_work->iterations; + + // status + int acados_status = daqp_status; + if (daqp_status == EXIT_OPTIMAL || daqp_status == EXIT_SOFT_OPTIMAL) + acados_status = ACADOS_SUCCESS; + else if (daqp_status == EXIT_ITERLIMIT) + acados_status = ACADOS_MAXITER; + // NOTE: There are also: + // EXIT_INFEASIBLE, EXIT_CYCLE, EXIT_UNBOUNDED, EXIT_NONCONVEX, EXIT_OVERDETERMINED_INITIAL + + return acados_status; +} + + +void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +{ + printf("\nerror: dense_qp_daqp_eval_sens: not implemented yet\n"); + exit(1); +} + + +void dense_qp_daqp_config_initialize_default(void *config_) +{ + qp_solver_config *config = config_; + + config->opts_calculate_size = (acados_size_t (*)(void *, void *)) & dense_qp_daqp_opts_calculate_size; + config->opts_assign = (void *(*) (void *, void *, void *) ) & dense_qp_daqp_opts_assign; + config->opts_initialize_default = + (void (*)(void *, void *, void *)) & dense_qp_daqp_opts_initialize_default; + config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_daqp_opts_update; + config->opts_set = &dense_qp_daqp_opts_set; + config->memory_calculate_size = + (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_memory_calculate_size; + config->memory_assign = + (void *(*) (void *, void *, void *, void *) ) & dense_qp_daqp_memory_assign; + config->memory_get = &dense_qp_daqp_memory_get; + config->workspace_calculate_size = + (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_workspace_calculate_size; + config->eval_sens = &dense_qp_daqp_eval_sens; + // config->memory_reset = &dense_qp_daqp_memory_reset; + config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_daqp; + + return; +} diff --git a/acados/dense_qp/dense_qp_daqp.h b/acados/dense_qp/dense_qp_daqp.h new file mode 100644 index 0000000000..6e2770f222 --- /dev/null +++ b/acados/dense_qp/dense_qp_daqp.h @@ -0,0 +1,113 @@ +/* + * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, + * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, + * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, + * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ +#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// daqp +#include "daqp/include/types.h" + +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + + +typedef struct dense_qp_daqp_opts_ +{ + DAQPSettings* daqp_opts; + int warm_start; +} dense_qp_daqp_opts; + + +typedef struct dense_qp_daqp_memory_ +{ + double* lb_tmp; + double* ub_tmp; + int* idxb; + int* idxv_to_idxb; + int* idxs; + int* idxdaqp_to_idxs; + + double* Zl; + double* Zu; + double* zl; + double* zu; + double* d_ls; + double* d_us; + + double time_qp_solver_call; + int iter; + DAQPWorkspace * daqp_work; + +} dense_qp_daqp_memory; + +// opts +acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +// memory +acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +// functions +int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_daqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 0b44e3458b..8134fde7b4 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -89,6 +89,8 @@ acados_size_t dense_qp_qpoases_opts_calculate_size(void *config_, dense_qp_dims acados_size_t size = 0; size += sizeof(dense_qp_qpoases_opts); + make_int_multiple_of(8, &size); + return size; } @@ -103,7 +105,7 @@ void *dense_qp_qpoases_opts_assign(void *config_, dense_qp_dims *dims, void *raw opts = (dense_qp_qpoases_opts *) c_ptr; c_ptr += sizeof(dense_qp_qpoases_opts); - assert((char *) raw_memory + dense_qp_qpoases_opts_calculate_size(config_, dims) == c_ptr); + assert((char *) raw_memory + dense_qp_qpoases_opts_calculate_size(config_, dims) >= c_ptr); return (void *) opts; } @@ -384,7 +386,7 @@ void dense_qp_qpoases_memory_get(void *config_, void *mem_, const char *field, v /************************************************ - * workspcae + * workspace ************************************************/ acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) @@ -681,7 +683,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo acados_tic(&interface_timer); // copy prim_sol and dual_sol to qp_out blasfeo_pack_dvec(nv2, prim_sol, 1, qp_out->v, 0); - for (int ii = 0; ii < 2 * nb + 2 * ng + 2 * ns; ii++) qp_out->lam->pa[ii] = 0.0; + blasfeo_dvecse(2 * nb + 2 * ng + 2 * ns, 0.0, qp_out->lam, 0); for (int ii = 0; ii < nb; ii++) { if (dual_sol[idxb[ii]] >= 0.0) diff --git a/cmake/.gitignore b/cmake/.gitignore index 38bedca5a6..bb8a490c37 100644 --- a/cmake/.gitignore +++ b/cmake/.gitignore @@ -2,3 +2,4 @@ # cmake --build build --target install *Config*.cmake *Targets*.cmake +daqp*.cmake 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 0e366cde8e..5229ca5467 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 @@ -51,7 +51,7 @@ nlp_solver = 'sqp'; % sqp, sqp_rti qp_solver = 'partial_condensing_hpipm'; -% full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases +% full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases, full_condensing_daqp qp_solver_cond_N = 5; % for partial condensing % we add some model-plant mismatch by choosing different integration 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 79e1e27952..ddd7446ce2 100644 --- a/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m @@ -46,7 +46,7 @@ nlp_solver = 'sqp'; % sqp, sqp_rti qp_solver = 'partial_condensing_hpipm'; - % full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases + % full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases, full_condensing_daqp qp_solver_cond_N = 5; % for partial condensing % integrator type sim_method = 'erk'; % erk, irk, irk_gnsf diff --git a/examples/acados_matlab_octave/test/test_ocp_wtnx6.m b/examples/acados_matlab_octave/test/test_ocp_wtnx6.m index 9544ea4bb9..36fe706aaf 100644 --- a/examples/acados_matlab_octave/test/test_ocp_wtnx6.m +++ b/examples/acados_matlab_octave/test/test_ocp_wtnx6.m @@ -35,6 +35,8 @@ addpath('../wind_turbine_nx6/'); +% qpOASES too slow to test on CI +for itest = 1:3 %% arguments compile_interface = 'auto'; @@ -61,8 +63,17 @@ ocp_nlp_solver_tol_ineq = 1e-8; ocp_nlp_solver_tol_comp = 1e-8; ocp_nlp_solver_ext_qp_res = 1; -ocp_qp_solver = 'partial_condensing_hpipm'; -%ocp_qp_solver = 'full_condensing_hpipm'; +switch itest +case 1 + ocp_qp_solver = 'partial_condensing_hpipm'; +case 2 + ocp_qp_solver = 'full_condensing_hpipm'; +case 3 + ocp_qp_solver = 'full_condensing_daqp'; +case 4 + ocp_qp_solver = 'full_condensing_qpoases'; +end +fprintf(['\n\nrunning with qp solver ', ocp_qp_solver, '\n']) ocp_qp_solver_cond_N = 5; ocp_qp_solver_cond_ric_alg = 0; ocp_qp_solver_ric_alg = 0; @@ -71,10 +82,12 @@ ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4 * ones(ocp_N, 1); % scalar or vector of size ocp_N; ocp_sim_method_num_steps = 1 * ones(ocp_N, 1); % scalar or vector of size ocp_N; -ocp_sim_method_newton_iter = 3 * ones(ocp_N, 1); % scalar or vector of size ocp_N; +ocp_sim_method_newton_iter = 3% * ones(ocp_N, 1); % scalar or vector of size ocp_N; %cost_type = 'linear_ls'; cost_type = 'nonlinear_ls'; +% get references +compute_setup; %% create model entries @@ -242,8 +255,8 @@ ocp_model.set('constr_Jsh_e', Jsh_e); % initial state dummy -ocp_model.set('constr_x0', zeros(nx, 1)); - +ocp_model.set('constr_x0', x0_ref); +% ocp_model.model_struct; @@ -265,6 +278,7 @@ ocp_opts.set('nlp_solver_tol_comp', ocp_nlp_solver_tol_comp); end ocp_opts.set('qp_solver', ocp_qp_solver); +ocp_opts.set('qp_solver_iter_max', 500); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_cond_N', ocp_qp_solver_cond_N); ocp_opts.set('qp_solver_cond_ric_alg', ocp_qp_solver_cond_ric_alg); @@ -278,6 +292,8 @@ ocp_opts.set('regularize_method', 'no_regularize'); ocp_opts.set('ext_fun_compile_flags', ''); +ocp_opts.set('parameter_values', wind0_ref(:,1)); + ocp_opts.opts_struct; @@ -289,8 +305,6 @@ %ocp.C_ocp %ocp.C_ocp_ext_fun - - %% acados sim model sim_model = acados_sim_model(); % symbolics @@ -335,9 +349,6 @@ %% closed loop simulation -% get references -compute_setup; - n_sim = 100; n_sim_max = length(wind0_ref) - ocp_N; if n_sim>n_sim_max @@ -348,12 +359,17 @@ u_sim = zeros(nu, n_sim); sqp_iter_sim = zeros(n_sim,1); +time_ext = zeros(n_sim, 1); +time_tot = zeros(n_sim, 1); +time_lin = zeros(n_sim, 1); +time_qp_sol = zeros(n_sim, 1); % set trajectory initialization x_traj_init = repmat(x0_ref, 1, ocp_N+1); u_traj_init = repmat(u0_ref, 1, ocp_N); pi_traj_init = zeros(nx, ocp_N); + for ii=1:n_sim % fprintf('\nsimulation step %d\n', ii); @@ -425,23 +441,26 @@ u_traj_init = [u(:,2:ocp_N), u(:,ocp_N)]; pi_traj_init = [pi(:,2:ocp_N), pi(:,ocp_N)]; - time_ext = toc; + time_ext(ii) = toc; 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'); + 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'); + qp_iter = sqp_stats(:,7); sqp_iter_sim(ii) = sqp_iter; if status ~= 0 - error('ocp_nlp solver returned nonzero status!'); + ocp.print() + error(['ocp_nlp solver returned status ', num2str(status), '!= 0 in simulation instance ', num2str(ii)]); end - fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms]), Pel = %f',... - status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, electrical_power); + fprintf('\nstatus = %d, sqp_iter = %d, qp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms]), Pel = %f',... + 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') @@ -477,6 +496,9 @@ err_vs_ref = x_sim_ref - x_sim(:,end); +fprintf('\nmedian computation times: time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])', ... + median(time_ext)*1e3, median(time_tot)*1e3, median(time_lin)*1e3, median(time_qp_sol)*1e3) + if status~=0 error('test_ocp_wtnx6: solution failed!'); elseif err_vs_ref > 1e-14 @@ -523,6 +545,7 @@ end end +end % remove temporary created files delete('y_ref') diff --git a/examples/acados_python/getting_started/ocp/minimal_example_ocp.py b/examples/acados_python/getting_started/ocp/minimal_example_ocp.py index 35d293ce14..5a3f778fb3 100644 --- a/examples/acados_python/getting_started/ocp/minimal_example_ocp.py +++ b/examples/acados_python/getting_started/ocp/minimal_example_ocp.py @@ -90,7 +90,7 @@ # 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 +# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'IRK' # ocp.solver_options.print_level = 1 diff --git a/examples/acados_python/pmsm_example/main.py b/examples/acados_python/pmsm_example/main.py index 07d7f6695a..03867f28c9 100644 --- a/examples/acados_python/pmsm_example/main.py +++ b/examples/acados_python/pmsm_example/main.py @@ -357,6 +357,7 @@ def get_general_terminal_constraints_DC(): # ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' +# ocp.solver_options.qp_solver = 'FULL_CONDENSING_DAQP' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'IRK' # ocp.solver_options.integrator_type = 'ERK' diff --git a/examples/acados_python/tests/soft_constraint_test.py b/examples/acados_python/tests/soft_constraint_test.py index ef9c2d6d45..db6ee63729 100644 --- a/examples/acados_python/tests/soft_constraint_test.py +++ b/examples/acados_python/tests/soft_constraint_test.py @@ -40,12 +40,21 @@ import numpy as np import scipy.linalg -nFormulations = 2 -tol = 1E-6 +import itertools -# FORMULATION = 1 # 0 for soft constraints on x - using bounds on x -# # 1 for soft constraints on x - using nonlinear constraint h -def run_closed_loop_experiment(FORMULATION): +SOFT_CONSTRAINT_TYPES = ['bx', 'h'] +TOL = 1E-6 +N = 20 + +QP_SOLVERS = ('PARTIAL_CONDENSING_HPIPM', \ + 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ + 'FULL_CONDENSING_DAQP' + ) +# no soft constraint support: +# 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ + + +def run_closed_loop_experiment(soft_constr_type='bx', verbose=False, qp_solver='PARTIAL_CONDENSING_HPIPM'): # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -58,7 +67,6 @@ def run_closed_loop_experiment(FORMULATION): nu = model.u.size()[0] ny = nx + nu ny_e = nx - N = 20 # set dimensions # NOTE: all dimensions but N ar detected @@ -68,10 +76,10 @@ def run_closed_loop_experiment(FORMULATION): ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' - Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) - R = 2*np.diag([1e-2]) + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) - ocp.cost.W = scipy.linalg.block_diag(Q, R) + ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx,:nx] = np.eye(nx) @@ -81,16 +89,11 @@ def run_closed_loop_experiment(FORMULATION): ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) - ocp.cost.W_e = Q + ocp.cost.W_e = Q_mat - ocp.cost.yref = np.zeros((ny, )) + ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) - ocp.cost.zl = 2000*np.ones((1,)) - ocp.cost.Zl = 1*np.ones((1,)) - ocp.cost.zu = 2000*np.ones((1,)) - ocp.cost.Zu = 1*np.ones((1,)) - # set constraints Fmax = 80 vmax = 5 @@ -102,7 +105,9 @@ def run_closed_loop_experiment(FORMULATION): ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) - if FORMULATION == 0: + + # soft constraint on x (either via bx or h) + if soft_constr_type == 'bx': # soft bound on x ocp.constraints.lbx = np.array([-vmax]) ocp.constraints.ubx = np.array([+vmax]) @@ -110,7 +115,7 @@ def run_closed_loop_experiment(FORMULATION): # indices of slacked constraints within bx ocp.constraints.idxsbx = np.array([0]) - elif FORMULATION == 1: + elif soft_constr_type == 'h': # soft bound on x, using constraint h v1 = ocp.model.x[2] ocp.model.con_h_expr = v1 @@ -119,14 +124,25 @@ def run_closed_loop_experiment(FORMULATION): ocp.constraints.uh = np.array([+vmax]) # indices of slacked constraints within h ocp.constraints.idxsh = np.array([0]) + else: + raise Exception(f"soft_constr_type must be 'bx', or 'h', got {soft_constr_type}.") + + ocp.cost.zl = 2000*np.ones((1,)) + ocp.cost.Zl = 1*np.ones((1,)) + ocp.cost.zu = 2000*np.ones((1,)) + ocp.cost.Zu = 1*np.ones((1,)) # 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.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' - ocp.solver_options.tol = 1e-1 * tol + ocp.solver_options.tol = 1e-1 * TOL + ocp.solver_options.nlp_solver_ext_qp_res = 1 + + ocp.solver_options.qp_solver_warm_start = 0 + ocp.solver_options.qp_solver_iter_max = 200 json_filename = 'pendulum_soft_constraints.json' acados_ocp_solver = AcadosOcpSolver(ocp, json_file = json_filename) @@ -137,6 +153,8 @@ def run_closed_loop_experiment(FORMULATION): simX = np.ndarray((Nsim+1, nx)) simU = np.ndarray((Nsim, nu)) xcurrent = x0 + qp_iter = np.zeros(Nsim) + sqp_iter = np.zeros(Nsim) for i in range(Nsim): simX[i,:] = xcurrent @@ -146,9 +164,16 @@ def run_closed_loop_experiment(FORMULATION): acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() + if verbose: + acados_ocp_solver.print_statistics() + if status != 0: + acados_ocp_solver.print_statistics() raise Exception('acados acados_ocp_solver returned status {}. Exiting.'.format(status)) + qp_iter[i] = np.sum(acados_ocp_solver.get_stats('qp_iter')) + sqp_iter[i] = acados_ocp_solver.get_stats('sqp_iter') + simU[i,:] = acados_ocp_solver.get(0, "u") # simulate system @@ -173,32 +198,49 @@ def run_closed_loop_experiment(FORMULATION): # plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) # store results - np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX) - np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU) + np.savetxt(f'test_results/simX_soft_formulation_{soft_constr_type}_{qp_solver}', simX) + np.savetxt(f'test_results/simU_soft_formulation_{soft_constr_type}_{qp_solver}', simU) + np.savetxt(f'test_results/sqp_iter_soft_formulation_{soft_constr_type}_{qp_solver}', sqp_iter) - print("soft constraint example: ran formulation", FORMULATION, "successfully.") + print(f"\nsoft constraint example: ran formulation {soft_constr_type} with {qp_solver} successfully.\n") + print(f"took {sqp_iter} SQP iterations and {qp_iter} QP iterations.") + del acados_ocp_solver -if __name__ == "__main__": +def main(): + # import pdb; pdb.set_trace() + for (soft_constr_type, qp_solver) in itertools.product(SOFT_CONSTRAINT_TYPES, QP_SOLVERS): + run_closed_loop_experiment(soft_constr_type=soft_constr_type, qp_solver=qp_solver) - for i in range(nFormulations): - run_closed_loop_experiment(i) + # load reference solution + soft_constr_type = 'bx' + qp_solver = 'PARTIAL_CONDENSING_HPIPM' + simX_ref = np.loadtxt(f'test_results/simX_soft_formulation_{soft_constr_type}_{qp_solver}') + simU_ref = np.loadtxt(f'test_results/simU_soft_formulation_{soft_constr_type}_{qp_solver}') + sqp_iter_ref = np.loadtxt(f'test_results/sqp_iter_soft_formulation_{soft_constr_type}_{qp_solver}') - simX_ref = np.loadtxt('test_results/simX_soft_formulation_' + str(0)) - simU_ref = np.loadtxt('test_results/simU_soft_formulation_' + str(0)) - for i in range(1, nFormulations): - simX = np.loadtxt('test_results/simX_soft_formulation_' + str(i)) - simU = np.loadtxt('test_results/simU_soft_formulation_' + str(i)) + # compare + for (soft_constr_type, qp_solver) in itertools.product(SOFT_CONSTRAINT_TYPES, QP_SOLVERS): + simX = np.loadtxt(f'test_results/simX_soft_formulation_{soft_constr_type}_{qp_solver}') + simU = np.loadtxt(f'test_results/simU_soft_formulation_{soft_constr_type}_{qp_solver}') + sqp_iter = np.loadtxt(f'test_results/sqp_iter_soft_formulation_{soft_constr_type}_{qp_solver}') error_x = np.linalg.norm(simX_ref - simX) error_u = np.linalg.norm(simU_ref - simU) error_xu = max([error_x, error_u]) - print("soft constraint example: formulation", i, " solution deviates from reference by", error_xu, ".") + print(f"soft constraint example: formulation {soft_constr_type} with {qp_solver} deviates from reference by {error_xu}") + + if error_xu > TOL: + raise Exception(f"soft constraint example: formulations should return same solution up to {TOL:.2e}, got error_x {error_x}, error_u {error_u} for {soft_constr_type}, {qp_solver}") + + if any(sqp_iter != sqp_iter_ref): + raise Exception(f"all formulations should take the same number of SQP iterations.") - if error_xu > tol: - raise Exception("soft constraint example: formulations should return same solution up to" + str(tol)) + print(f"soft constraint example: SUCCESS, got same solutions for equivalent formulations up to tolerance {TOL:.2e}") - print("soft constraint example: SUCCESS, got same solutions for equivalent formulations up to tolerance %1.e." %(tol)) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/examples/c/dense_qp.c b/examples/c/dense_qp.c index 745787e381..e1aa989ef2 100644 --- a/examples/c/dense_qp.c +++ b/examples/c/dense_qp.c @@ -88,6 +88,7 @@ int main() { dense_qp_solver_plan plan; // plan.qp_solver = DENSE_QP_QPOASES; +// plan.qp_solver = DENSE_QP_DAQP; plan.qp_solver = DENSE_QP_HPIPM; qp_solver_config *config = dense_qp_config_create(&plan); diff --git a/examples/c/ocp_qp.c b/examples/c/ocp_qp.c index 1fca66943a..ffabdf96c0 100644 --- a/examples/c/ocp_qp.c +++ b/examples/c/ocp_qp.c @@ -58,7 +58,7 @@ int main() { ocp_qp_solver_plan_t plan; plan.qp_solver = FULL_CONDENSING_HPIPM; //FULL_CONDENSING_QPOASES; // FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM // PARTIAL_CONDENSING_HPIPM, PARTIAL_CONDENSING_HPMPC, PARTIAL_CONDENSING_OOQP, - // PARTIAL_CONDENSING_OSQP, PARTIAL_CONDENSING_QPDUNES, FULL_CONDENSING_QORE, FULL_CONDENSING_OOQP, + // PARTIAL_CONDENSING_OSQP, PARTIAL_CONDENSING_QPDUNES, FULL_CONDENSING_QORE, FULL_CONDENSING_OOQP, FULL_CONDENSING_DAQP ocp_qp_xcond_solver_config *config = ocp_qp_xcond_solver_config_create(plan); diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt index 30b8e816b8..f42b4b8437 100644 --- a/external/CMakeLists.txt +++ b/external/CMakeLists.txt @@ -60,6 +60,12 @@ if(ACADOS_WITH_QPOASES) add_subdirectory(qpoases) endif() +if(ACADOS_WITH_DAQP) + set(PROFILING OFF CACHE BOOL "Turn off profiling in DAQP") + set(SOFT_WEIGHTS ON CACHE BOOL "Turn on soft weights in DAQP") + add_subdirectory(daqp) +endif() + if(ACADOS_WITH_OSQP) set(PROFILING OFF CACHE BOOL "Turn off profiling in OSQP") set(CTRLC OFF CACHE BOOL "Turn off CTRLC in OSQP") diff --git a/external/daqp b/external/daqp new file mode 160000 index 0000000000..40f42016fc --- /dev/null +++ b/external/daqp @@ -0,0 +1 @@ +Subproject commit 40f42016fcbc63994e2deee629e501980d711cc3 diff --git a/interfaces/acados_c/dense_qp_interface.c b/interfaces/acados_c/dense_qp_interface.c index 422e87e553..b9dabcbc35 100644 --- a/interfaces/acados_c/dense_qp_interface.c +++ b/interfaces/acados_c/dense_qp_interface.c @@ -53,6 +53,9 @@ #ifdef ACADOS_WITH_QPOASES #include "acados/dense_qp/dense_qp_qpoases.h" #endif +#ifdef ACADOS_WITH_DAQP +#include "acados/dense_qp/dense_qp_daqp.h" +#endif #ifdef ACADOS_WITH_OOQP #include "acados/dense_qp/dense_qp_ooqp.h" #endif @@ -75,6 +78,11 @@ qp_solver_config *dense_qp_config_create(dense_qp_solver_plan *plan) dense_qp_qpoases_config_initialize_default(solver_config); break; #endif +#ifdef ACADOS_WITH_DAQP + case DENSE_QP_DAQP: + dense_qp_daqp_config_initialize_default(solver_config); + break; +#endif #ifdef ACADOS_WITH_QORE case DENSE_QP_QORE: dense_qp_qore_config_initialize_default(solver_config); diff --git a/interfaces/acados_c/dense_qp_interface.h b/interfaces/acados_c/dense_qp_interface.h index 4ecc77381d..9de77f242d 100644 --- a/interfaces/acados_c/dense_qp_interface.h +++ b/interfaces/acados_c/dense_qp_interface.h @@ -41,7 +41,7 @@ extern "C" { #include "acados/dense_qp/dense_qp_common.h" -typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t; +typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP, DENSE_QP_DAQP } dense_qp_solver_t; typedef struct { diff --git a/interfaces/acados_c/ocp_qp_interface.c b/interfaces/acados_c/ocp_qp_interface.c index 6fe299fe6d..a09afbf62d 100644 --- a/interfaces/acados_c/ocp_qp_interface.c +++ b/interfaces/acados_c/ocp_qp_interface.c @@ -55,6 +55,10 @@ #include "acados/dense_qp/dense_qp_qpoases.h" #endif +#ifdef ACADOS_WITH_DAQP +#include "acados/dense_qp/dense_qp_daqp.h" +#endif + #include "acados/ocp_qp/ocp_qp_hpipm.h" #ifdef ACADOS_WITH_HPMPC #include "acados/ocp_qp/ocp_qp_hpmpc.h" @@ -82,63 +86,70 @@ void ocp_qp_xcond_solver_config_initialize_from_plan( switch (solver_name) { case PARTIAL_CONDENSING_HPIPM: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); ocp_qp_hpipm_config_initialize_default(solver_config->qp_solver); - ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #ifdef ACADOS_WITH_HPMPC case PARTIAL_CONDENSING_HPMPC: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); ocp_qp_hpmpc_config_initialize_default(solver_config->qp_solver); - ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #endif #ifdef ACADOS_WITH_OOQP case PARTIAL_CONDENSING_OOQP: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); ocp_qp_ooqp_config_initialize_default(solver_config->qp_solver); - ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #endif #ifdef ACADOS_WITH_OSQP case PARTIAL_CONDENSING_OSQP: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); ocp_qp_osqp_config_initialize_default(solver_config->qp_solver); - ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #endif #ifdef ACADOS_WITH_QPDUNES case PARTIAL_CONDENSING_QPDUNES: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); ocp_qp_qpdunes_config_initialize_default(solver_config->qp_solver); - ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_partial_condensing_config_initialize_default(solver_config->xcond); break; #endif case FULL_CONDENSING_HPIPM: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); dense_qp_hpipm_config_initialize_default(solver_config->qp_solver); - ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); break; #ifdef ACADOS_WITH_QPOASES case FULL_CONDENSING_QPOASES: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); dense_qp_qpoases_config_initialize_default(solver_config->qp_solver); - ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); + break; +#endif +#ifdef ACADOS_WITH_DAQP + case FULL_CONDENSING_DAQP: + ocp_qp_xcond_solver_config_initialize_default(solver_config); + dense_qp_daqp_config_initialize_default(solver_config->qp_solver); + ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); break; #endif #ifdef ACADOS_WITH_QORE case FULL_CONDENSING_QORE: - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); dense_qp_qore_config_initialize_default(solver_config->qp_solver); - ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); break; #endif #ifdef ACADOS_WITH_OOQP case FULL_CONDENSING_OOQP: printf("\nocp_qp_xcond_solver: FULL_CONDENSING_OOQP.\n"); - ocp_qp_xcond_solver_config_initialize_default(solver_config); + ocp_qp_xcond_solver_config_initialize_default(solver_config); dense_qp_ooqp_config_initialize_default(solver_config->qp_solver); - ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); + ocp_qp_full_condensing_config_initialize_default(solver_config->xcond); break; #endif case INVALID_QP_SOLVER: @@ -151,7 +162,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan( exit(1); } - return; + return; } diff --git a/interfaces/acados_c/ocp_qp_interface.h b/interfaces/acados_c/ocp_qp_interface.h index 2582f142da..0514b64c46 100644 --- a/interfaces/acados_c/ocp_qp_interface.h +++ b/interfaces/acados_c/ocp_qp_interface.h @@ -87,6 +87,11 @@ typedef enum { #else FULL_CONDENSING_QPOASES_NOT_AVAILABLE, #endif +#ifdef ACADOS_WITH_DAQP + FULL_CONDENSING_DAQP, +#else + FULL_CONDENSING_DAQP_NOT_AVAILABLE, +#endif #ifdef ACADOS_WITH_QORE FULL_CONDENSING_QORE, #else diff --git a/interfaces/acados_matlab_octave/ocp_compile_interface.m b/interfaces/acados_matlab_octave/ocp_compile_interface.m index 0fcfa67f87..81bf32f591 100644 --- a/interfaces/acados_matlab_octave/ocp_compile_interface.m +++ b/interfaces/acados_matlab_octave/ocp_compile_interface.m @@ -96,6 +96,9 @@ function ocp_compile_interface(opts) if ~isempty(libs.qpoases) defines_tmp = [defines_tmp, ' -DACADOS_WITH_QPOASES']; end + if ~isempty(libs.daqp) + defines_tmp = [defines_tmp, ' -DACADOS_WITH_DAQP']; + end if ~isempty(libs.osqp) defines_tmp = [defines_tmp, ' -DACADOS_WITH_OSQP']; end @@ -133,6 +136,9 @@ function ocp_compile_interface(opts) if ~isempty(libs.qpoases) defines_tmp = [defines_tmp, ' -DACADOS_WITH_QPOASES']; end + if ~isempty(libs.daqp) + defines_tmp = [defines_tmp, ' -DACADOS_WITH_DAQP']; + end if ~isempty(libs.osqp) defines_tmp = [defines_tmp, ' -DACADOS_WITH_OSQP']; end @@ -168,7 +174,7 @@ function ocp_compile_interface(opts) % NOTE: empty linker flags do not work in Octave mex(mex_flags, FLAGS, LDFLAGS, COMPDEFINES, COMPFLAGS, acados_include, acados_interfaces_include, external_include, blasfeo_include, hpipm_include,... acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo', libs.qpoases,... - libs.qpdunes, libs.osqp, libs.hpmpc, libs.ooqp, mex_files{ii}, '-outdir', opts.output_dir) + libs.daqp, libs.qpdunes, libs.osqp, libs.hpmpc, libs.ooqp, mex_files{ii}, '-outdir', opts.output_dir) end end diff --git a/interfaces/acados_matlab_octave/ocp_create.c b/interfaces/acados_matlab_octave/ocp_create.c index ecd2bc4520..bfc1de5dbc 100644 --- a/interfaces/acados_matlab_octave/ocp_create.c +++ b/interfaces/acados_matlab_octave/ocp_create.c @@ -365,6 +365,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES; } #endif +#if defined(ACADOS_WITH_DAQP) + else if (!strcmp(qp_solver, "full_condensing_daqp")) + { + plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_DAQP; + } +#endif #if defined(ACADOS_WITH_HPMPC) else if (!strcmp(qp_solver, "partial_condensing_hpmpc")) { diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 37fa37cd7e..4892a1838e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -2166,7 +2166,7 @@ def __init__(self): @property def qp_solver(self): """QP solver to be used in the NLP solver. - String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP'). + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'). Default: 'PARTIAL_CONDENSING_HPIPM'. """ return self.__qp_solver @@ -2594,7 +2594,8 @@ def ext_cost_num_hess(self): def qp_solver(self, qp_solver): qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ - 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP') + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ + 'FULL_CONDENSING_DAQP') if qp_solver in qp_solvers: self.__qp_solver = qp_solver else: diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 6c4c6720e6..c788af121a 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -121,6 +121,8 @@ {%- set openmp_flag = " " %} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} {%- set link_libs = "-lqpOASES_e" %} + {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} + {%- set link_libs = "-ldaqp" %} {%- else %} {%- set link_libs = "" %} {%- endif %} @@ -293,6 +295,9 @@ set(CMAKE_C_FLAGS " {%- if qp_solver == "FULL_CONDENSING_QPOASES" -%} -DACADOS_WITH_QPOASES {%- endif -%} +{%- if qp_solver == "FULL_CONDENSING_DAQP" -%} + -DACADOS_WITH_DAQP +{%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%} -DACADOS_WITH_OSQP {%- endif -%} @@ -310,6 +315,9 @@ include_directories( {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} ${ACADOS_INCLUDE_PATH}/qpOASES_e/ {%- endif %} +{%- if qp_solver == "FULL_CONDENSING_DAQP" %} + ${ACADOS_INCLUDE_PATH}/daqp/include +{%- endif %} ) # linker flags diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index bee84fce7b..3eac72c65a 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -120,6 +120,8 @@ {%- set openmp_flag = " " %} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} {%- set link_libs = "-lqpOASES_e" %} + {%- elif qp_solver == "FULL_CONDENSING_DAQP" %} + {%- set link_libs = "-ldaqp" %} {%- else %} {%- set link_libs = "" %} {%- endif %} @@ -275,6 +277,9 @@ LIB_PATH = {{ acados_lib_path }} {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} CPPFLAGS += -DACADOS_WITH_QPOASES {%- endif %} +{%- if qp_solver == "FULL_CONDENSING_DAQP" %} +CPPFLAGS += -DACADOS_WITH_DAQP +{%- endif %} {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %} CPPFLAGS += -DACADOS_WITH_OSQP {%- endif %} @@ -288,6 +293,9 @@ CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include {%- if qp_solver == "FULL_CONDENSING_QPOASES" %} CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/ {%- endif %} + {%- if qp_solver == "FULL_CONDENSING_DAQP" %} +CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include + {%- endif %} {# c-compiler flags #} # define the c-compiler flags for make's implicit rules diff --git a/interfaces/acados_template/acados_template/c_templates_tera/make_sfun.in.m b/interfaces/acados_template/acados_template/c_templates_tera/make_sfun.in.m index 96b9fa144a..e650dff30f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/make_sfun.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/make_sfun.in.m @@ -133,6 +133,9 @@ {%- elif solver_options.qp_solver is containing("QPDUNES") %} CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ]; COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ]; +{%- elif solver_options.qp_solver is containing("DAQP") %} +CFLAGS = [ CFLAGS, ' -DACADOS_WITH_DAQP' ]; +COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_DAQP' ]; {%- elif solver_options.qp_solver is containing("HPMPC") %} CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ]; COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ]; @@ -153,6 +156,9 @@ {% if solver_options.qp_solver is containing("QPOASES") %} LIBS{end+1} = '-lqpOASES_e'; {% endif %} + {% if solver_options.qp_solver is containing("DAQP") %} +LIBS{end+1} = '-ldaqp'; + {% endif %} {%- endif %} mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... diff --git a/test/ocp_qp/test_qpsolvers.cpp b/test/ocp_qp/test_qpsolvers.cpp index 3047eac06d..cac7801c47 100644 --- a/test/ocp_qp/test_qpsolvers.cpp +++ b/test/ocp_qp/test_qpsolvers.cpp @@ -60,6 +60,9 @@ ocp_qp_solver_t hashit(std::string const &inString) #ifdef ACADOS_WITH_QPOASES if (inString == "DENSE_QPOASES") return FULL_CONDENSING_QPOASES; #endif +#ifdef ACADOS_WITH_DAQP + if (inString == "DENSE_DAQP") return FULL_CONDENSING_DAQP; +#endif #ifdef ACADOS_WITH_QPDUNES if (inString == "SPARSE_QPDUNES") return PARTIAL_CONDENSING_QPDUNES; #endif @@ -85,6 +88,7 @@ double solver_tolerance(std::string const &inString) // if (inString == "SPARSE_QPDUNES") return 1e-8; if (inString == "DENSE_HPIPM") return 1e-8; if (inString == "DENSE_QPOASES") return 1e-10; + if (inString == "DENSE_DAQP") return 1e-10; if (inString == "DENSE_QORE") return 1e-10; if (inString == "SPARSE_OOQP") return 1e-5; if (inString == "DENSE_OOQP") return 1e-5; @@ -125,6 +129,10 @@ TEST_CASE("mass spring example", "[QP solvers]") , "DENSE_QPOASES" #endif +#ifdef ACADOS_WITH_DAQP + , + "DENSE_DAQP" +#endif // #ifdef ACADOS_WITH_QPDUNES // ,"SPARSE_QPDUNES" // #endif