HaraldSchafer
4 years ago
committed by
GitHub
parent
edae2218d7
commit
17f066e2fe
53 changed files with 7368 additions and 9 deletions
@ -1,4 +1,4 @@ |
||||
acados/ |
||||
acados_repo/ |
||||
!x86_64/ |
||||
!larch64/ |
||||
!aarch64/ |
||||
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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 <stdbool.h> |
||||
|
||||
#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_
|
||||
@ -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_
|
||||
@ -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 <stdbool.h> |
||||
|
||||
#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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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_
|
||||
@ -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 <stdio.h> |
||||
#include <stdbool.h> |
||||
|
||||
#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_
|
||||
@ -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_
|
||||
@ -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 <string> |
||||
#define STD(x) std::x |
||||
namespace std |
||||
{ |
||||
#else |
||||
#include <string.h> |
||||
#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_
|
||||
@ -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 <Windows.h> |
||||
|
||||
/** 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 <mach/mach_time.h> |
||||
|
||||
/** 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 <brtenv.h> |
||||
|
||||
typedef struct acados_timer_ |
||||
{ |
||||
double time; |
||||
} acados_timer; |
||||
|
||||
#else |
||||
|
||||
/* Use POSIX clock_gettime() for timing on non-Windows machines. */ |
||||
#include <time.h> |
||||
|
||||
#if (__STDC_VERSION__ >= 199901L) && !(defined __MINGW32__ || defined __MINGW64__) // C99 Mode
|
||||
|
||||
#include <sys/stat.h> |
||||
#include <sys/time.h> |
||||
|
||||
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_
|
||||
@ -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 <stdbool.h> |
||||
#include <stddef.h> |
||||
|
||||
#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_
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue