diff --git a/phonelibs/acados/.gitignore b/phonelibs/acados/.gitignore index 90d32700b8..9787bd1b55 100644 --- a/phonelibs/acados/.gitignore +++ b/phonelibs/acados/.gitignore @@ -1,4 +1,4 @@ -acados/ +acados_repo/ !x86_64/ !larch64/ !aarch64/ diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh index ccc1031f0b..5a8ec6e1d0 100755 --- a/phonelibs/acados/build.sh +++ b/phonelibs/acados/build.sh @@ -12,11 +12,11 @@ elif [ -f /EON ]; then BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" fi -if [ ! -d acados/ ]; then +if [ ! -d acados_repo/ ]; then #git clone https://github.com/acados/acados.git $DIR/acados - git clone https://github.com/commaai/acados.git $DIR/acados + git clone https://github.com/commaai/acados.git $DIR/acados_repo fi -cd acados +cd acados_repo git fetch git checkout 05bcbfe42818738c74572f27d06ad75a28d3b380 git submodule update --recursive --init @@ -31,14 +31,14 @@ INSTALL_DIR="$DIR/$ARCHNAME" rm -rf $INSTALL_DIR mkdir -p $INSTALL_DIR -rm $DIR/acados/lib/*.json +rm $DIR/acados_repo/lib/*.json -cp -r $DIR/acados/include $DIR -cp -r $DIR/acados/lib $INSTALL_DIR -cp -r $DIR/acados/interfaces/acados_template/acados_template $DIR/../../pyextra +cp -r $DIR/acados_repo/include $DIR +cp -r $DIR/acados_repo/lib $INSTALL_DIR +cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra #pip3 install -e $DIR/acados/interfaces/acados_template # build tera -cd $DIR/acados/interfaces/acados_template/tera_renderer/ +cd $DIR/acados_repo/interfaces/acados_template/tera_renderer/ cargo build --verbose --release cp target/release/t_renderer $INSTALL_DIR/ diff --git a/phonelibs/acados/include/acados/dense_qp/dense_qp_common.h b/phonelibs/acados/include/acados/dense_qp/dense_qp_common.h new file mode 100644 index 0000000000..f3809c4294 --- /dev/null +++ b/phonelibs/acados/include/acados/dense_qp/dense_qp_common.h @@ -0,0 +1,150 @@ +/* + * 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_COMMON_H_ +#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_dense_qp.h" +#include "hpipm/include/hpipm_d_dense_qp_res.h" +#include "hpipm/include/hpipm_d_dense_qp_sol.h" +// acados +#include "acados/utils/types.h" + +typedef struct d_dense_qp_dim dense_qp_dims; +typedef struct d_dense_qp dense_qp_in; +typedef struct d_dense_qp_sol dense_qp_out; +typedef struct d_dense_qp_res dense_qp_res; +typedef struct d_dense_qp_res_ws dense_qp_res_ws; + + + +#ifndef QP_SOLVER_CONFIG_ +#define QP_SOLVER_CONFIG_ +typedef struct +{ + void (*dims_set)(void *config_, void *dims_, const char *field, const int* value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *args); + void (*opts_update)(void *config, void *dims, void *args); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args); + void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args); + int (*evaluate)(void *config, void *qp_in, void *qp_out, void *args, void *mem, void *work); + void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} qp_solver_config; +#endif + + + +#ifndef QP_INFO_ +#define QP_INFO_ +typedef struct +{ + double solve_QP_time; + double condensing_time; + double interface_time; + double total_time; + int num_iter; + int t_computed; +} qp_info; +#endif + + + +/* config */ +// +acados_size_t dense_qp_solver_config_calculate_size(); +// +qp_solver_config *dense_qp_solver_config_assign(void *raw_memory); + +/* dims */ +// +acados_size_t dense_qp_dims_calculate_size(); +// +dense_qp_dims *dense_qp_dims_assign(void *raw_memory); +// +void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value); +// + +/* in */ +// +acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims); +// +dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory); + +/* out */ +// +acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims); +// +dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_out_get(dense_qp_out *out, const char *field, void *value); + +/* res */ +// +acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims); +// +dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory); +// +acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims); +// +dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out); +// +void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws); +// +void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]); + +/* misc */ +// +void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); +// +void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out); +// +void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_ diff --git a/phonelibs/acados/include/acados/dense_qp/dense_qp_hpipm.h b/phonelibs/acados/include/acados/dense_qp/dense_qp_hpipm.h new file mode 100644 index 0000000000..20eedc26a2 --- /dev/null +++ b/phonelibs/acados/include/acados/dense_qp/dense_qp_hpipm.h @@ -0,0 +1,94 @@ +/* + * 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_HPIPM_H_ +#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_dense_qp.h" +#include "hpipm/include/hpipm_d_dense_qp_ipm.h" +#include "hpipm/include/hpipm_d_dense_qp_sol.h" +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + + + +typedef struct dense_qp_hpipm_opts_ +{ + struct d_dense_qp_ipm_arg *hpipm_opts; +} dense_qp_hpipm_opts; + + + +typedef struct dense_qp_hpipm_memory_ +{ + struct d_dense_qp_ipm_ws *hpipm_workspace; + double time_qp_solver_call; + int iter; + +} dense_qp_hpipm_memory; + + + +// +acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims); +// +void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory); +// +void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_); +// +void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_); +// +acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_); +// +void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_); +// +int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_hpipm_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_ diff --git a/phonelibs/acados/include/acados/dense_qp/dense_qp_ooqp.h b/phonelibs/acados/include/acados/dense_qp/dense_qp_ooqp.h new file mode 100644 index 0000000000..646f11f06f --- /dev/null +++ b/phonelibs/acados/include/acados/dense_qp/dense_qp_ooqp.h @@ -0,0 +1,128 @@ +/* + * 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_OOQP_H_ +#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +enum dense_qp_ooqp_termination_code +{ + DENSE_SUCCESSFUL_TERMINATION = 0, + DENSE_NOT_FINISHED, + DENSE_MAX_ITS_EXCEEDED, + DENSE_INFEASIBLE, + DENSE_UNKNOWN +}; + +typedef struct dense_qp_ooqp_opts_ +{ + int printLevel; + int useDiagonalWeights; // TODO(dimitris): implement option + int fixHessian; + int fixDynamics; + int fixInequalities; +} dense_qp_ooqp_opts; + +typedef struct dense_qp_ooqp_workspace_ +{ + double *x; + double *gamma; + double *phi; + double *y; + double *z; + double *lambda; + double *pi; + double objectiveValue; +} dense_qp_ooqp_workspace; + +typedef struct dense_qp_ooqp_memory_ +{ + int firstRun; + int nx; + int my; + int mz; + double *c; + double *dQ; + double *xlow; + char *ixlow; + double *xupp; + char *ixupp; + double *dA; + double *bA; + double *dC; + double *clow; + char *iclow; + double *cupp; + char *icupp; + double time_qp_solver_call; + int iter; + +} dense_qp_ooqp_memory; + +// +acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims); +// +void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_); +// +void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, + void *raw_memory); +// +acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_); +// +int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, + void *memory_, void *work_); +// +void dense_qp_ooqp_destroy(void *mem_, void *work); +// +void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_ooqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_ diff --git a/phonelibs/acados/include/acados/dense_qp/dense_qp_qore.h b/phonelibs/acados/include/acados/dense_qp/dense_qp_qore.h new file mode 100644 index 0000000000..52606fac5d --- /dev/null +++ b/phonelibs/acados/include/acados/dense_qp/dense_qp_qore.h @@ -0,0 +1,127 @@ +/* + * 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_QORE_H_ +#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// qore +#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h" +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +typedef struct dense_qp_qore_opts_ +{ + int nsmax; // maximum size of Schur complement + int print_freq; // print frequency, + // prtfreq < 0: disable printing; + // prtfreq == 0: print on each call and include working set changes; + // prtfreq > 0: print on every prtfreq seconds, but do not include working set + // changes; + int warm_start; // warm start with updated matrices H and C + int warm_strategy; // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous + // solution + int hot_start; // hot start with unchanged matrices H and C + int max_iter; // maximum number of iterations + int compute_t; // compute t in qp_out (to have correct residuals in NLP) +} dense_qp_qore_opts; + +typedef struct dense_qp_qore_memory_ +{ + double *H; + double *HH; + double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; + double *A; + double *b; + double *C; + double *CC; + double *Ct; + double *CCt; + double *d_lb0; + double *d_ub0; + double *d_lb; + double *d_ub; + double *d_lg; + double *d_ug; + double *d_ls; + double *d_us; + double *lb; + double *ub; + int *idxb; + int *idxb_stacked; + int *idxs; + double *prim_sol; + double *dual_sol; + QoreProblemDense *QP; + int num_iter; + dense_qp_in *qp_stacked; + double time_qp_solver_call; + int iter; + +} dense_qp_qore_memory; + +acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_qore_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_QORE_H_ diff --git a/phonelibs/acados/include/acados/dense_qp/dense_qp_qpoases.h b/phonelibs/acados/include/acados/dense_qp/dense_qp_qpoases.h new file mode 100644 index 0000000000..8fca17dd17 --- /dev/null +++ b/phonelibs/acados/include/acados/dense_qp/dense_qp_qpoases.h @@ -0,0 +1,127 @@ +/* + * 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_QPOASES_H_ +#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +typedef struct dense_qp_qpoases_opts_ +{ + double max_cputime; // maximum cpu time in seconds + int max_nwsr; // maximum number of working set recalculations + int warm_start; // warm start with dual_sol in memory + int use_precomputed_cholesky; + int hotstart; // this option requires constant data matrices! (eg linear MPC, inexact schemes + // with frozen sensitivities) + int set_acado_opts; // use same options as in acado code generation + int compute_t; // compute t in qp_out (to have correct residuals in NLP) + double tolerance; // terminationTolerance +} dense_qp_qpoases_opts; + +typedef struct dense_qp_qpoases_memory_ +{ + double *H; + double *HH; + double *R; + double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; + double *A; + double *b; + double *d_lb0; + double *d_ub0; + double *d_lb; + double *d_ub; + double *C; + double *CC; + double *d_lg0; + double *d_ug0; + double *d_lg; + double *d_ug; + double *d_ls; + double *d_us; + int *idxb; + int *idxb_stacked; + int *idxs; + double *prim_sol; + double *dual_sol; + void *QPB; // NOTE(giaf): cast to QProblemB to use + void *QP; // NOTE(giaf): cast to QProblem to use + double cputime; // cputime of qpoases + int nwsr; // performed number of working set recalculations + int first_it; // to be used with hotstart + dense_qp_in *qp_stacked; + double time_qp_solver_call; // equal to cputime + int iter; + +} dense_qp_qpoases_memory; + +acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_qpoases_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_ diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_common.h new file mode 100644 index 0000000000..0407633816 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_common.h @@ -0,0 +1,427 @@ +/* + * 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.; + */ + + + +/// \defgroup ocp_nlp ocp_nlp +/// @{ +/// @} + +/// \defgroup ocp_nlp_solver ocp_nlp_solver +/// @{ +/// @} + +/// \ingroup ocp_nlp +/// @{ + +/// \ingroup ocp_nlp_solver +/// @{ + +/// \defgroup ocp_nlp_common ocp_nlp_common +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_xcond_solver.h" +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct ocp_nlp_config +{ + int N; // number of stages + + // solver-specific implementations of memory management functions + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts_); + void (*opts_update)(void *config, void *dims, void *opts_); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_); + void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value); + // evaluate solver // TODO rename into solve + int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); + void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work, + char *field, int stage, int index, void *sens_nlp_out); + // prepare memory + int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); + // initalize this struct with default values + void (*config_initialize_default)(void *config); + // general getter + void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_); + void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_); + void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_); + // config structs of submodules + ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver + ocp_nlp_dynamics_config **dynamics; + ocp_nlp_cost_config **cost; + ocp_nlp_constraints_config **constraints; + ocp_nlp_reg_config *regularize; + +} ocp_nlp_config; + +// +acados_size_t ocp_nlp_config_calculate_size(int N); +// +ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory); + + + +/************************************************ + * dims + ************************************************/ + +/// Structure to store dimensions/number of variables. +typedef struct ocp_nlp_dims +{ + void **cost; + void **dynamics; + void **constraints; + ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ?? + ocp_nlp_reg_dims *regularize; + + int *nv; // number of primal variables (states+controls+slacks) + int *nx; // number of differential states + int *nu; // number of inputs + int *ni; // number of two-sided inequality constraints: nb+ng+nh+ns + int *nz; // number of algebraic variables + int *ns; // number of slack variables + int N; // number of shooting nodes +} ocp_nlp_dims; + +// +acados_size_t ocp_nlp_dims_calculate_size(void *config); +// +ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory); + +/// Sets the dimension of optimization variables +/// (states, constrols, algebraic variables, slack variables). +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param field The type of optimization variables, either nx, nu, nz, or ns. +/// \param value_array Number of variables for each stage. +void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_, + const char *field, const void* value_array); + +/// Sets the dimensions of constraints functions for a stage +/// (bounds on states, bounds on controls, equality constraints, +/// inequality constraints). +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh. +/// \param value_field Number of constraints/bounds for the given stage. +void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, + const char *field, const void* value_field); + +/// Sets the dimensions of the cost terms for a stage. +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field Type of cost term, can be eiter ny. +/// \param value_field Number of cost terms/residuals for the given stage. +void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field, + const void* value_field); + +/// Sets the dimensions of the dynamics for a stage. +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field TBD +/// \param value TBD +void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field, + const void* value); + +/************************************************ + * Inputs + ************************************************/ + +/// Struct for storing the inputs of an OCP NLP solver +typedef struct ocp_nlp_in +{ + /// Length of sampling intervals/timesteps. + double *Ts; + + /// Pointers to cost functions (TBC). + void **cost; + + /// Pointers to dynamics functions (TBC). + void **dynamics; + + /// Pointers to constraints functions (TBC). + void **constraints; + +} ocp_nlp_in; + +// +acados_size_t ocp_nlp_in_calculate_size_self(int N); +// +acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims); +// +ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory); +// +ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory); + + +/************************************************ + * out + ************************************************/ + +typedef struct ocp_nlp_out +{ + struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs? + struct blasfeo_dvec *z; // algebraic vairables + struct blasfeo_dvec *pi; // multipliers for dynamics + struct blasfeo_dvec *lam; // inequality mulitpliers + struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution) + + // NOTE: the inequalities are internally organized in the following order: + // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + int sqp_iter; + int qp_iter; + double inf_norm_res; + double total_time; + +} ocp_nlp_out; + +// +acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims); +// +ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + void *raw_memory); + + + +/************************************************ + * options + ************************************************/ + +/// Globalization types +typedef enum +{ + FIXED_STEP, + MERIT_BACKTRACKING, +} ocp_nlp_globalization_t; + +typedef struct ocp_nlp_opts +{ + ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ??? + void *regularize; + void **dynamics; // dynamics_opts + void **cost; // cost_opts + void **constraints; // constraints_opts + double step_length; // step length in case of FIXED_STEP + double levenberg_marquardt; // LM factor to be added to the hessian before regularization + int reuse_workspace; + int num_threads; + + // TODO: move to separate struct? + ocp_nlp_globalization_t globalization; + double alpha_min; + double alpha_reduction; +} ocp_nlp_opts; + +// +acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value); +// +void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value); + + +/************************************************ + * residuals + ************************************************/ + +typedef struct ocp_nlp_res +{ + struct blasfeo_dvec *res_stat; // stationarity + struct blasfeo_dvec *res_eq; // dynamics + struct blasfeo_dvec *res_ineq; // inequality constraints + struct blasfeo_dvec *res_comp; // complementarity + double inf_norm_res_stat; + double inf_norm_res_eq; + double inf_norm_res_ineq; + double inf_norm_res_comp; + acados_size_t memsize; +} ocp_nlp_res; + +// +acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims); +// +ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory); + +/************************************************ + * memory + ************************************************/ + +typedef struct ocp_nlp_memory +{ +// void *qp_solver_mem; // xcond solver mem instead ??? + ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ??? + void *regularize_mem; + void **dynamics; // dynamics memory + void **cost; // cost memory + void **constraints; // constraints memory + + // residuals + ocp_nlp_res *nlp_res; + + // qp in & out + ocp_qp_in *qp_in; + ocp_qp_out *qp_out; + // QP stuff not entering the qp_in struct + struct blasfeo_dmat *dzduxt; // dzdux transposed + struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables + + struct blasfeo_dvec *cost_grad; + struct blasfeo_dvec *ineq_fun; + struct blasfeo_dvec *ineq_adj; + struct blasfeo_dvec *dyn_fun; + struct blasfeo_dvec *dyn_adj; + + double cost_value; + + bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables + struct blasfeo_dvec *sim_guess; + + int *sqp_iter; // pointer to iteration number + +} ocp_nlp_memory; + +// +acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts); +// +ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_opts *opts, void *raw_memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct ocp_nlp_workspace +{ + + void *qp_work; + void **dynamics; // dynamics_workspace + void **cost; // cost_workspace + void **constraints; // constraints_workspace + + ocp_nlp_out *tmp_nlp_out; + ocp_nlp_out *weight_merit_fun; + +} ocp_nlp_workspace; + +// +acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts); +// +ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory); + + + +/************************************************ + * function + ************************************************/ + +// +void ocp_nlp_initialize_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha); +// +double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, + ocp_nlp_res *res, ocp_nlp_memory *mem); +// +void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ +/// @} +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h new file mode 100644 index 0000000000..7f7a30faf3 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -0,0 +1,241 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; + int nu; + int nz; + int nb; // nbx + nbu + int nbu; // number of input box constraints + int nbx; // number of state box constraints + int ng; // number of general linear constraints + int nh; // number of nonlinear path constraints + int ns; // nsbu + nsbx + nsg + nsh + int nsbu; // number of softened input bounds + int nsbx; // number of softened state bounds + int nsg; // number of softened general linear constraints + int nsh; // number of softened nonlinear constraints + int nbue; // number of input box constraints which are equality + int nbxe; // number of state box constraints which are equality + int nge; // number of general linear constraints which are equality + int nhe; // number of nonlinear path constraints which are equality +} ocp_nlp_constraints_bgh_dims; + +// +acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config); +// +void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_constraints_bgh_dims_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx, + int nbu, int ng, int nh, int dummy0, int ns); +// +void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, + const char *field, const int* value); + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + int *idxb; + int *idxs; + int *idxe; + struct blasfeo_dvec d; // gathers bounds + struct blasfeo_dmat DCt; // general linear constraint matrix + // lg <= [D, C] * [u; x] <= ug + external_function_generic *nl_constr_h_fun; // nonlinear: lh <= h(x,u) <= uh + external_function_generic *nl_constr_h_fun_jac; // nonlinear: lh <= h(x,u) <= uh + external_function_generic *nl_constr_h_fun_jac_hess; // nonlinear: lh <= h(x,u) <= uh +} ocp_nlp_constraints_bgh_model; + +// +acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, + void *model_, const char *field, void *value); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_constraints_bgh_opts; + +// +acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dvec *lam; // pointer to lam in nlp_out + struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out + struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dmat *DCt; // pointer to DCt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory + int *idxb; // pointer to idxb[ii] in qp_in + int *idxs_rev; // pointer to idxs_rev[ii] in qp_in + int *idxe; // pointer to idxe[ii] in qp_in +} ocp_nlp_constraints_bgh_memory; + +// +acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); +// +void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_nv; + struct blasfeo_dmat tmp_nz_nh; + struct blasfeo_dmat tmp_nv_nh; + struct blasfeo_dmat tmp_nz_nv; + struct blasfeo_dmat hess_z; + struct blasfeo_dvec tmp_ni; + struct blasfeo_dvec tmp_nh; +} ocp_nlp_constraints_bgh_workspace; + +// +acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts); + +/* functions */ + +// +void ocp_nlp_constraints_bgh_config_initialize_default(void *config); +// +void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts, + void *mem, void *work); +// +void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + +// +void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h new file mode 100644 index 0000000000..beeec78411 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -0,0 +1,221 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/* dims */ + +typedef struct +{ + int nx; + int nu; + int nz; + int nb; // nbx + nbu + int nbu; + int nbx; + int ng; // number of general linear constraints + int nphi; // dimension of convex outer part + int ns; // nsbu + nsbx + nsg + nsphi + int nsbu; // number of softened input bounds + int nsbx; // number of softened state bounds + int nsg; // number of softened general linear constraints + int nsphi; // number of softened nonlinear constraints + int nr; // dimension of nonlinear function in convex_over_nonlinear constraint + int nbue; // number of input box constraints which are equality + int nbxe; // number of state box constraints which are equality + int nge; // number of general linear constraints which are equality + int nphie; // number of nonlinear path constraints which are equality +} ocp_nlp_constraints_bgp_dims; + +// +acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config); +// +void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_constraints_bgp_dims_initialize(void *config, void *dims, int nx, int nu, int nz, + int nbx, int nbu, int ng, int nphi, int nq, int ns); +// +void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value); + + +/* model */ + +typedef struct +{ + // ocp_nlp_constraints_bgp_dims *dims; + int *idxb; + int *idxs; + int *idxe; + struct blasfeo_dvec d; + struct blasfeo_dmat DCt; + external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux; + external_function_generic *nl_constr_phi_o_r_fun; + external_function_generic *nl_constr_r_fun_jac; +} ocp_nlp_constraints_bgp_model; + +// +acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, + void *model_, const char *field, void *value); + +/* options */ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_constraints_bgp_opts; + +// +acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value); + +/* memory */ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dvec *lam; // pointer to lam in nlp_out + struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out + struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dmat *DCt; // pointer to DCt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory + int *idxb; // pointer to idxb[ii] in qp_in + int *idxs_rev; // pointer to idxs_rev[ii] in qp_in + int *idxe; // pointer to idxe[ii] in qp_in +} ocp_nlp_constraints_bgp_memory; + +// +acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts, + void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); +// +void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +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_); +// +void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_); + +/* workspace */ + +typedef struct +{ + struct blasfeo_dvec tmp_ni; + struct blasfeo_dmat jac_r_ux_tran; + struct blasfeo_dmat tmp_nr_nphi_nr; + struct blasfeo_dmat tmp_nv_nr; + struct blasfeo_dmat tmp_nv_nphi; + struct blasfeo_dmat tmp_nz_nphi; +} ocp_nlp_constraints_bgp_workspace; + +// +acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts); + +/* functions */ + +// +void ocp_nlp_constraints_bgp_config_initialize_default(void *config); +// +void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model, + void *opts, void *mem, void *work); +// +void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims, + void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims, + void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h new file mode 100644 index 0000000000..7cadecab46 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -0,0 +1,112 @@ +/* + * 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.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_constraints ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng, + int nh, int nq, int ns); + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts); + void (*opts_update)(void *config, void *dims, void *opts); + void (*opts_set)(void *config, void *opts, char *field, void *value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory); + struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); + void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory); + void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory); + void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); + 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); + void (*memory_set_idxe_ptr)(int *idxe, void *memory); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*config_initialize_default)(void *config); + // dimension setters + void (*dims_set)(void *config_, void *dims_, const char *field, const int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int* value); +} ocp_nlp_constraints_config; + +// +acados_size_t ocp_nlp_constraints_config_calculate_size(); +// +ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h new file mode 100644 index 0000000000..c9fbbfb404 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -0,0 +1,109 @@ +/* + * 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.; + */ + + +/// +/// \defgroup ocp_nlp_cost ocp_nlp_cost +/// + +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); + void (*dims_set)(void *config_, void *dims_, const char *field, int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int *value); + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts); + void (*opts_update)(void *config, void *dims, void *opts); + void (*opts_set)(void *config, void *opts, const char *field, void *value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + double *(*memory_get_fun_ptr)(void *memory); + struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); + void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); + void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + + // computes the function value, gradient and hessian (approximation) of the cost function + void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + // computes the cost function value (intended for globalization) + void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*config_initialize_default)(void *config); +} ocp_nlp_cost_config; + +// +acados_size_t ocp_nlp_cost_config_calculate_size(); +// +ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h new file mode 100644 index 0000000000..f2196dbee7 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -0,0 +1,187 @@ +/* + * 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_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states + int nu; // number of inputs + int ns; // number of slacks +} ocp_nlp_cost_external_dims; + +// +acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config); +// +void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx, + int nu, int ny, int ns, int nz); +// +void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value); + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + external_function_generic *ext_cost_fun; // function + external_function_generic *ext_cost_fun_jac_hess; // function, gradient and hessian + external_function_generic *ext_cost_fun_jac; // function, gradient + struct blasfeo_dvec Z; + struct blasfeo_dvec z; + struct blasfeo_dmat numerical_hessian; // custom hessian approximation + double scaling; +} ocp_nlp_cost_external_model; + +// +acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation +} ocp_nlp_cost_external_opts; + +// +acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec grad; // gradient of cost function + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + double fun; ///< value of the cost function +} ocp_nlp_cost_external_memory; + +// +acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_nv; + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns +} ocp_nlp_cost_external_workspace; + +// +acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_cost_external_config_initialize_default(void *config); +// +void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_, + void *opts_, void *mem_, void *work_); +// +void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h new file mode 100644 index 0000000000..3cf759504a --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -0,0 +1,259 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls +/// \brief This module implements linear-least squares costs of the form +/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$. +/// @{ + + + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + +//////////////////////////////////////////////////////////////////////////////// +// dims // +//////////////////////////////////////////////////////////////////////////////// + +typedef struct +{ + int nx; // number of states + int nz; // number of algebraic variables + int nu; // number of inputs + int ny; // number of outputs + int ns; // number of slacks +} ocp_nlp_cost_ls_dims; + + +/// Calculate the size of the ocp_nlp_cost_ls_dims struct +/// +/// \param[in] config_ structure containing configuration of ocp_nlp_cost +/// module +/// \param[out] [] +/// \return \c size of ocp_nlp_dims struct +acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config); + + +/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct +/// +/// \param[in] config structure containing configuration of ocp_nlp_cost +/// module +/// \param[in] raw_memory pointer to memory location +/// \param[out] [] +/// \return dims +void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory); + + +/// Initialize the dimensions struct of the +/// ocp_nlp-cost_ls component +/// +/// \param[in] config structure containing configuration ocp_nlp-cost_ls component +/// \param[in] nx number of states +/// \param[in] nu number of inputs +/// \param[in] ny number of residuals +/// \param[in] ns number of slacks +/// \param[in] nz number of algebraic variables +/// \param[out] dims +/// \return size +void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx, + int nu, int ny, int ns, int nz); + +// +void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value); + + +//////////////////////////////////////////////////////////////////////////////// +// model // +//////////////////////////////////////////////////////////////////////////////// + + +/// structure containing the data describing the linear least-square cost +typedef struct +{ + // slack penalty has the form z^T * s + .5 * s^T * Z * s + struct blasfeo_dmat Cyt; ///< output matrix: Cy * [u,x] = y; in transposed form + struct blasfeo_dmat Vz; ///< Vz in ls cost Vx*x + Vu*u + Vz*z + struct blasfeo_dmat W; ///< ls norm corresponding to this matrix + struct blasfeo_dvec y_ref; ///< yref + struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper) + struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper) + double scaling; +} ocp_nlp_cost_ls_model; + +// +acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_, + const char *field, void *value_); + + + +//////////////////////////////////////////////////////////////////////////////// +// options // +//////////////////////////////////////////////////////////////////////////////// + + + +typedef struct +{ + int dummy; // struct can't be void +} ocp_nlp_cost_ls_opts; + +// +acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value); + + + +//////////////////////////////////////////////////////////////////////////////// +// memory // +//////////////////////////////////////////////////////////////////////////////// + + + +/// structure containing the memory associated with cost_ls component +/// of the ocp_nlp module +typedef struct +{ + struct blasfeo_dmat hess; ///< hessian of cost function + struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix + struct blasfeo_dvec res; ///< ls residual r(x) + struct blasfeo_dvec grad; ///< gradient of cost function + struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; ///< pointer to Z in qp_in + double fun; ///< value of the cost function +} ocp_nlp_cost_ls_memory; + +// +acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + + + +//////////////////////////////////////////////////////////////////////////////// +// workspace // +//////////////////////////////////////////////////////////////////////////////// + + + +typedef struct +{ + struct blasfeo_dmat tmp_nv_ny; // temporary matrix of dimensions nv, ny + struct blasfeo_dmat Cyt_tilde; // updated Cyt (after z elimination) + struct blasfeo_dmat dzdux_tran; // derivatives of z wrt u and x (tran) + struct blasfeo_dvec tmp_ny; // temporary vector of dimension ny + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny + struct blasfeo_dvec tmp_nz; // temporary vector of dimension nz + struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination) +} ocp_nlp_cost_ls_workspace; + +// +acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts); + + + +//////////////////////////////////////////////////////////////////////////////// +// functions // +//////////////////////////////////////////////////////////////////////////////// + + + +// +void ocp_nlp_cost_ls_config_initialize_default(void *config); +// +void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_, + void *mem_, void *work_); +// +void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ +/// @} +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h new file mode 100644 index 0000000000..aafb6b3540 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -0,0 +1,210 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls +/// \brief This module implements nonlinear-least squares costs of the form +/// \f$\min_{x,u} \| r(x,u) - y_{\text{ref}} \|_W^2\f$. +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states + int nu; // number of inputs + int ny; // number of outputs + int ns; // number of slacks +} ocp_nlp_cost_nls_dims; + +// +acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config); +// +void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); +// +void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + // nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost + // slack penalty has the form z^T * s + .5 * s^T * Z * s + external_function_generic *nls_y_fun; // evaluation of nls function + external_function_generic *nls_y_fun_jac; // evaluation nls function and jacobian + external_function_generic *nls_y_hess; // hessian*seeds of nls residuals + struct blasfeo_dmat W; // + struct blasfeo_dvec y_ref; + struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector + struct blasfeo_dvec z; // gradient of slacks as vector + double scaling; +} ocp_nlp_cost_nls_model; + +// +acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + bool gauss_newton_hess; // gauss-newton hessian approximation +} ocp_nlp_cost_nls_opts; + +// +acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat W_chol; // cholesky factor of weight matrix + struct blasfeo_dmat Jt; // jacobian of nls fun + struct blasfeo_dvec res; // nls residual r(x) + struct blasfeo_dvec grad; // gradient of cost function + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + double fun; ///< value of the cost function +} ocp_nlp_cost_nls_memory; + +// +acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_ny; + struct blasfeo_dmat tmp_nv_nv; + struct blasfeo_dvec tmp_ny; + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny +} ocp_nlp_cost_nls_workspace; + +// +acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_cost_nls_config_initialize_default(void *config); +// +void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); +// +void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ +/// @} +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h new file mode 100644 index 0000000000..45dac20a25 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -0,0 +1,122 @@ +/* + * 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.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + void (*config_initialize_default)(void *config); + sim_config *sim_solver; + /* dims */ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz); + void (*dims_set)(void *config_, void *dims_, const char *field, int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int* value); + /* model */ + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_); + /* opts */ + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void *value); + void (*opts_update)(void *config, void *dims, void *opts); + /* memory */ + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_); + struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_); + void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_); + void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_); + void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_); + void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_); + void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_); + void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_); + void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_); + void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value); + /* workspace */ + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); +} ocp_nlp_dynamics_config; + +// +acados_size_t ocp_nlp_dynamics_config_calculate_size(); +// +ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h new file mode 100644 index 0000000000..59a2df4f47 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -0,0 +1,216 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" +#include "acados_c/sim_interface.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + void *sim; + int nx; // number of states at the current stage + int nz; // number of algebraic states at the current stage + int nu; // number of inputs at the current stage + int nx1; // number of states at the next stage + int nu1; // number of inputes at the next stage +} ocp_nlp_dynamics_cont_dims; + +// +acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config); +// +void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_cont_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, + int nu1, int nz); + +// +void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value); + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + void *sim_solver; + int compute_adj; + int compute_hess; +} ocp_nlp_dynamics_cont_opts; + +// +acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage + struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage + struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage + struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage + struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage + struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *z_alg; // pointer to output z at t = 0 + bool *set_sim_guess; // indicate if initialization for integrator is set from outside + struct blasfeo_dvec *sim_guess; // initializations for integrator + // struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage + struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed + void *sim_solver; // sim solver memory +} ocp_nlp_dynamics_cont_memory; + +// +acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat hess; + sim_in *sim_in; + sim_out *sim_out; + void *sim_solver; // sim solver workspace +} ocp_nlp_dynamics_cont_workspace; + +acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + void *sim_model; + // double *state_transition; // TODO + double T; // simulation time +} ocp_nlp_dynamics_cont_model; + +// +acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); + + + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_dynamics_cont_config_initialize_default(void *config); +// +void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h new file mode 100644 index 0000000000..8b2a6177bf --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h @@ -0,0 +1,199 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states at the current stage + int nu; // number of inputs at the current stage + int nx1; // number of states at the next stage + int nu1; // number of inputes at the next stage +} ocp_nlp_dynamics_disc_dims; + +// +acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config); +// +void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_disc_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, + int nu1, int nz); + +// +void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value); + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_dynamics_disc_opts; + +// +acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts); +// +int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_, + void *mem_, void *work_); + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage + struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage + struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage + struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage + struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage + struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_dynamics_disc_memory; + +// +acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_nv; +} ocp_nlp_dynamics_disc_workspace; + +acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + external_function_generic *disc_dyn_fun; + external_function_generic *disc_dyn_fun_jac; + external_function_generic *disc_dyn_fun_jac_hess; +} ocp_nlp_dynamics_disc_model; + +// +acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); + + + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_dynamics_disc_config_initialize_default(void *config); +// +void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h new file mode 100644 index 0000000000..cd26788a56 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -0,0 +1,125 @@ +/* + * 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.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_reg ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" + + + +/* dims */ + +//typedef ocp_qp_dims ocp_nlp_reg_dims; +typedef struct +{ + int *nx; + int *nu; + int *nbu; + int *nbx; + int *ng; + int N; +} ocp_nlp_reg_dims; + +// +acados_size_t ocp_nlp_reg_dims_calculate_size(int N); +// +ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory); +// +void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value); + + + +/* config */ + +typedef struct +{ + /* dims */ + acados_size_t (*dims_calculate_size)(int N); + ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory); + void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value); + /* opts */ + acados_size_t (*opts_calculate_size)(void); + void *(*opts_assign)(void *raw_memory); + void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts); + void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value); + /* memory */ + acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts); + void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value); + void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); + void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); + void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory); + void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); + void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + /* functions */ + void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); + void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); +} ocp_nlp_reg_config; + +// +acados_size_t ocp_nlp_reg_config_calculate_size(void); +// +void *ocp_nlp_reg_config_assign(void *raw_memory); + + + +/* regularization help functions */ +void acados_reconstruct_A(int dim, double *A, double *V, double *d); +void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon); +void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h new file mode 100644 index 0000000000..df31361680 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -0,0 +1,149 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double delta; + double epsilon; +// double gamma; // 0.0 +} ocp_nlp_reg_convexify_opts; + +// +acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void); +// +void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct { + double *R; + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + double *reg_hess; // TODO move to workspace + + struct blasfeo_dmat Q_tilde; + struct blasfeo_dmat Q_bar; + struct blasfeo_dmat BAQ; + struct blasfeo_dmat L; + struct blasfeo_dmat delta_eye; + struct blasfeo_dmat St_copy; + + struct blasfeo_dmat *original_RSQrq; + struct blasfeo_dmat tmp_RSQ; + + struct blasfeo_dvec tmp_nuxM; + struct blasfeo_dvec tmp_nbgM; + +// struct blasfeo_dvec grad; +// struct blasfeo_dvec b2; + + // giaf's + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec **rq; // pointer to rq in qp_in + struct blasfeo_dmat **BAbt; // pointer to BAbt in qp_in + struct blasfeo_dvec **b; // pointer to b in qp_in + struct blasfeo_dmat **DCt; // pointer to DCt in qp_in + struct blasfeo_dvec **ux; // pointer to ux in qp_out + struct blasfeo_dvec **pi; // pointer to pi in qp_out + struct blasfeo_dvec **lam; // pointer to lam in qp_out + int **idxb; // pointer to idxb in qp_in + +} ocp_nlp_reg_convexify_memory; + +// +acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h new file mode 100644 index 0000000000..f6bd7dcafd --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h @@ -0,0 +1,124 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_mirror_opts; + +// +acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void); +// +void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_mirror_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + double *reg_hess; // TODO move to workspace + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + + // giaf's + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_mirror_memory; + +// +acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h new file mode 100644 index 0000000000..8d2b6ecc13 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h @@ -0,0 +1,120 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int dummy; +} ocp_nlp_reg_noreg_opts; + +// +acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void); +// +void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + int dummy; +} ocp_nlp_reg_noreg_memory; + +// +acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + +// not needed + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ + +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h new file mode 100644 index 0000000000..104c297207 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h @@ -0,0 +1,124 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_project_opts; + +// +acados_size_t ocp_nlp_reg_project_opts_calculate_size(void); +// +void *ocp_nlp_reg_project_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_project_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + double *reg_hess; // TODO move to workspace + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + + // giaf's + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_project_memory; + +// +acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h new file mode 100644 index 0000000000..e0b854bc1a --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -0,0 +1,135 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double thr_eig; + double min_eig; + double min_pivot; + int pivoting; +} ocp_nlp_reg_project_reduc_hess_opts; + +// +acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void); +// +void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + double *reg_hess; // TODO move to workspace + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + + // giaf's + struct blasfeo_dmat L; // TODO move to workspace + struct blasfeo_dmat L2; // TODO move to workspace + struct blasfeo_dmat L3; // TODO move to workspace + struct blasfeo_dmat Ls; // TODO move to workspace + struct blasfeo_dmat P; // TODO move to workspace + struct blasfeo_dmat AL; // TODO move to workspace + + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat **BAbt; // pointer to RSQrq in qp_in +} ocp_nlp_reg_project_reduc_hess_memory; + +// +acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h new file mode 100644 index 0000000000..81a4afd030 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h @@ -0,0 +1,171 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_solver +/// @{ +/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_ +#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/types.h" + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + ocp_nlp_opts *nlp_opts; + double tol_stat; // exit tolerance on stationarity condition + double tol_eq; // exit tolerance on equality constraints + double tol_ineq; // exit tolerance on inequality constraints + double tol_comp; // exit tolerance on complementarity condition + int max_iter; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) + int qp_warm_start; // qp_warm_start in all but the first sqp iterations + bool warm_start_first_qp; // to set qp_warm_start in first iteration + int rti_phase; // only phase 0 at the moment + int print_level; // verbosity + int initialize_t_slacks; // 0-false or 1-true + +} ocp_nlp_sqp_opts; + +// +acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value); +// +void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + // nlp memory + ocp_nlp_memory *nlp_mem; + + double time_qp_sol; + double time_qp_solver_call; + double time_qp_xcond; + double time_lin; + double time_reg; + double time_tot; + double time_glob; + double time_sim; + double time_sim_la; + double time_sim_ad; + + // statistics + double *stat; + int stat_m; + int stat_n; + + int status; + int sqp_iter; + +} ocp_nlp_sqp_memory; + +// +acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + ocp_nlp_workspace *nlp_work; + + // temp QP in & out (to be used as workspace in param sens) + ocp_qp_in *tmp_qp_in; + ocp_qp_out *tmp_qp_out; + + // qp residuals + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; + +} ocp_nlp_sqp_workspace; + +// +acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_); + + + +/************************************************ + * functions + ************************************************/ + +// +int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out, + void *args, void *mem, void *work_); +// +void ocp_nlp_sqp_config_initialize_default(void *config_); +// +int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, + void *opts_, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_H_ +/// @} +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h new file mode 100644 index 0000000000..6f16594d2b --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -0,0 +1,172 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_solver +/// @{ +/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ +#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/types.h" + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + ocp_nlp_opts *nlp_opts; + int compute_dual_sol; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) + int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp. + bool warm_start_first_qp; // to set qp_warm_start in first iteration + int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both) + int print_level; // verbosity + +} ocp_nlp_sqp_rti_opts; + +// +acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_); +// +void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory); +// +void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_); +// +void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_); +// +void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value); +// +void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage, + const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + // nlp memory + ocp_nlp_memory *nlp_mem; + + double time_qp_sol; + double time_qp_solver_call; + double time_qp_xcond; + double time_lin; + double time_reg; + double time_tot; + double time_glob; + + // statistics + double *stat; + int stat_m; + int stat_n; + + int status; + +} ocp_nlp_sqp_rti_memory; + +// +acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_); +// +void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, + void *raw_memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + ocp_nlp_workspace *nlp_work; + + // temp QP in & out (to be used as workspace in param sens) + ocp_qp_in *tmp_qp_in; + ocp_qp_out *tmp_qp_out; + + // qp residuals + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; + + +} ocp_nlp_sqp_rti_workspace; + +// +acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_); + + + +/************************************************ + * functions + ************************************************/ + +void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_); +// +void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); +// +int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, + void *opts_, void *mem_, void *work_); +// +void ocp_nlp_sqp_rti_config_initialize_default(void *config_); +// +int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ +/// @} +/// @} +/// @} diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common.h new file mode 100644 index 0000000000..bba55f8f75 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common.h @@ -0,0 +1,182 @@ +/* + * 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_OCP_QP_OCP_QP_COMMON_H_ +#define ACADOS_OCP_QP_OCP_QP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp.h" +#include "hpipm/include/hpipm_d_ocp_qp_dim.h" +#include "hpipm/include/hpipm_d_ocp_qp_res.h" +#include "hpipm/include/hpipm_d_ocp_qp_sol.h" +// acados +#include "acados/utils/types.h" + + + +typedef struct d_ocp_qp_dim ocp_qp_dims; +typedef struct d_ocp_qp ocp_qp_in; +typedef struct d_ocp_qp_sol ocp_qp_out; +typedef struct d_ocp_qp_res ocp_qp_res; +typedef struct d_ocp_qp_res_ws ocp_qp_res_ws; + + + +#ifndef QP_SOLVER_CONFIG_ +#define QP_SOLVER_CONFIG_ +typedef struct +{ + void (*dims_set)(void *config_, void *dims_, int stage, const char *field, int* value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts); + void (*opts_update)(void *config, void *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} qp_solver_config; +#endif + + + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config, int N); + void *(*dims_assign)(void *config, int N, void *raw_memory); + void (*dims_set)(void *config, void *dims_, int stage, const char *field, int* value); + void (*dims_get)(void *config, void *dims, const char *field, void* value); + // TODO add config everywhere !!!!! + acados_size_t (*opts_calculate_size)(void *dims); + void *(*opts_assign)(void *dims, void *raw_memory); + void (*opts_initialize_default)(void *dims, void *opts); + void (*opts_update)(void *dims, void *opts); + void (*opts_set)(void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *dims, void *opts); + void *(*memory_assign)(void *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config, void *mem, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *dims, void *opts); + int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} ocp_qp_xcond_config; + + + +/// Struct containing metrics of the qp solver. +#ifndef QP_INFO_ +#define QP_INFO_ +typedef struct +{ + double solve_QP_time; + double condensing_time; + double interface_time; + double total_time; + int num_iter; + int t_computed; +} qp_info; +#endif + + + +/* config */ +// +acados_size_t ocp_qp_solver_config_calculate_size(); +// +qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory); +// +acados_size_t ocp_qp_condensing_config_calculate_size(); +// +ocp_qp_xcond_config *ocp_qp_condensing_config_assign(void *raw_memory); + + +/* dims */ +// +acados_size_t ocp_qp_dims_calculate_size(int N); +// +ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory); +// +void ocp_qp_dims_set(void *config_, void *dims, int stage, const char *field, int* value); +// +void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, int* value); + + +/* in */ +// +acados_size_t ocp_qp_in_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_in *ocp_qp_in_assign(ocp_qp_dims *dims, void *raw_memory); + + +/* out */ +// +acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory); + +/* res */ +// +acados_size_t ocp_qp_res_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory); +// +acados_size_t ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws); +// +void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]); + + +/* misc */ +// +void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out); +// +void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out); +// +void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_COMMON_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h new file mode 100644 index 0000000000..50b80850c6 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h @@ -0,0 +1,121 @@ +/* + * 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_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ +#define ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" + +typedef struct +{ + int N; + int *nx; + int *nu; + int *nb; + int *nc; + double **A; + double **B; + double **b; + double **Q; + double **S; + double **R; + double **q; + double **r; + int **idxb; + double **lb; + double **ub; + double **Cx; + double **Cu; + double **lc; + double **uc; +} colmaj_ocp_qp_in; + +typedef struct +{ + double **x; + double **u; + double **pi; + double **lam; +} colmaj_ocp_qp_out; + +typedef struct +{ + double **res_r; + double **res_q; + double **res_ls; + double **res_us; + double **res_b; + double **res_d_lb; + double **res_d_ub; + double **res_d_lg; + double **res_d_ug; + double **res_d_ls; + double **res_d_us; + double **res_m_lb; + double **res_m_ub; + double **res_m_lg; + double **res_m_ug; + double **res_m_ls; + double **res_m_us; + double res_nrm_inf[4]; +} colmaj_ocp_qp_res; + +// +acados_size_t colmaj_ocp_qp_in_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_in(ocp_qp_dims *dims, colmaj_ocp_qp_in **qp_in, void *ptr); +// +acados_size_t colmaj_ocp_qp_out_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_out(ocp_qp_dims *dims, colmaj_ocp_qp_out **qp_out, void *ptr); +// +acados_size_t colmaj_ocp_qp_res_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_res(ocp_qp_dims *dims, colmaj_ocp_qp_res **qp_res, void *ptr); +// +void convert_colmaj_to_ocp_qp_in(colmaj_ocp_qp_in *cm_qp_in, ocp_qp_in *qp_in); +// +void convert_ocp_qp_out_to_colmaj(ocp_qp_out *qp_out, colmaj_ocp_qp_out *cm_qp_out); +// +void convert_ocp_qp_res_to_colmaj(ocp_qp_res *qp_res, colmaj_ocp_qp_res *cm_qp_res); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h new file mode 100644 index 0000000000..14ac97bbf4 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h @@ -0,0 +1,117 @@ +/* + * 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_OCP_QP_OCP_QP_FULL_CONDENSING_H_ +#define ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_red.h" +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + ocp_qp_dims *red_dims; // dims of reduced qp + dense_qp_dims *fcond_dims; +} ocp_qp_full_condensing_dims; + + + +typedef struct ocp_qp_full_condensing_opts_ +{ + struct d_cond_qp_arg *hpipm_cond_opts; + struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts; +// dense_qp_dims *fcond_dims; // TODO(all): move to dims + int cond_hess; // 0 cond only rhs, 1 cond hess + rhs + int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol + int ric_alg; + int mem_qp_in; // allocate qp_in in memory +} ocp_qp_full_condensing_opts; + + + +typedef struct ocp_qp_full_condensing_memory_ +{ + struct d_cond_qp_ws *hpipm_cond_work; + struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work; + // in memory + dense_qp_in *fcond_qp_in; + dense_qp_out *fcond_qp_out; + ocp_qp_in *red_qp; // reduced qp + ocp_qp_out *red_sol; // reduced qp sol + // only pointer + ocp_qp_in *ptr_qp_in; + qp_info *qp_out_info; // info in fcond_qp_in + double time_qp_xcond; +} ocp_qp_full_condensing_memory; + + + +// +acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims); +// +void *ocp_qp_full_condensing_opts_assign(void *dims, void *raw_memory); +// +void ocp_qp_full_condensing_opts_initialize_default(void *dims, void *opts_); +// +void ocp_qp_full_condensing_opts_update(void *dims, void *opts_); +// +void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value); +// +acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims, void *opts_); +// +void *ocp_qp_full_condensing_memory_assign(void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void *opts_); +// +int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work); +// +void ocp_qp_full_condensing_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpipm.h new file mode 100644 index 0000000000..84e874863f --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpipm.h @@ -0,0 +1,100 @@ +/* + * 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_OCP_QP_OCP_QP_HPIPM_H_ +#define ACADOS_OCP_QP_OCP_QP_HPIPM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_ipm.h" +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + + + +// struct of arguments to the solver +// TODO(roversch): why not make this a typedef of the underlying struct? +typedef struct ocp_qp_hpipm_opts_ +{ + struct d_ocp_qp_ipm_arg *hpipm_opts; +} ocp_qp_hpipm_opts; + + + +// TODO(roversch): why not make this a typedef of the underlying struct? +// struct of the solver memory +typedef struct ocp_qp_hpipm_memory_ +{ + struct d_ocp_qp_ipm_ws *hpipm_workspace; + double time_qp_solver_call; + int iter; + +} ocp_qp_hpipm_memory; + + + +// +acados_size_t ocp_qp_hpipm_opts_calculate_size(void *config, void *dims); +// +void *ocp_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_); +// +void ocp_qp_hpipm_opts_update(void *config, void *dims, void *opts_); +// +void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value); +// +acados_size_t ocp_qp_hpipm_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *ocp_qp_hpipm_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_hpipm_workspace_calculate_size(void *config, void *dims, void *opts_); +// +int ocp_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpipm_config_initialize_default(void *config); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_HPIPM_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h new file mode 100644 index 0000000000..37d1c0a337 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h @@ -0,0 +1,129 @@ +/* + * 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_OCP_QP_OCP_QP_HPMPC_H_ +#define ACADOS_OCP_QP_OCP_QP_HPMPC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef enum hpmpc_options_t_ { HPMPC_DEFAULT_ARGUMENTS } hpmpc_options_t; + +typedef struct ocp_qp_hpmpc_opts_ +{ + double tol; + int max_iter; + double mu0; + double alpha_min; + int warm_start; + int N2; // horizion length of the partially condensed problem + + // partial tightening + double sigma_mu; + int N; + int M; +} ocp_qp_hpmpc_opts; + +// struct of the solver memory +typedef struct ocp_qp_hpmpc_memory_ +{ + struct blasfeo_dvec *hpi; + double *stats; + + // workspace + void *hpmpc_work; // raw workspace + + // partial tightening-specific (init of extra variables) + struct blasfeo_dvec *lam0; + struct blasfeo_dvec *ux0; + struct blasfeo_dvec *pi0; + struct blasfeo_dvec *t0; + + // 2. workspace + struct blasfeo_dmat *hsL; + struct blasfeo_dmat *hsric_work_mat; + struct blasfeo_dmat sLxM; + struct blasfeo_dmat sPpM; + + struct blasfeo_dvec *hsQx; + struct blasfeo_dvec *hsqx; + struct blasfeo_dvec *hstinv; + struct blasfeo_dvec *hsrq; + struct blasfeo_dvec *hsdux; + + struct blasfeo_dvec *hsdlam; + struct blasfeo_dvec *hsdt; + struct blasfeo_dvec *hsdpi; + struct blasfeo_dvec *hslamt; + + struct blasfeo_dvec *hsPb; + + void *work_ric; + + int out_iter; + + double time_qp_solver_call; + int iter; + +} ocp_qp_hpmpc_memory; + +acados_size_t ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_hpmpc_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_hpmpc_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_hpmpc_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpmpc_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_HPMPC_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_ooqp.h new file mode 100644 index 0000000000..e1b789b96a --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_ooqp.h @@ -0,0 +1,146 @@ +/* + * 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_OCP_QP_OCP_QP_OOQP_H_ +#define ACADOS_OCP_QP_OCP_QP_OOQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +enum ocp_qp_ooqp_termination_code +{ + SPARSE_SUCCESSFUL_TERMINATION = 0, + SPARSE_NOT_FINISHED, + SPARSE_MAX_ITS_EXCEEDED, + SPARSE_INFEASIBLE, + SPARSE_UNKNOWN +}; + +typedef struct ocp_qp_ooqp_opts_ +{ + int printLevel; + int useDiagonalWeights; // TODO(dimitris): implement option + int fixHessian; + int fixHessianSparsity; + int fixDynamics; + int fixDynamicsSparsity; + int fixInequalities; + int fixInequalitiesSparsity; +} ocp_qp_ooqp_opts; + +typedef struct ocp_qp_ooqp_workspace_ +{ + double *x; + double *gamma; + double *phi; + double *y; + double *z; + double *lambda; + double *pi; + double objectiveValue; + int *tmpInt; // temporary vector to sort indicies sparse matrices + double *tmpReal; // temporary vector to sort data of sparse matrices + // int ierr; +} ocp_qp_ooqp_workspace; + +typedef struct ocp_qp_ooqp_memory_ +{ + int firstRun; + double *c; + int nx; + int *irowQ; + int nnzQ; + int *jcolQ; + int *orderQ; + double *dQ; + double *xlow; + char *ixlow; + double *xupp; + char *ixupp; + int *irowA; + int nnzA; + int *jcolA; + int *orderA; + double *dA; + double *bA; + int my; + int *irowC; + int nnzC; + int *jcolC; + int *orderC; + double *dC; + double *clow; + int mz; + char *iclow; + double *cupp; + char *icupp; + int nnz; // max(nnzQ, nnzA, nnzC) + double time_qp_solver_call; + int iter; + +} ocp_qp_ooqp_memory; + +// +acados_size_t ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_, + void *work_); +// +void ocp_qp_ooqp_destroy(void *mem_, void *work); +// +void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_ooqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_OOQP_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_osqp.h new file mode 100644 index 0000000000..55321a46b2 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_osqp.h @@ -0,0 +1,105 @@ +/* + * 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_OCP_QP_OCP_QP_OSQP_H_ +#define ACADOS_OCP_QP_OCP_QP_OSQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// osqp +#include "osqp/include/types.h" + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef struct ocp_qp_osqp_opts_ +{ + OSQPSettings *osqp_opts; +} ocp_qp_osqp_opts; + + +typedef struct ocp_qp_osqp_memory_ +{ + c_int first_run; + + c_float *q; + c_float *l; + c_float *u; + + c_int P_nnzmax; + c_int *P_i; + c_int *P_p; + c_float *P_x; + + c_int A_nnzmax; + c_int *A_i; + c_int *A_p; + c_float *A_x; + + OSQPData *osqp_data; + OSQPWorkspace *osqp_work; + + double time_qp_solver_call; + int iter; + +} ocp_qp_osqp_memory; + +acados_size_t ocp_qp_osqp_opts_calculate_size(void *config, void *dims); +// +void *ocp_qp_osqp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_qp_osqp_opts_initialize_default(void *config, void *dims, void *opts_); +// +void ocp_qp_osqp_opts_update(void *config, void *dims, void *opts_); +// +acados_size_t ocp_qp_osqp_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *ocp_qp_osqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, void *opts_); +// +int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_osqp_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_OSQP_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h new file mode 100644 index 0000000000..b95a11114e --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -0,0 +1,123 @@ +/* + * 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_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ +#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_red.h" +// acados +#include "acados/ocp_qp/ocp_qp_common.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + ocp_qp_dims *red_dims; // dims of reduced qp + ocp_qp_dims *pcond_dims; + int *block_size; + int N2; + int N2_bkp; +} ocp_qp_partial_condensing_dims; + + + +typedef struct ocp_qp_partial_condensing_opts_ +{ + struct d_part_cond_qp_arg *hpipm_pcond_opts; + struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts; +// ocp_qp_dims *pcond_dims; // TODO(all): move to dims +// int *block_size; + int N2; + int N2_bkp; +// int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol + int ric_alg; + int mem_qp_in; // allocate qp_in in memory +} ocp_qp_partial_condensing_opts; + + + +typedef struct ocp_qp_partial_condensing_memory_ +{ + struct d_part_cond_qp_ws *hpipm_pcond_work; + struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work; + // in memory + ocp_qp_in *pcond_qp_in; + ocp_qp_out *pcond_qp_out; + ocp_qp_in *red_qp; // reduced qp + ocp_qp_out *red_sol; // reduced qp sol + // only pointer + ocp_qp_in *ptr_qp_in; + ocp_qp_in *ptr_pcond_qp_in; + qp_info *qp_out_info; // info in pcond_qp_in + double time_qp_xcond; +} ocp_qp_partial_condensing_memory; + + + +// +acados_size_t ocp_qp_partial_condensing_opts_calculate_size(void *dims); +// +void *ocp_qp_partial_condensing_opts_assign(void *dims, void *raw_memory); +// +void ocp_qp_partial_condensing_opts_initialize_default(void *dims, void *opts_); +// +void ocp_qp_partial_condensing_opts_update(void *dims, void *opts_); +// +void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value); +// +acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims, void *opts_); +// +void *ocp_qp_partial_condensing_memory_assign(void *dims, void *opts, void *raw_memory); +// +acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, void *opts_); +// +int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work); +// +void ocp_qp_partial_condensing_config_initialize_default(void *config_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h new file mode 100644 index 0000000000..348627c676 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h @@ -0,0 +1,121 @@ +/* + * 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_OCP_QP_OCP_QP_QPDUNES_H_ +#define ACADOS_OCP_QP_OCP_QP_QPDUNES_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "qpDUNES.h" + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef enum qpdunes_options_t_ { + QPDUNES_DEFAULT_ARGUMENTS, + QPDUNES_LINEAR_MPC, // TODO(dimitris): partly implemented + QPDUNES_NONLINEAR_MPC, // TODO(dimitris): not implemented yet + QPDUNES_ACADO_SETTINGS +} qpdunes_options_t; + +typedef enum { QPDUNES_WITH_QPOASES, QPDUNES_WITH_CLIPPING } qpdunes_stage_qp_solver_t; + +typedef struct ocp_qp_qpdunes_opts_ +{ + qpOptions_t options; + qpdunes_stage_qp_solver_t stageQpSolver; + int warmstart; // warmstart = 0: all multipliers set to zero, warmstart = 1: use previous mult. + bool isLinearMPC; +} ocp_qp_qpdunes_opts; + +typedef struct ocp_qp_qpdunes_memory_ +{ + int firstRun; + int nx; + int nu; + int nz; + int nDmax; // max(dims->ng) + qpData_t qpData; + double time_qp_solver_call; + int iter; + +} ocp_qp_qpdunes_memory; + +typedef struct ocp_qp_qpdunes_workspace_ +{ + double *H; + double *Q; + double *R; + double *S; + double *g; + double *ABt; + double *b; + double *Ct; + double *lc; + double *uc; + double *zLow; + double *zUpp; +} ocp_qp_qpdunes_workspace; + +// +acados_size_t ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_, + void *work_); +// +void ocp_qp_qpdunes_free_memory(void *mem_); +// +void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_qpdunes_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_QPDUNES_H_ diff --git a/phonelibs/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h new file mode 100644 index 0000000000..9d4f309bd9 --- /dev/null +++ b/phonelibs/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -0,0 +1,151 @@ +/* + * 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_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ +#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + void *xcond_dims; +} ocp_qp_xcond_solver_dims; + + + +typedef struct ocp_qp_xcond_solver_opts_ +{ + void *xcond_opts; + void *qp_solver_opts; +} ocp_qp_xcond_solver_opts; + + + +typedef struct ocp_qp_xcond_solver_memory_ +{ + void *xcond_memory; + void *solver_memory; + void *xcond_qp_in; + void *xcond_qp_out; +} ocp_qp_xcond_solver_memory; + + + +typedef struct ocp_qp_xcond_solver_workspace_ +{ + void *xcond_work; + void *qp_solver_work; +} ocp_qp_xcond_solver_workspace; + + + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config, int N); + ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory); + void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); + acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims); + void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void (*opts_update)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void *(*memory_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); + void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver + ocp_qp_xcond_config *xcond; +} ocp_qp_xcond_solver_config; // pcond - partial condensing or fcond - full condensing + + + +/* config */ +// +acados_size_t ocp_qp_xcond_solver_config_calculate_size(); +// +ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory); + +/* dims */ +// +acados_size_t ocp_qp_xcond_solver_dims_calculate_size(void *config, int N); +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config, int N, void *raw_memory); +// +void ocp_qp_xcond_solver_dims_set_(void *config, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); + +/* opts */ +// +acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims); +// +void *ocp_qp_xcond_solver_opts_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory); +// +void ocp_qp_xcond_solver_opts_initialize_default(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void ocp_qp_xcond_solver_opts_update(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void ocp_qp_xcond_solver_opts_set_(void *config_, void *opts_, const char *field, void* value); + +/* memory */ +// +acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void *ocp_qp_xcond_solver_memory_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_, void *raw_memory); + +/* workspace */ +// +acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); + +/* config */ +// +int ocp_qp_xcond_solver(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *mem_, void *work_); + +// +void ocp_qp_xcond_solver_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_collocation_utils.h b/phonelibs/acados/include/acados/sim/sim_collocation_utils.h new file mode 100644 index 0000000000..850dab50dc --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_collocation_utils.h @@ -0,0 +1,91 @@ +/* + * 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_SIM_SIM_COLLOCATION_UTILS_H_ +#define ACADOS_SIM_SIM_COLLOCATION_UTILS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + + + +enum Newton_type_collocation +{ + exact = 0, + simplified_in, + simplified_inis +}; + + + +typedef struct +{ + enum Newton_type_collocation type; + double *eig; + double *low_tria; + bool single; + bool freeze; + + double *transf1; + double *transf2; + + double *transf1_T; + double *transf2_T; +} Newton_scheme; + + + +// +acados_size_t gauss_nodes_work_calculate_size(int ns); +// +void gauss_nodes(int ns, double *nodes, void *raw_memory); +// +acados_size_t gauss_simplified_work_calculate_size(int ns); +// +void gauss_simplified(int ns, Newton_scheme *scheme, void *work); +// +acados_size_t butcher_table_work_calculate_size(int ns); +// +void butcher_table(int ns, double *nodes, double *b, double *A, void *work); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_common.h b/phonelibs/acados/include/acados/sim/sim_common.h new file mode 100644 index 0000000000..177f0893dd --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_common.h @@ -0,0 +1,221 @@ +/* + * 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_SIM_SIM_COMMON_H_ +#define ACADOS_SIM_SIM_COMMON_H_ + +#include + +#include "acados/sim/sim_collocation_utils.h" +#include "acados/utils/timing.h" +#include "acados/utils/types.h" + +#include "acados/utils/external_function_generic.h" + +// maximum number of integration stages +#define NS_MAX 15 + + + +typedef enum +{ + // ERK and LIFTED_ERK + EXPL_ODE_FUN, + EXPL_ODE_HES, // wrt x and u ??? + EXPL_VDE_FOR, + EXPL_VDE_ADJ, + // IRK + IMPL_ODE_FUN, + IMPL_ODE_FUN_JAC_X_XDOT, + IMPL_ODE_JAC_X_XDOT_U, + IMPL_ODE_FUN_JAC_X_XDOT_U, + IMPL_ODE_HESS, + // gnsf + PHI_FUN, + PHI_FUN_JAC_Y, + PHI_JAC_Y_UHAT, + LO_FUN, + GET_GNSF_MATRICES +} sim_function_t; + + + +typedef struct +{ + void *dims; + + double *x; // x[NX] - initial state value for simulation + double *u; // u[NU] - control - constant over simulation time + + double *S_forw; // forward seed [Sx, Su] + double *S_adj; // backward seed + + bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)] + + void *model; + + double T; // simulation time + +} sim_in; + + + +typedef struct +{ + double CPUtime; // in seconds + double LAtime; // in seconds + double ADtime; // in seconds + +} sim_info; + + + +typedef struct +{ + double *xn; // xn[NX] + double *S_forw; // S_forw[NX*(NX+NU)] + double *S_adj; // + double *S_hess; // + + double *zn; // z - algebraic variables - reported at start of simulation interval + double *S_algebraic; // sensitivities of reported value of algebraic variables w.r.t. + // initial stat & control (x_n,u) + + double *grad; // gradient correction + + sim_info *info; + +} sim_out; + + + +typedef struct +{ + int ns; // number of integration stages + + int num_steps; + int num_forw_sens; + + int tableau_size; // check that is consistent with ns + // only update when butcher tableau is changed + // kind of private -> no setter! + double *A_mat; + double *c_vec; + double *b_vec; + + bool sens_forw; + bool sens_adj; + bool sens_hess; + + bool output_z; // 1 -- if zn should be computed + bool sens_algebraic; // 1 -- if S_algebraic should be computed + bool exact_z_output; // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations + + // for explicit integrators: newton_iter == 0 && scheme == NULL + // && jac_reuse=false + int newton_iter; + bool jac_reuse; + Newton_scheme *scheme; + + // workspace + void *work; + +} sim_opts; + + + +typedef struct +{ + int (*evaluate)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work); + int (*precompute)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work); + // opts + acados_size_t (*opts_calculate_size)(void *config_, void *dims); + void *(*opts_assign)(void *config_, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config_, void *dims, void *opts); + void (*opts_update)(void *config_, void *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void *value); + void (*opts_get)(void *config_, void *opts_, const char *field, void *value); + // mem + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + int (*memory_set)(void *config, void *dims, void *mem, const char *field, void *value); + int (*memory_set_to_zero)(void *config, void *dims, void *opts, void *mem, const char *field); + void (*memory_get)(void *config, void *dims, void *mem, const char *field, void *value); + // work + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + // model + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + int (*model_set)(void *model, const char *field, void *value); + // config + void (*config_initialize_default)(void *config); + // dims + acados_size_t (*dims_calculate_size)(); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_set)(void *config, void *dims, const char *field, const int *value); + void (*dims_get)(void *config, void *dims, const char *field, int *value); + +} sim_config; + + + +/* config */ +// +acados_size_t sim_config_calculate_size(); +// +sim_config *sim_config_assign(void *raw_memory); + +/* in */ +// +acados_size_t sim_in_calculate_size(void *config, void *dims); +// +sim_in *sim_in_assign(void *config, void *dims, void *raw_memory); +// +int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value); + +/* out */ +// +acados_size_t sim_out_calculate_size(void *config, void *dims); +// +sim_out *sim_out_assign(void *config, void *dims, void *raw_memory); +// +int sim_out_get_(void *config, void *dims, sim_out *out, const char *field, void *value); + +/* opts */ +// +void sim_opts_set_(sim_opts *opts, const char *field, void *value); +// +void sim_opts_get_(sim_config *config, sim_opts *opts, const char *field, void *value); + +#endif // ACADOS_SIM_SIM_COMMON_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_erk_integrator.h b/phonelibs/acados/include/acados/sim/sim_erk_integrator.h new file mode 100644 index 0000000000..24a00c7077 --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_erk_integrator.h @@ -0,0 +1,143 @@ +/* + * 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_SIM_SIM_ERK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_ERK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + int nx; + int nu; + int nz; +} sim_erk_dims; + + + +typedef struct +{ + /* external functions */ + // explicit ode + external_function_generic *expl_ode_fun; + // hessian explicit ode + external_function_generic *expl_ode_hes; + // forward explicit vde + external_function_generic *expl_vde_for; + // adjoint explicit vde + external_function_generic *expl_vde_adj; + +} erk_model; + + + +typedef struct +{ + // memory + double time_sim; + double time_ad; + double time_la; + + // workspace structs +} sim_erk_memory; + + + +typedef struct +{ + // workspace mem + double *rhs_forw_in; // x + S + p + + double *K_traj; // (stages*nX) or (steps*stages*nX) for adj + double *out_forw_traj; // S or (steps+1)*nX for adj + + double *rhs_adj_in; + double *out_adj_tmp; + double *adj_traj; + +} sim_erk_workspace; + + + +// dims +acados_size_t sim_erk_dims_calculate_size(); +void *sim_erk_dims_assign(void *config_, void *raw_memory); +void sim_erk_dims_set(void *config_, void *dims_, const char *field, const int* value); +void sim_erk_dims_get(void *config_, void *dims_, const char *field, int* value); + +// model +acados_size_t sim_erk_model_calculate_size(void *config, void *dims); +void *sim_erk_model_assign(void *config, void *dims, void *raw_memory); +int sim_erk_model_set(void *model, const char *field, void *value); + +// opts +acados_size_t sim_erk_opts_calculate_size(void *config, void *dims); +// +void sim_erk_opts_update(void *config_, void *dims, void *opts_); +// +void *sim_erk_opts_assign(void *config, void *dims, void *raw_memory); +// +void sim_erk_opts_initialize_default(void *config, void *dims, void *opts_); +// +void sim_erk_opts_set(void *config_, void *opts_, const char *field, void *value); + + +// memory +acados_size_t sim_erk_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *sim_erk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +int sim_erk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value); + + +// workspace +acados_size_t sim_erk_workspace_calculate_size(void *config, void *dims, void *opts_); + +// +int sim_erk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_); +// +void sim_erk_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_ERK_INTEGRATOR_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_gnsf.h b/phonelibs/acados/include/acados/sim/sim_gnsf.h new file mode 100644 index 0000000000..5524b384e0 --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_gnsf.h @@ -0,0 +1,367 @@ +/* + * 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_SIM_SIM_GNSF_H_ +#define ACADOS_SIM_SIM_GNSF_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "acados/utils/timing.h" +#include "acados/utils/types.h" +#include "acados/sim/sim_common.h" + +#include "blasfeo/include/blasfeo_common.h" +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" +#include "blasfeo/include/blasfeo_d_blas.h" +#include "blasfeo/include/blasfeo_d_kernel.h" +#include "blasfeo/include/blasfeo_i_aux_ext_dep.h" +#include "blasfeo/include/blasfeo_target.h" + +/* +GNSF - Generalized Nonlinear Static Feedback Model +has the following form +https://github.com/acados/acados/files/3359595/gnsf_structure_blo.pdf + +Details on the algorithm can be found in master thesis of Jonathan Frey, +which presents a slightly different format without the terms B_LO, c_LO. +https://github.com/acados/acados/files/2318322/gnsf_structure.pdf +https://cdn.syscop.de/publications/Frey2018.pdf +https://cdn.syscop.de/publications/Frey2019.pdf +*/ + +typedef struct +{ + int nx; // total number of differential states + int nu; // total number of inputs + int nz; // total number of algebraic states + int nx1; // number of differential states in NSF part + int nz1; // number of algebraic states in NSF part + int n_out; // output dimension of phi + int ny; // dimension of first input of phi + int nuhat; // dimension of second input of phi + +} sim_gnsf_dims; + + + +typedef struct +{ + /* external functions */ + // phi: nonlinearity function + external_function_generic *phi_fun; + external_function_generic *phi_fun_jac_y; + external_function_generic *phi_jac_y_uhat; + + // f_lo: linear output function + external_function_generic *f_lo_fun_jac_x1_x1dot_u_z; + + // to import model matrices + external_function_generic *get_gnsf_matrices; + + // flag indicating, if model defining matrices are imported via external (casadi) function, + // [default]: true -> auto; + bool auto_import_gnsf; + + // booleans from structure detection + bool nontrivial_f_LO; // indicates if f_LO is constant zero function + bool fully_linear; // indicates if model is fully linear LOS + + /* model defining matrices */ + // TODO: add setters to set manually + double *A; + double *B; + double *C; + double *E; + + double *L_x; + double *L_xdot; + double *L_z; + double *L_u; + + double *A_LO; + double *B_LO; + double *E_LO; + + /* constant vector */ + double *c; + double *c_LO; + + // permutation vector - to have GNSF order of x, z within sim_gnsf only + int *ipiv_x; + int *ipiv_z; + + double *ipiv_x_double; + double *ipiv_z_double; + +} gnsf_model; + + + +// pre_workspace - workspace used in the precomputation phase +typedef struct +{ + struct blasfeo_dmat E11; + struct blasfeo_dmat E12; + struct blasfeo_dmat E21; + struct blasfeo_dmat E22; + + struct blasfeo_dmat A1; + struct blasfeo_dmat A2; + struct blasfeo_dmat B1; + struct blasfeo_dmat B2; + struct blasfeo_dmat C1; + struct blasfeo_dmat C2; + + struct blasfeo_dmat AA1; + struct blasfeo_dmat AA2; + struct blasfeo_dmat BB1; + struct blasfeo_dmat BB2; + struct blasfeo_dmat CC1; + struct blasfeo_dmat CC2; + struct blasfeo_dmat DD1; + struct blasfeo_dmat DD2; + struct blasfeo_dmat EE1; + struct blasfeo_dmat EE2; + + struct blasfeo_dmat QQ1; + + struct blasfeo_dmat LLZ; + struct blasfeo_dmat LLx; + struct blasfeo_dmat LLK; + + int *ipivEE1; // index of pivot vector + int *ipivEE2; + int *ipivQQ1; + + // for algebraic sensitivity propagation + struct blasfeo_dmat Q1; + + // for constant term in NSF + struct blasfeo_dvec cc1; + struct blasfeo_dvec cc2; + +} gnsf_pre_workspace; + + + +// workspace +typedef struct +{ + double *Z_work; // used to perform computations to get out->zn + + int *ipiv; // index of pivot vector + + struct blasfeo_dvec *vv_traj; + struct blasfeo_dvec *yy_traj; + struct blasfeo_dmat *f_LO_jac_traj; + + struct blasfeo_dvec K2_val; + struct blasfeo_dvec x0_traj; + struct blasfeo_dvec res_val; + struct blasfeo_dvec u0; + struct blasfeo_dvec lambda; + struct blasfeo_dvec lambda_old; + + struct blasfeo_dvec yyu; + struct blasfeo_dvec yyss; + + struct blasfeo_dvec K1_val; + struct blasfeo_dvec f_LO_val; + struct blasfeo_dvec x1_stage_val; + struct blasfeo_dvec Z1_val; + + struct blasfeo_dvec K1u; + struct blasfeo_dvec Zu; + struct blasfeo_dvec ALOtimesx02; + struct blasfeo_dvec BLOtimesu0; + + struct blasfeo_dvec uhat; + + struct blasfeo_dmat J_r_vv; + struct blasfeo_dmat J_r_x1u; + + struct blasfeo_dmat dK1_dx1; + struct blasfeo_dmat dK1_du; + struct blasfeo_dmat dZ_dx1; + struct blasfeo_dmat dZ_du; + struct blasfeo_dmat J_G2_K1; + + struct blasfeo_dmat dK2_dx1; + struct blasfeo_dmat dK2_dvv; + struct blasfeo_dmat dxf_dwn; + struct blasfeo_dmat S_forw_new; + struct blasfeo_dmat S_algebraic_aux; + + struct blasfeo_dmat dPsi_dvv; + struct blasfeo_dmat dPsi_dx; + struct blasfeo_dmat dPsi_du; + + struct blasfeo_dmat dPHI_dyuhat; + struct blasfeo_dvec z0; + + // memory only available if (opts->sens_algebraic) + // struct blasfeo_dvec y_one_stage; + // struct blasfeo_dvec x0dot_1; + // struct blasfeo_dmat dz10_dx1u; // (nz1) * (nx1+nu); + // struct blasfeo_dmat dr0_dvv0; // (n_out * n_out) + // struct blasfeo_dmat f_LO_jac0; // (nx2+nz2) * (2*nx1 + nz1 + nu) + // struct blasfeo_dmat sens_z2_rhs; // (nx2 + nz2) * (nx1 + nu) + // int *ipiv_vv0; + +} gnsf_workspace; + + + +// memory +typedef struct +{ + bool first_call; + + // simulation time for one step + double dt; + + // (scaled) butcher table + double *A_dt; + double *b_dt; + double *c_butcher; + + // value used to initialize integration variables - corresponding to value of phi + double *phi_guess; // n_out + + struct blasfeo_dmat S_forw; + struct blasfeo_dmat S_algebraic; + + // precomputed matrices + struct blasfeo_dmat KKv; + struct blasfeo_dmat KKx; + struct blasfeo_dmat KKu; + + struct blasfeo_dmat YYv; + struct blasfeo_dmat YYx; + struct blasfeo_dmat YYu; + + struct blasfeo_dmat ZZv; + struct blasfeo_dmat ZZx; + struct blasfeo_dmat ZZu; + + struct blasfeo_dmat ALO; + struct blasfeo_dmat BLO; + struct blasfeo_dmat M2_LU; + int *ipivM2; + + struct blasfeo_dmat dK2_dx2; + struct blasfeo_dmat dK2_du; + struct blasfeo_dmat dx2f_dx2u; + + struct blasfeo_dmat Lu; + + // precomputed vectors for constant term in NSF + struct blasfeo_dvec KK0; + struct blasfeo_dvec YY0; + struct blasfeo_dvec ZZ0; + + // for algebraic sensitivities only; + // struct blasfeo_dmat *Z0x; + // struct blasfeo_dmat *Z0u; + // struct blasfeo_dmat *Z0v; + + // struct blasfeo_dmat *Y0x; + // struct blasfeo_dmat *Y0u; + // struct blasfeo_dmat *Y0v; + + // struct blasfeo_dmat *K0x; + // struct blasfeo_dmat *K0u; + // struct blasfeo_dmat *K0v; + + // struct blasfeo_dmat *ELO_LU; + // int *ipiv_ELO; + // struct blasfeo_dmat *ELO_inv_ALO; + + // struct blasfeo_dmat *Lx; + // struct blasfeo_dmat *Lxdot; + // struct blasfeo_dmat *Lz; + + double time_sim; + double time_ad; + double time_la; + +} sim_gnsf_memory; + + + +// gnsf dims +acados_size_t sim_gnsf_dims_calculate_size(); +void *sim_gnsf_dims_assign(void *config_, void *raw_memory); + +// get & set functions +void sim_gnsf_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_gnsf_dims_get(void *config_, void *dims_, const char *field, int* value); + +// opts +acados_size_t sim_gnsf_opts_calculate_size(void *config, void *dims); +void *sim_gnsf_opts_assign(void *config, void *dims, void *raw_memory); +void sim_gnsf_opts_initialize_default(void *config, void *dims, void *opts_); +void sim_gnsf_opts_update(void *config_, void *dims, void *opts_); +void sim_gnsf_opts_set(void *config_, void *opts_, const char *field, void *value); + +// model +acados_size_t sim_gnsf_model_calculate_size(void *config, void *dims_); +void *sim_gnsf_model_assign(void *config, void *dims_, void *raw_memory); +int sim_gnsf_model_set(void *model_, const char *field, void *value); + +// precomputation +int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, + void *work_); + +// workspace & memory +acados_size_t sim_gnsf_workspace_calculate_size(void *config, void *dims_, void *args); +acados_size_t sim_gnsf_memory_calculate_size(void *config, void *dims_, void *opts_); +void *sim_gnsf_memory_assign(void *config, void *dims_, void *opts_, void *raw_memory); + +// interface +void sim_gnsf_config_initialize_default(void *config_); + +// integrator +int sim_gnsf(void *config, sim_in *in, sim_out *out, void *opts, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_GNSF_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_irk_integrator.h b/phonelibs/acados/include/acados/sim/sim_irk_integrator.h new file mode 100644 index 0000000000..6851bacb3a --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_irk_integrator.h @@ -0,0 +1,186 @@ +/* + * 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_SIM_SIM_IRK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_IRK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + +#include "blasfeo/include/blasfeo_common.h" + +typedef struct +{ + int nx; + int nu; + int nz; + +} sim_irk_dims; + + + +typedef struct +{ + /* external functions */ + // implicit fun - can either be fully implicit ode or dae + // - i.e. dae has z as additional last argument & nz > 0 + external_function_generic *impl_ode_fun; + // implicit ode & jac_x & jax_xdot & jac_z + external_function_generic *impl_ode_fun_jac_x_xdot_z; + // jax_x & jac_xdot & jac_u & jac_z of implicit ode + external_function_generic *impl_ode_jac_x_xdot_u_z; + // hessian of implicit ode: + external_function_generic *impl_ode_hess; +} irk_model; + + + +typedef struct +{ + struct blasfeo_dvec *rG; // residuals of G (nx*ns) + struct blasfeo_dvec *K; // internal K variables ((nx+nz)*ns) + struct blasfeo_dvec *xt; // temporary x + struct blasfeo_dvec *xn; // x at each integration step + struct blasfeo_dvec xtdot; // temporary xdot + + struct blasfeo_dvec *lambda; // adjoint sensitivities (nx + nu) + struct blasfeo_dvec *lambdaK; // auxiliary variable ((nx+nz)*ns) for adjoint propagation + + struct blasfeo_dmat df_dx; // temporary Jacobian of ode w.r.t x (nx+nz, nx) + struct blasfeo_dmat df_dxdot; // temporary Jacobian of ode w.r.t xdot (nx+nz, nx) + struct blasfeo_dmat df_du; // temporary Jacobian of ode w.r.t u (nx+nz, nu) + struct blasfeo_dmat df_dz; // temporary Jacobian of ode w.r.t z (nx+nz, nu) + + /* NOTE: the memory allocation corresponding to the following fields is CONDITIONAL */ + + // only allocated if (opts->sens_algebraic || opts->output_z) + int *ipiv_one_stage; // index of pivot vector (nx + nz) + double *Z_work; // used to perform computations to get out->zn (ns) + + // df_dxdotz, dk0_dxu, only allocated if (opts->sens_algebraic && opts->exact_z_output) + // used for algebraic sensitivity generation + struct blasfeo_dmat df_dxdotz; // temporary Jacobian of ode w.r.t. xdot,z (nx+nz, nx+nz); + struct blasfeo_dmat dk0_dxu; // intermediate result, (nx+nz, nx+nu) + + // dK_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of (num_steps) blasfeo_dmat + // to store intermediate results + struct blasfeo_dmat *dK_dxu; // jacobian of (K,Z) over x and u ((nx+nz)*ns, nx+nu); + + // S_forw: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of (num_steps + 1) blasfeo_dmat + // to store intermediate results + struct blasfeo_dmat *S_forw; // forward sensitivities (nx, nx+nu) + + // dG_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results + struct blasfeo_dmat *dG_dxu; // jacobian of G over x and u ((nx+nz)*ns, nx+nu) + + // dG_dK: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results + struct blasfeo_dmat *dG_dK; // jacobian of G over K ((nx+nz)*ns, (nx+nz)*ns) + + // ipiv: index of pivot vector + // if (!opts->sens_hess) - array (ns * (nx + nz)) that is reused + // if ( opts->sens_hess) - array (ns * (nx + nz)) * num_steps, to store all + // pivot vectors for dG_dxu + int *ipiv; // index of pivot vector + + // xn_traj, K_traj only available if( opts->sens_adj || opts->sens_hess ) + struct blasfeo_dvec *xn_traj; // xn trajectory + struct blasfeo_dvec *K_traj; // K trajectory + + /* the following variables are only available if (opts->sens_hess) */ + // For Hessian propagation + struct blasfeo_dmat Hess; // temporary Hessian (nx + nu, nx + nu) + // output of impl_ode_hess + struct blasfeo_dmat f_hess; // size: (nx + nu, nx + nu) + struct blasfeo_dmat dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu) + struct blasfeo_dmat tmp_dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu) + +} sim_irk_workspace; + + +typedef struct +{ + double *xdot; // xdot[NX] - initialization for state derivatives k within the integrator + double *z; // z[NZ] - initialization for algebraic variables z + + double time_sim; + double time_ad; + double time_la; +} sim_irk_memory; + + +// get & set functions +void sim_irk_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_irk_dims_get(void *config_, void *dims_, const char *field, int* value); + +// dims +acados_size_t sim_irk_dims_calculate_size(); +void *sim_irk_dims_assign(void *config_, void *raw_memory); + +// model +acados_size_t sim_irk_model_calculate_size(void *config, void *dims); +void *sim_irk_model_assign(void *config, void *dims, void *raw_memory); +int sim_irk_model_set(void *model, const char *field, void *value); + +// opts +acados_size_t sim_irk_opts_calculate_size(void *config, void *dims); +void *sim_irk_opts_assign(void *config, void *dims, void *raw_memory); +void sim_irk_opts_initialize_default(void *config, void *dims, void *opts_); +void sim_irk_opts_update(void *config_, void *dims, void *opts_); +void sim_irk_opts_set(void *config_, void *opts_, const char *field, void *value); + +// memory +acados_size_t sim_irk_memory_calculate_size(void *config, void *dims, void *opts_); +void *sim_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value); + +// workspace +acados_size_t sim_irk_workspace_calculate_size(void *config, void *dims, void *opts_); +void sim_irk_config_initialize_default(void *config); + +// main +int sim_irk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_IRK_INTEGRATOR_H_ diff --git a/phonelibs/acados/include/acados/sim/sim_lifted_irk_integrator.h b/phonelibs/acados/include/acados/sim/sim_lifted_irk_integrator.h new file mode 100644 index 0000000000..8e476b0e27 --- /dev/null +++ b/phonelibs/acados/include/acados/sim/sim_lifted_irk_integrator.h @@ -0,0 +1,159 @@ +/* + * 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_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + +typedef struct +{ + int nx; + int nu; + int nz; +} sim_lifted_irk_dims; + + + +typedef struct +{ + /* external functions */ + // implicit ode + external_function_generic *impl_ode_fun; + // implicit ode & jax_x & jac_xdot & jac_u implicit ode + external_function_generic *impl_ode_fun_jac_x_xdot_u; + +} lifted_irk_model; + + + +typedef struct +{ + + struct blasfeo_dmat *J_temp_x; // temporary Jacobian of ode w.r.t x (nx, nx) + struct blasfeo_dmat *J_temp_xdot; // temporary Jacobian of ode w.r.t xdot (nx, nx) + struct blasfeo_dmat *J_temp_u; // temporary Jacobian of ode w.r.t u (nx, nu) + + struct blasfeo_dvec *rG; // residuals of G (nx*ns) + struct blasfeo_dvec *xt; // temporary x + struct blasfeo_dvec *xn; // x at each integration step (for evaluations) + struct blasfeo_dvec *xn_out; // x at each integration step (output) + struct blasfeo_dvec *dxn; // dx at each integration step + struct blasfeo_dvec *w; // stacked x and u + + int *ipiv; // index of pivot vector + +} sim_lifted_irk_workspace; + + + +typedef struct +{ + // memory for lifted integrators + struct blasfeo_dmat *S_forw; // forward sensitivities + struct blasfeo_dmat *JGK; // jacobian of G over K (nx*ns, nx*ns) + struct blasfeo_dmat *JGf; // jacobian of G over x and u (nx*ns, nx+nu); + struct blasfeo_dmat *JKf; // jacobian of K over x and u (nx*ns, nx+nu); + + struct blasfeo_dvec *K; // internal variables (nx*ns) + struct blasfeo_dvec *x; // states (nx) -- for expansion step + struct blasfeo_dvec *u; // controls (nu) -- for expansion step + + int update_sens; + + double time_sim; + double time_ad; + double time_la; + +} sim_lifted_irk_memory; + + + +/* dims */ +void sim_lifted_irk_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_lifted_irk_dims_get(void *config_, void *dims_, const char *field, int* value); + +acados_size_t sim_lifted_irk_dims_calculate_size(); +// +void *sim_lifted_irk_dims_assign(void* config_, void *raw_memory); + +/* model */ +// +acados_size_t sim_lifted_irk_model_calculate_size(void *config, void *dims); +// +void *sim_lifted_irk_model_assign(void *config, void *dims, void *raw_memory); +// +int sim_lifted_irk_model_set(void *model_, const char *field, void *value); + +/* opts */ +// +acados_size_t sim_lifted_irk_opts_calculate_size(void *config, void *dims); +// +void *sim_lifted_irk_opts_assign(void *config, void *dims, void *raw_memory); +// +void sim_lifted_irk_opts_initialize_default(void *config, void *dims, void *opts_); +// +void sim_lifted_irk_opts_update(void *config_, void *dims, void *opts_); +// +void sim_lifted_irk_opts_set(void *config_, void *opts_, const char *field, void *value); + +/* memory */ +// +acados_size_t sim_lifted_irk_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *sim_lifted_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); + +/* workspace */ +// +acados_size_t sim_lifted_irk_workspace_calculate_size(void *config, void *dims, void *opts_); +// +void sim_lifted_irk_config_initialize_default(void *config); + +/* solver */ +// +int sim_lifted_irk(void *config, sim_in *in, sim_out *out, void *opts_, + void *mem_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ diff --git a/phonelibs/acados/include/acados/utils/external_function_generic.h b/phonelibs/acados/include/acados/utils/external_function_generic.h new file mode 100644 index 0000000000..021363f26e --- /dev/null +++ b/phonelibs/acados/include/acados/utils/external_function_generic.h @@ -0,0 +1,245 @@ +/* + * 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_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ +#define ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + +/************************************************ + * generic external function + ************************************************/ + +// type of arguments +typedef enum { + COLMAJ, + BLASFEO_DMAT, + BLASFEO_DVEC, + COLMAJ_ARGS, + BLASFEO_DMAT_ARGS, + BLASFEO_DVEC_ARGS, + IGNORE_ARGUMENT +} ext_fun_arg_t; + +struct colmaj_args +{ + double *A; + int lda; +}; + +struct blasfeo_dmat_args +{ + struct blasfeo_dmat *A; + int ai; + int aj; +}; + +struct blasfeo_dvec_args +{ + struct blasfeo_dvec *x; + int xi; +}; + +// prototype of an external function +typedef struct +{ + // public members (have to be before private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // private members + // ..... +} external_function_generic; + + + +/************************************************ + * generic external parametric function + ************************************************/ + +// prototype of a parametric external function +typedef struct +{ + // public members for core (have to be before private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // public members for interfaces + void (*get_nparam)(void *, int *); + void (*set_param)(void *, double *); + void (*set_param_sparse)(void *, int n_update, int *idx, double *); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*fun)(void **, void **, void *); + double *p; // parameters + int np; // number of parameters + // ..... +} external_function_param_generic; + +// +acados_size_t external_function_param_generic_struct_size(); +// +void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value); +// +acados_size_t external_function_param_generic_calculate_size(external_function_param_generic *fun, int np); +// +void external_function_param_generic_assign(external_function_param_generic *fun, void *mem); +// +void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out); +// +void external_function_param_generic_get_nparam(void *self, int *np); +// +void external_function_param_generic_set_param(void *self, double *p); + + +/************************************************ + * casadi external function + ************************************************/ + +typedef struct +{ + // public members (have to be the same as in the prototype, and before the private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*casadi_fun)(const double **, double **, int *, double *, void *); + int (*casadi_work)(int *, int *, int *, int *); + const int *(*casadi_sparsity_in)(int); + const int *(*casadi_sparsity_out)(int); + int (*casadi_n_in)(); + int (*casadi_n_out)(); + double **args; + double **res; + double *w; + int *iw; + int *args_size; // size of args[i] + int *res_size; // size of res[i] + int args_num; // number of args arrays + int args_size_tot; // total size of args arrays + int res_num; // number of res arrays + int res_size_tot; // total size of res arrays + int in_num; // number of input arrays + int out_num; // number of output arrays + int iw_size; // number of ints for worksapce + int w_size; // number of doubles for workspace +} external_function_casadi; + +// +acados_size_t external_function_casadi_struct_size(); +// +void external_function_casadi_set_fun(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_work(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_sparsity_in(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_sparsity_out(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_n_in(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_n_out(external_function_casadi *fun, void *value); +// +acados_size_t external_function_casadi_calculate_size(external_function_casadi *fun); +// +void external_function_casadi_assign(external_function_casadi *fun, void *mem); +// +void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in, + ext_fun_arg_t *type_out, void **out); + +/************************************************ + * casadi external parametric function + ************************************************/ + +typedef struct +{ + // public members for core (have to be the same as in the prototype, and before the private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // public members for interfaces + void (*get_nparam)(void *, int *); + void (*set_param)(void *, double *); + void (*set_param_sparse)(void *, int n_update, int *idx, double *); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*casadi_fun)(const double **, double **, int *, double *, void *); + int (*casadi_work)(int *, int *, int *, int *); + const int *(*casadi_sparsity_in)(int); + const int *(*casadi_sparsity_out)(int); + int (*casadi_n_in)(); + int (*casadi_n_out)(); + double **args; + double **res; + double *w; + int *iw; + int *args_size; // size of args[i] + int *res_size; // size of res[i] + int args_num; // number of args arrays + int args_size_tot; // total size of args arrays + int res_num; // number of res arrays + int res_size_tot; // total size of res arrays + int in_num; // number of input arrays + int out_num; // number of output arrays + int iw_size; // number of ints for worksapce + int w_size; // number of doubles for workspace + int np; // number of parameters +} external_function_param_casadi; + +// +acados_size_t external_function_param_casadi_struct_size(); +// +void external_function_param_casadi_set_fun(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_work(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_sparsity_in(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_sparsity_out(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_n_in(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_n_out(external_function_param_casadi *fun, void *value); +// +acados_size_t external_function_param_casadi_calculate_size(external_function_param_casadi *fun, int np); +// +void external_function_param_casadi_assign(external_function_param_casadi *fun, void *mem); +// +void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in, + ext_fun_arg_t *type_out, void **out); +// +void external_function_param_casadi_get_nparam(void *self, int *np); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ diff --git a/phonelibs/acados/include/acados/utils/math.h b/phonelibs/acados/include/acados/utils/math.h new file mode 100644 index 0000000000..ac0d3263e8 --- /dev/null +++ b/phonelibs/acados/include/acados/utils/math.h @@ -0,0 +1,105 @@ +/* + * 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_UTILS_MATH_H_ +#define ACADOS_UTILS_MATH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + +#if defined(__DSPACE__) +double fmax(double a, double b); +#endif + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +void dgemm_nn_3l(int m, int n, int k, double *A, int lda, double *B, int ldb, double *C, int ldc); +// void dgemv_n_3l(int m, int n, double *A, int lda, double *x, double *y); +// void dgemv_t_3l(int m, int n, double *A, int lda, double *x, double *y); +// void dcopy_3l(int n, double *x, int incx, double *y, int incy); +void daxpy_3l(int n, double da, double *dx, double *dy); +void dscal_3l(int n, double da, double *dx); +double twonormv(int n, double *ptrv); + +/* copies a matrix into another matrix */ +void dmcopy(int row, int col, double *ptrA, int lda, double *ptrB, int ldb); + +/* solution of a system of linear equations */ +void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info); + +/* matrix exponential */ +void expm(int row, double *A); + +int idamax_3l(int n, double *x); + +void dswap_3l(int n, double *x, int incx, double *y, int incy); + +void dger_3l(int m, int n, double alpha, double *x, int incx, double *y, int incy, double *A, + int lda); + +void dgetf2_3l(int m, int n, double *A, int lda, int *ipiv, int *info); + +void dlaswp_3l(int n, double *A, int lda, int k1, int k2, int *ipiv); + +void dtrsm_l_l_n_u_3l(int m, int n, double *A, int lda, double *B, int ldb); + +void dgetrs_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb); + +void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info); + +double onenorm(int row, int col, double *ptrA); + +// double twonormv(int n, double *ptrv); + +void padeapprox(int m, int row, double *A); + +void expm(int row, double *A); + +// void d_compute_qp_size_ocp2dense_rev(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, +// int *nvd, int *ned, int *nbd, int *ngd); + +void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double *e); + +double minimum_of_doubles(double *x, int n); + +void neville_algorithm(double xx, int n, double *x, double *Q, double *out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_MATH_H_ diff --git a/phonelibs/acados/include/acados/utils/mem.h b/phonelibs/acados/include/acados/utils/mem.h new file mode 100644 index 0000000000..7b9efc5ed8 --- /dev/null +++ b/phonelibs/acados/include/acados/utils/mem.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_UTILS_MEM_H_ +#define ACADOS_UTILS_MEM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "types.h" + +// blasfeo +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" + +// TODO(dimitris): probably does not belong here +typedef struct +{ + int (*fun)(void *); + acados_size_t (*calculate_args_size)(void *); + void *(*assign_args)(void *); + void (*initialize_default_args)(void *); + acados_size_t (*calculate_memory_size)(void *); + void *(*assign_memory)(void *); + acados_size_t (*calculate_workspace_size)(void *); +} module_solver; + +// make int counter of memory multiple of a number (typically 8 or 64) +void make_int_multiple_of(acados_size_t num, acados_size_t *size); + +// align char pointer to number (typically 8 for pointers and doubles, +// 64 for blasfeo structs) and return offset +int align_char_to(int num, char **c_ptr); + +// switch between malloc and calloc (for valgrinding) +void *acados_malloc(size_t nitems, acados_size_t size); + +// uses always calloc +void *acados_calloc(size_t nitems, acados_size_t size); + +// allocate vector of pointers to vectors of doubles and advance pointer +void assign_and_advance_double_ptrs(int n, double ***v, char **ptr); + +// allocate vector of pointers to vectors of ints and advance pointer +void assign_and_advance_int_ptrs(int n, int ***v, char **ptr); + +// allocate vector of pointers to strvecs and advance pointer +void assign_and_advance_blasfeo_dvec_structs(int n, struct blasfeo_dvec **sv, char **ptr); + +// allocate vector of pointers to strmats and advance pointer +void assign_and_advance_blasfeo_dmat_structs(int n, struct blasfeo_dmat **sm, char **ptr); + +// allocate vector of pointers to vector of pointers to strmats and advance pointer +void assign_and_advance_blasfeo_dmat_ptrs(int n, struct blasfeo_dmat ***sm, char **ptr); + +// allocate vector of chars and advance pointer +void assign_and_advance_char(int n, char **v, char **ptr); + +// allocate vector of ints and advance pointer +void assign_and_advance_int(int n, int **v, char **ptr); + +// allocate vector of bools and advance pointer +void assign_and_advance_bool(int n, bool **v, char **ptr); + +// allocate vector of doubles and advance pointer +void assign_and_advance_double(int n, double **v, char **ptr); + +// allocate strvec and advance pointer +void assign_and_advance_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char **ptr); + +// allocate strmat and advance pointer +void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_MEM_H_ diff --git a/phonelibs/acados/include/acados/utils/print.h b/phonelibs/acados/include/acados/utils/print.h new file mode 100644 index 0000000000..2993447dbd --- /dev/null +++ b/phonelibs/acados/include/acados/utils/print.h @@ -0,0 +1,109 @@ +/* + * 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_UTILS_PRINT_H_ +#define ACADOS_UTILS_PRINT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_common_frontend.h" +#include "acados/utils/types.h" + +// void print_matrix(char *file_name, const real_t *matrix, const int_t nrows, const int_t ncols); + +// void print_matrix_name(char *file_name, char *name, const real_t *matrix, const int_t nrows, +// const int_t ncols); + +// void print_int_matrix(char *file_name, const int_t *matrix, const int_t nrows, const int_t ncols); + +// void print_array(char *file_name, real_t *array, int_t size); + +// void print_int_array(char *file_name, const int_t *array, int_t size); + +void read_matrix(const char *file_name, real_t *array, const int_t nrows, const int_t ncols); + +void write_double_vector_to_txt(real_t *vec, int_t n, const char *fname); + +// ocp nlp +// TODO(andrea): inconsistent naming +void ocp_nlp_dims_print(ocp_nlp_dims *dims); +// TODO(andrea): inconsistent naming +void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out); +// TODO(andrea): inconsistent naming +void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res); + +// ocp qp +void print_ocp_qp_dims(ocp_qp_dims *dims); + +// void print_dense_qp_dims(dense_qp_dims *dims); + +void print_ocp_qp_in(ocp_qp_in *qp_in); + +void print_ocp_qp_out(ocp_qp_out *qp_out); + +// void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in); + +// void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out); + +void print_ocp_qp_res(ocp_qp_res *qp_res); + +// void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp); + +// void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp); + +// void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out); + +void print_dense_qp_in(dense_qp_in *qp_in); + +void print_qp_info(qp_info *info); + +// void acados_warning(char warning_string[]); + +// void acados_error(char error_string[]); + +// void acados_not_implemented(char feature_string[]); + +// blasfeo +// void print_blasfeo_target(); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_PRINT_H_ diff --git a/phonelibs/acados/include/acados/utils/strsep.h b/phonelibs/acados/include/acados/utils/strsep.h new file mode 100644 index 0000000000..02f1835593 --- /dev/null +++ b/phonelibs/acados/include/acados/utils/strsep.h @@ -0,0 +1,72 @@ +/* + * 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_UTILS_STRSEP_H_ +#define ACADOS_UTILS_STRSEP_H_ + +#ifdef __cplusplus +#include +#define STD(x) std::x +namespace std +{ +#else +#include +#define STD(x) x +#endif + +char* strsep_acados(char** stringp, const char* delim) +{ + char* result; + + if ((stringp == NULL) || (*stringp == NULL)) return NULL; + + result = *stringp; + + while (**stringp && !STD(strchr)(delim, **stringp)) ++*stringp; + + if (**stringp) + *(*stringp)++ = '\0'; + else + *stringp = NULL; + + return result; +} + +#ifdef __cplusplus +} // namespace std +#endif + +#undef STD + +#endif // ACADOS_UTILS_STRSEP_H_ diff --git a/phonelibs/acados/include/acados/utils/timing.h b/phonelibs/acados/include/acados/utils/timing.h new file mode 100644 index 0000000000..823b763f66 --- /dev/null +++ b/phonelibs/acados/include/acados/utils/timing.h @@ -0,0 +1,125 @@ +/* + * 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_UTILS_TIMING_H_ +#define ACADOS_UTILS_TIMING_H_ + +#include "acados/utils/types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef MEASURE_TIMINGS +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acados_timer; + +#elif defined(__APPLE__) + +#include + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acados_timer; + +#elif defined(__DSPACE__) + +#include + +typedef struct acados_timer_ +{ + double time; +} acados_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if (__STDC_VERSION__ >= 199901L) && !(defined __MINGW32__ || defined __MINGW64__) // C99 Mode + +#include +#include + +typedef struct acados_timer_ +{ + struct timeval tic; + struct timeval toc; +} acados_timer; + +#else // ANSI C Mode + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + struct timespec tic; + struct timespec toc; +} acados_timer; + +#endif // __STDC_VERSION__ >= 199901L + +#endif // (defined _WIN32 || defined _WIN64) + +#else + +// Dummy type when timings are off +typedef real_t acados_timer; + +#endif // MEASURE_TIMINGS + +/** A function for measurement of the current time. */ +void acados_tic(acados_timer* t); + +/** A function which returns the elapsed time. */ +real_t acados_toc(acados_timer* t); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_TIMING_H_ diff --git a/phonelibs/acados/include/acados/utils/types.h b/phonelibs/acados/include/acados/utils/types.h new file mode 100644 index 0000000000..99ad375fea --- /dev/null +++ b/phonelibs/acados/include/acados/utils/types.h @@ -0,0 +1,79 @@ +/* + * 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_UTILS_TYPES_H_ +#define ACADOS_UTILS_TYPES_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#define MAX_STR_LEN 256 +#define ACADOS_EPS 1e-12 +#define ACADOS_NEG_INFTY -1.0e9 +#define ACADOS_POS_INFTY +1.0e9 +#define UNUSED(x) ((void)(x)) + + + +typedef double real_t; +typedef int int_t; +typedef size_t acados_size_t; + + +typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, double* w, void* mem); + + + +// enum of return values +enum return_values +{ + ACADOS_SUCCESS, + ACADOS_FAILURE, + ACADOS_MAXITER, + ACADOS_MINSTEP, + ACADOS_QP_FAILURE, + ACADOS_READY, +}; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_TYPES_H_ diff --git a/phonelibs/acados/x86_64/lib/libacados.so b/phonelibs/acados/x86_64/lib/libacados.so index 32d2498fe6..afe8fc4da5 100644 Binary files a/phonelibs/acados/x86_64/lib/libacados.so and b/phonelibs/acados/x86_64/lib/libacados.so differ diff --git a/phonelibs/acados/x86_64/lib/libhpipm.so b/phonelibs/acados/x86_64/lib/libhpipm.so index 9a79bfbd31..5d19a60032 100644 Binary files a/phonelibs/acados/x86_64/lib/libhpipm.so and b/phonelibs/acados/x86_64/lib/libhpipm.so differ diff --git a/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 b/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 index 93967ae04a..0693375308 100644 Binary files a/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 and b/phonelibs/acados/x86_64/lib/libqpOASES_e.so.3.1 differ diff --git a/phonelibs/acados/x86_64/t_renderer b/phonelibs/acados/x86_64/t_renderer index f2d22170a1..9e10d4b8b4 100755 Binary files a/phonelibs/acados/x86_64/t_renderer and b/phonelibs/acados/x86_64/t_renderer differ